#include <Servo.h>

//Servo Stuff
int servoRotationPin = 0;
int servoHorizontalPin = 1;

int rotMin = 0; //Min position of servo
int rotMax = 160; //Max position of servo
int rotStart = 100; //Start position of servo

int hozMin = 94; //Min position of servo
int hozMax = 184; //Max position of servo
int hozStart = 142; //Start position of servo

int panInterval = 5;
int tiltInterval = 50;

Servo rotServo;
Servo hozServo;
Servo fwdServo;

int currentRotPosition = rotStart;
int currentHosPosition = hozStart;


int leftWheelPWMPin = 3;  //PWM (Sets Speed) Range from 0-255
int rightWheelPWMPin = 9;  //PWM (Sets Speed) Range from 0-255
int leftWheelDirectionPin = 2;  //Direction LOW is Forward. HIGH is Backwards
int rightWheelDirectionPin = 8;  //Direction LOW is Forward. HIGH is Backwards

//Generally the wheels won't turn at the same speed. To fix this we use an offset to lower the power from the stronger motor.
int leftWheelSpeedOffset = 0;
int rightWheelSpeedOffset = 0;

int spd = 255; //Default speed
char incomingByte = 0; //Single character used for serial communication to determine the direction to go

void setup()
{
  //Set control pins to be outputs
  pinMode(leftWheelPWMPin, OUTPUT);
  pinMode(rightWheelPWMPin, OUTPUT);
  pinMode(leftWheelDirectionPin, OUTPUT);
  pinMode(rightWheelDirectionPin, OUTPUT);

  //Start the serial interface
  Serial.begin(115200);

  //Servo setup
  rotServo.attach(servoRotationPin);
  hozServo.attach(servoHorizontalPin);

 //Servo set default points
  rotServo.write(rotStart);
  hozServo.write(hozStart);
}

void loop()
{
  if (Serial.available() > 0) {
    // read the incoming byte:
    incomingByte = Serial.read();
    switch(incomingByte){
    case 70: //F
      forward(spd);
      break;
    case 66: //B
      backward(spd);
      break;
    case 76: //L
      left(spd);
      break;
    case 82: //R
      right(spd);
      break;
    case 83: //S
      brake();
      break;
    case 65: //A
      rotateLeft();
      break;
    case 67: //C
      rotateRight();
      break;
    case 71: //G
      armUp();
      break;
    case 72: //H
      armDown();
      break;
    }
  }
}

void forward(int speed){
  setSpeed(speed);
  digitalWrite(leftWheelDirectionPin, LOW); //Left Side Forward
  digitalWrite(rightWheelDirectionPin, LOW); //Right Side Forward


}

void backward(int speed){
  setSpeed(speed);
  digitalWrite(leftWheelDirectionPin, HIGH); //Left Side Backward
  digitalWrite(rightWheelDirectionPin, HIGH); //Right Side Backward
}

void left(int speed){
  setSpeed(speed);
  digitalWrite(leftWheelDirectionPin, HIGH); //Left Side Backward
  digitalWrite(rightWheelDirectionPin, LOW); //Right Side Forward
}

void right(int speed){
  setSpeed(speed);
  digitalWrite(leftWheelDirectionPin, LOW);   //Left Side Forward
  digitalWrite(rightWheelDirectionPin, HIGH); //Right Side Backward
}

void brake(){
  analogWrite(leftWheelPWMPin, 0);   //Set Left Side Speed to off
  analogWrite(rightWheelPWMPin, 0);  //Set Right Side Speed to off
}

void setSpeed(int speed){
  int leftWheelSpeed = 0;
  int rightWheelSpeed = 0;
  if(speed + leftWheelSpeedOffset > 255){ //Make sure we don't set the pwm speed over the max of 255
    leftWheelSpeed = 255;
  }else if(speed + leftWheelSpeedOffset < 0){ //Make sure we don't set the pwm speed under the min of 0
    leftWheelSpeed = 0;
  }else{ //If it is in the range then just set it
    leftWheelSpeed = speed + leftWheelSpeedOffset;
  }

  if(speed + rightWheelSpeedOffset > 255){ //Make sure we don't set the pwm speed over the max of 255
    rightWheelSpeed = 255;
  }else if(speed + rightWheelSpeedOffset < 0){ //Make sure we don't set the pwm speed under the min of 0
    rightWheelSpeed = 0;
  }else{ //If it is in the range then just set it
    rightWheelSpeed = speed + rightWheelSpeedOffset;
  }
  analogWrite(leftWheelPWMPin, leftWheelSpeed);    //Left Side Speed
  analogWrite(rightWheelPWMPin, rightWheelSpeed);  //Right Side Speed
}



void rotateLeft(){
  if(currentRotPosition < rotMax){
    rotServo.write(currentRotPosition+=10);
  }
}


void rotateRight(){
  if(currentRotPosition > rotMin){
    rotServo.write(currentRotPosition-=10);
  }
}

void armUp(){
  if(currentHosPosition < hozMax){
    hozServo.write(currentHosPosition+=10);
  }
}
void armDown(){
  if(currentHosPosition > hozMin){
    hozServo.write(currentHosPosition-=10);
  }
}