const int STEERING_POT_CS_N  = 11;    // Select pin for Potentiometer 1 (servo steering)
const int STEERING_POT_UD    = 12;    // Up/Down pin for Potentiometer 1
const int STEERING_POT_INC_N = 13;    // Increment pin for Potentiometer 1

const int MOTOR_POT_CS_N  = 4;     // Select pin for Potentiometer 2 (motor power)
const int MOTOR_POT_UD    = 5;     // Up/Down pin for Potentiometer 2
const int MOTOR_POT_INC_N = 6;     // Increment pin for Potentiometer 2

// potentiometer step constants
const int STATIC_MOTOR_STEP = 60;
const int STATIC_STEERING_STEP = 17;
const int STEERING_MAX = 34;
const int MOTOR_MAX = 100;
const int MOTOR_MIN = 0;

const int PI_BAUD_RATE = 9600;

int steering_counter = 0;   // value that stores the amount we've incremented by for pot1
int motor_counter = 0;      // value that stores the amount we've incremented by for pot2

String incomingDirection = "stop";

int desired_speed;      // dynamic target value for the movement motor
int desired_angle;      // dynamic target value for the servo steering motor

int forward_speed_cap;
int backward_speed_cap;

bool hit_motor_target;
bool hit_steering_target;

void setup() {
  // declare the Potentiometer pins as OUTPUTs:
  pinMode(STEERING_POT_CS_N, OUTPUT);
  pinMode(STEERING_POT_UD, OUTPUT);
  pinMode(STEERING_POT_INC_N, OUTPUT);

  pinMode(MOTOR_POT_CS_N, OUTPUT);
  pinMode(MOTOR_POT_UD, OUTPUT);
  pinMode(MOTOR_POT_INC_N, OUTPUT);

  steering_counter = 0;
  motor_counter = 0;

  desired_speed = STATIC_MOTOR_STEP;
  desired_angle = STATIC_STEERING_STEP;
  forward_speed_cap = 80;
  backward_speed_cap = 0;

  hit_motor_target = true;
  hit_steering_target = true;

  Serial.begin(PI_BAUD_RATE);
}

// RESET THE POTENTIOMETERS! **************************************************
void reset_potens() {
  // start high to reset potentiometer
  digitalWrite(STEERING_POT_CS_N, HIGH);
  digitalWrite(MOTOR_POT_CS_N, HIGH);
  digitalWrite(STEERING_POT_INC_N, HIGH);
  digitalWrite(MOTOR_POT_INC_N, HIGH);
  // ask_and_wait("set high");
  delay(2);

  // forcing the counter down to zero:
  digitalWrite(STEERING_POT_CS_N, LOW);  // activate potentiometer
  digitalWrite(MOTOR_POT_CS_N, LOW);  // activate potentiometer
  digitalWrite(STEERING_POT_UD, HIGH);    // counter set to increment down
  digitalWrite(MOTOR_POT_UD, HIGH);    // counter set to increment down
  // ask_and_wait("set low");

  // tick the counter down a bunch of times
  for (int i = 0; i < 100; i++) {
    digitalWrite(STEERING_POT_INC_N, HIGH);  // set pos
    digitalWrite(MOTOR_POT_INC_N, HIGH);  // set pos
    delay(2);
    
    digitalWrite(STEERING_POT_INC_N, LOW);   // make neg edge to increment
    digitalWrite(MOTOR_POT_INC_N, LOW);   // make neg edge to increment
    // Serial.print("stepping with reset value: ");
    // Serial.println(i);
    // ask_and_wait("");
    delay(2);

  }

  digitalWrite(STEERING_POT_INC_N, HIGH);    // set back to pos
  digitalWrite(MOTOR_POT_INC_N, HIGH);    // set back to pos
  delay(2);
}


/* ***** ***** SET TARGET FUNCTIONS ***** ***** */

void set_motor_target_forward() {
  desired_speed = forward_speed_cap;
}

void set_motor_target_backward() {
  desired_speed = backward_speed_cap;
}

void set_angle_target_right() {
  desired_angle = 0;
}

void set_angle_target_left() {
  desired_angle = STEERING_MAX;
}


void reset_motor_target() {
  desired_speed = STATIC_MOTOR_STEP;
}

void reset_angle_target() {
  desired_angle = STATIC_STEERING_STEP;
}


void reset_targets() {
  reset_angle_target();
  reset_motor_target();
}

void set_targets(String direction) {
  if (!direction.compareTo("stop")) {
    reset_targets();
  } else if (!direction.compareTo("forward")) {
    set_motor_target_forward();
    reset_angle_target();
    Serial.println("Moving forward");
  } else if (!direction.compareTo("left")) {
    set_motor_target_forward();
    set_angle_target_left();
    Serial.println("Drifting left");
  } else if (!direction.compareTo("right")) {
    set_motor_target_forward();
    set_angle_target_right();
    Serial.println("Drifting right");
  } else {
    Serial.print("Received invalid direction: ");
    Serial.println(direction);
  }
}

/* ***** ***** REACT/CHASE TARGET FUNCTIONS ***** ***** */

// set UD counter to high or low based on the current value and target
// FOR THE STEERING SERVO
void react_to_angle_target() {
  if (steering_counter < desired_angle) {
    digitalWrite(STEERING_POT_UD, LOW);
    hit_steering_target = false;
  } else if (steering_counter > desired_angle) {
    digitalWrite(STEERING_POT_UD, HIGH);
    hit_steering_target = false;
  }
}

// create negative edges for the INCREMENT values as needed
// FOR THE STEERING SERVO
void chase_angle_target() {
  if (steering_counter < desired_angle) {
    digitalWrite(STEERING_POT_INC_N, LOW);
    steering_counter++;
  } else if (steering_counter > desired_angle) {
    digitalWrite(STEERING_POT_INC_N, LOW);
    steering_counter--;
  }

  if (steering_counter == desired_angle) {
    hit_steering_target = true;
  }
}

// set UD counter to high or low based on the current value and target
// FOR THE MOTOR
void react_to_motor_target() {
  if (motor_counter < desired_speed) {
    digitalWrite(MOTOR_POT_UD, LOW);
    hit_motor_target = false;
  } else if (motor_counter > desired_speed) {
    digitalWrite(MOTOR_POT_UD, HIGH);
    hit_motor_target = false;
  }
}

// create negative edges for the INCREMENT values as needed
// FOR THE MOTOR
void chase_motor_target() {
  if (motor_counter < desired_speed) {
    digitalWrite(MOTOR_POT_INC_N, LOW);
    motor_counter++;
  } else if (motor_counter > desired_speed) {
    digitalWrite(MOTOR_POT_INC_N, LOW);
    motor_counter--;
  }

  if (motor_counter == desired_speed) {
    hit_motor_target = true;
  }
}

// first react to new angle/motor target changes by changing U/D pins
// then chase those targets with a INCREMENT Neg edge if needed
void chase_targets() {
  react_to_angle_target();
  react_to_motor_target();

  // make sure that combinational logic can propogate
  // in the digital potentiometers with a delay
  delay(2);

  chase_angle_target();
  chase_motor_target();
}




/* ***** ***** RASPI INTERFACE FUNCTIONS ***** ***** */
String get_direction() {
  
  // only change input if we've hit all targets
  // if (hit_motor_target && hit_steering_target) {
  //   incomingDirection = "stop";
  // }

  // String incomingString = Serial.read();
  // char incomingByte = incomingString.charAt(0));
  if (Serial.available()) {
    incomingDirection = Serial.readStringUntil('\n');
    // Serial.println(incomingByte);
  }

  return incomingDirection;
}

void loop() {
  // Serial.println("Initializing Potentiometers:");
  reset_potens();

  while (1) {

    // Serial.print("SPEED STEP = ");
    // Serial.print(motor_counter);
    // Serial.print(" | SERVO STEP = ");
    // Serial.println(steering_counter);

    String direction = get_direction();
    // c = 'a';

    set_targets(direction);

    chase_targets();

    // add slight delay to make negedges distinct between loops
    delay(2);
    // end by pulling all potens high again
    digitalWrite(STEERING_POT_INC_N, HIGH);
    digitalWrite(MOTOR_POT_INC_N, HIGH);
  }
}