#define THROTTLE_PIN 2
#define FORK_PIN 3

#define THROTTLE_A 5
#define THROTTLE_B 6

#define FORK_A 9
#define FORK_B 10

int loop_counter = 0;
// after 1000, start updating boundaries

int32_t lowest_throttle_pwm_value = 17900;
int32_t highest_throttle_pwm_value = 18670;
int throttle_delta = 780; // this doesn't change

int32_t lowest_fork_pwm_value = 17790;
int32_t highest_fork_pwm_value = 18780;
int fork_delta = 990; // this doesn't change

int32_t throttle_read_pwm;
int32_t fork_read_pwm;

int throttle_mapped;
int fork_mapped;

int throttle_speed;
int fork_speed;

bool do_update_boundaries = false;


void setup() {
  pinMode(THROTTLE_PIN, INPUT_PULLUP);
  pinMode(FORK_PIN, INPUT_PULLUP);
  pinMode(THROTTLE_A, OUTPUT);
  pinMode(THROTTLE_B, OUTPUT);
  pinMode(FORK_A, OUTPUT);
  pinMode(FORK_B, OUTPUT);
  Serial.begin(115200);
}

void update_boundaries(int32_t throttle_pwm, int32_t fork_pwm) {
  if(throttle_pwm > highest_throttle_pwm_value) {
    highest_throttle_pwm_value = throttle_pwm;
    lowest_throttle_pwm_value = highest_throttle_pwm_value - throttle_delta;
  }
  if(throttle_pwm < lowest_throttle_pwm_value) {
    lowest_throttle_pwm_value = throttle_pwm;
    highest_throttle_pwm_value = lowest_throttle_pwm_value + throttle_delta;
  }
  if(fork_pwm > highest_throttle_pwm_value) {
    highest_fork_pwm_value = fork_pwm;
    lowest_fork_pwm_value = highest_fork_pwm_value - fork_delta;
  }
  if(fork_pwm < lowest_throttle_pwm_value) {
    lowest_fork_pwm_value = fork_pwm;
    highest_fork_pwm_value = lowest_fork_pwm_value + fork_delta;
  }
}

void stop_throttle() {
  throttle_speed = 0;
  digitalWrite(THROTTLE_A, LOW);
  digitalWrite(THROTTLE_B, LOW);
  Serial.print(" TSTOPPED ");
}

void move_forward(int speed) {
  digitalWrite(THROTTLE_B, LOW);
  analogWrite(THROTTLE_A, speed);
}

void move_back(int speed) {
  digitalWrite(THROTTLE_A, LOW);
  analogWrite(THROTTLE_B, speed);
}

void stop_fork() {
  fork_speed = 0;
  digitalWrite(FORK_A, LOW);
  digitalWrite(FORK_B, LOW);
  Serial.print(" FSTOPPED ");
}

void fork_up(int speed) {
  digitalWrite(FORK_A, LOW);
  analogWrite(FORK_B, speed);
}

void fork_down(int speed) {
  digitalWrite(FORK_B, LOW);
  analogWrite(FORK_A, speed);
}

void throttle(int32_t throttle_pwm) {
  if (throttle_pwm < 10) {
    stop_throttle();
  } else {
    if (throttle_pwm > highest_throttle_pwm_value) {throttle_pwm = highest_throttle_pwm_value;}
    if (throttle_pwm < lowest_throttle_pwm_value) {throttle_pwm = lowest_throttle_pwm_value;} // This is just a contingency
    throttle_mapped = map(throttle_pwm, lowest_throttle_pwm_value, highest_throttle_pwm_value, 0, 1000);
    if (throttle_mapped >= 400 && throttle_mapped <= 600) {stop_throttle();}
    else if (throttle_mapped > 600) {
      throttle_speed = map(throttle_mapped, 600, 1000, 0, 255);
      move_forward(throttle_speed);
      Serial.print("FWD: ");
      Serial.print(throttle_speed);
      Serial.print("  | ");
    } else if (throttle_mapped < 400) {
      throttle_speed = map(throttle_mapped, 0, 400, 0, 255);
      throttle_speed = 255 - throttle_speed;
      move_back(throttle_speed);
      Serial.print("REV: ");
      Serial.print(throttle_speed);
      Serial.print("  | ");
    }
  }
}

void fork(int32_t fork_pwm) {

  if (fork_pwm < 10) {
    stop_fork();
  } else {
    if (fork_pwm > highest_fork_pwm_value) {fork_pwm = highest_fork_pwm_value;}
    if (fork_pwm < lowest_fork_pwm_value) {fork_pwm = lowest_fork_pwm_value;} // This is just a contingency
    fork_mapped = map(fork_pwm, lowest_fork_pwm_value, highest_fork_pwm_value, 0, 1000);
    if (fork_mapped >= 400 && fork_mapped <= 600) {stop_fork();}
    else if (fork_mapped > 600) {
      fork_speed = map(fork_mapped, 600, 1000, 0, 255);
      fork_up(fork_speed);
      Serial.print("UP: ");
      Serial.print(fork_speed);
      Serial.print("  | ");
    } else if (fork_mapped < 400) {
      fork_speed = map(fork_mapped, 0, 400, 0, 255);
      fork_speed = 255 - fork_speed;
      fork_down(fork_speed);
      Serial.print("DN: ");
      Serial.print(fork_speed);
      Serial.print("  | ");
    }
  }
}

void loop() {
  throttle_read_pwm = pulseIn(THROTTLE_PIN, LOW);
  fork_read_pwm = pulseIn(FORK_PIN, LOW);
  // if (do_update_boundaries == false){loop_counter++;}
  // if (loop_counter > 1000 && do_update_boundaries == false){do_update_boundaries = true;}
  // if (do_update_boundaries == true){update_boundaries(throttle_read_pwm, fork_read_pwm);}
  throttle(throttle_read_pwm);
  fork(fork_read_pwm);
  //Serial.print(throttle_read_pwm);
  //Serial.print(" | ");
  //Serial.print(fork_read_pwm);
  Serial.println();
}

/* FORK
 * max: 18700
 * min: 17800
 *
 * THROTTLE
 * max: 18900
 * min: 17600
 */