#include <Servo.h>

// pin definitions
const int YOKE_PIN  =  5;
const int MOTOR_PIN =  6;
const int TUG_PIN   =  7;
const int CLIMB_PIN =  8;
const int RACE_PIN  =  9;
const int ECHO_PIN  = 10;
const int TRIG_PIN  = 11;
const int MODE_PIN  = 12;
// steering constants
const int LEFT_TURN  =  60;
const int STRAIGHT   =  90;
const int RIGHT_TURN = 120;

// global variables
int compMode = 0; // can only be 1 of 3 values 0-2
int oldBtnState = HIGH;

// create servo object
Servo steering;

bool checkButton()  {
  bool wasPressed = false;

  int btnState = digitalRead(MODE_PIN);  // read the pin
  if (btnState != oldBtnState)  {       // if it changed
    oldBtnState = btnState;             // remember the state
    if (btnState == LOW)  {             // was just pressed
      //Serial.println("Button Pressed");
      wasPressed = true;
    } else  {                           // was just released
      //Serial.println("Button Released");
    }
    delay(20);  // short delay to debounce button
  }
  return wasPressed;
}

int getDistance() {
  // send trigger
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  // calculate distance
  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration * .0343) / 2;
  // return distance as an integer
  return (int)distance;
}

void climb() {
  //Serial.println("Climb");
}

void tug() {
  //Serial.println("Tug");
}

void race(int direction, int distance) {
  if (distance > 50) {
    direction = 0;
  }
  else if (distance < 10) {
    direction = 1;
  }
  else if (distance < 25) {
    direction = 2;
  }
  steer(direction);
}

int steer(int direction) {
  if (direction == 0) {
    steering.write(STRAIGHT);
  }
  if (direction == 1) {
    steering.write(LEFT_TURN);
  }
  if (direction == 2) {
    steering.write(RIGHT_TURN);
  }
  return;
}

void setup() {
  Serial.begin(115200);
  pinMode(MODE_PIN, INPUT_PULLUP); // setup mode toggle
  pinMode(RACE_PIN, OUTPUT); // set event LED pins
  pinMode(CLIMB_PIN, OUTPUT);
  pinMode(TUG_PIN, OUTPUT);
  pinMode(MOTOR_PIN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT); // setup ultrasonic sensor
  pinMode(ECHO_PIN, INPUT);
  steering.attach(YOKE_PIN); // setup steering controls
  steering.write(STRAIGHT);
}

void loop () {
  int direction = 0; // store left/right/straight, will convert to int when line follower is implemented
  int distance = getDistance(); // returns distance in cm
  bool isPressed = checkButton();
  if (isPressed)  {
    Serial.print("Mode: ");
    Serial.println(compMode);
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");
    compMode++;
    if (compMode == 3) {
      compMode = 0;
    }
  }
  switch (compMode) {
    case 0:
      digitalWrite(RACE_PIN, HIGH);
      digitalWrite(CLIMB_PIN, LOW);
      digitalWrite(TUG_PIN, LOW);
      race(direction, distance);
      break;
    case 1:
      digitalWrite(RACE_PIN, LOW);
      digitalWrite(CLIMB_PIN, HIGH);
      digitalWrite(TUG_PIN, LOW);
      climb();
      break;
    case 2:
      digitalWrite(RACE_PIN, LOW);
      digitalWrite(CLIMB_PIN, LOW);
      digitalWrite(TUG_PIN, HIGH);
      tug();
      break;
  }
}