#define HORN 5
#define BELL 6
#define FL 7
#define RL 8
#define DLFL 9
#define DLFR 10
#define DLRL 11
#define DLRR 12

#include <ServoInput.h>

// Throttle Setup
const int ThrottleSignalPin = 2;  // MUST be interrupt-capable!
const int ThrottlePulseMin = 1000;  // microseconds (us)
const int ThrottlePulseMax = 2000;  // Ideal values for your servo can be found with the "Calibration" example

// Steering Setup
const int SteeringSignalPin = 3;  // MUST be interrupt-capable!
const int SteeringPulseMin = 1000;  // microseconds (us)
const int SteeringPulseMax = 2000;  // Ideal values for your servo can be found with the "Calibration" example

ServoInputPin<SteeringSignalPin> steering(SteeringPulseMin, SteeringPulseMax);
ServoInputPin<ThrottleSignalPin> throttle(ThrottlePulseMin, ThrottlePulseMax);

bool bell = false;
bool horn = false;
long bell_delay = 0; 

void initTrack() {
  Serial.println("<1 JOIN>");
}

void setDccState(bool horn, bool bell) {
  if (bell) {Serial.println("<F 3333 1 1>");} else {Serial.println("<F 3333 1 0>");}
  if (horn) {Serial.println("<F 3333 2 1>");} else {Serial.println("<F 3333 2 0>");}
  while(Serial.available()) {Serial.read();}
}

void setup() {
  pinMode(HORN, OUTPUT);
  pinMode(BELL, OUTPUT);
  pinMode(FL, OUTPUT);
  pinMode(RL, OUTPUT);
  pinMode(DLFL, OUTPUT);
  pinMode(DLFR, OUTPUT);
  pinMode(DLRL, OUTPUT);
  pinMode(DLRR, OUTPUT);
  Serial.begin(115200);

	while (!ServoInput.available()) {  // wait for all signals to be ready
		delay(500);
	}
  initTrack();
}

void forward_lights() {
  digitalWrite(FL, HIGH);
  digitalWrite(RL, LOW);
}

void reverse_lights() {
  digitalWrite(FL, LOW);
  digitalWrite(RL, HIGH);
}

long ditch_light_delay = 0;
bool ditch_light_state = false;
int last_state = 1;

void runDitchLights(int state) {
  if (ditch_light_delay + 1000 < millis()) {
    ditch_light_state = !ditch_light_state;
    ditch_light_delay = millis();
  }
  if(state != 0) {
    last_state = state;
  }
  if (last_state == 1) {
    digitalWrite(DLFL, ditch_light_state);
    digitalWrite(DLFR, !ditch_light_state);
    digitalWrite(DLRL, LOW);
    digitalWrite(DLRR, LOW);
  } else if (last_state == -1) {
    digitalWrite(DLFL, LOW);
    digitalWrite(DLFR, LOW);
    digitalWrite(DLRL, ditch_light_state);
    digitalWrite(DLRR, !ditch_light_state);
  }
}

bool forward = true;

void loop() {

	float steeringAngle = 90.0 - steering.getAngle();  // returns 0 - 180, subtracting from 90 to center at "0" and invert for "normal" steering

	if (steeringAngle < -45 && bell_delay + 500 < millis()) {
    bell = !bell;
    bell_delay = millis();
  }

  horn = steeringAngle > 45;

  if (bell) {
    digitalWrite(BELL, HIGH);
  } else {
    digitalWrite(BELL, LOW);
  }
  if (horn) {
    digitalWrite(HORN, HIGH);
  } else {
    digitalWrite(HORN, LOW);
  }

	int throttlePercent = throttle.map(-100, 100);  // remap to a percentage both forward and reverse

	if (throttlePercent >= 20) {
    forward = true;
	}
	else if(throttlePercent <= -20) {
    forward = false;
	}

  if (forward) {
    forward_lights();
    runDitchLights(1);
  } else {
    reverse_lights();
    runDitchLights(-1);
  }

  setDccState(horn, bell);
  delay(10);
}