#define xaxis A1
#define yaxis A0
#define brake 46

#define Lproximity 44
#define Rproximity 42

#define REN1 39
#define REN2 41
#define RPWM 11

#define LEN1 38
#define LEN2 40
#define LPWM 12

int direction = 0, steerIntensity = 0, driveIntensity, braking = 0;
bool lproxval, rproxval;

#define FORWARD 0
#define FORWARDRIGHT 1
#define FORWARDLEFT 2
#define REVERSE 3
#define REVERSERIGHT 4
#define REVERSELEFT 5
#define RIGHT 6
#define LEFT 7
#define NOMOVE 4



void get_joystick(void){
int x = analogRead(xaxis);
int y = analogRead(yaxis);
braking = digitalRead(brake);
steerIntensity = 512 - x;
driveIntensity = y - 512;
}

void get_proximity(void){
lproxval = digitalRead(Lproximity);
rproxval = digitalRead(Rproximity);
}

void printInputs(void){
Serial.print("Intensity Horizontally = ");
Serial.print(steerIntensity);
Serial.print("\tIntensity Vertically = ");
Serial.print(driveIntensity);
Serial.print("\tBraking = ");
Serial.print(braking);
Serial.print("\tLeftProx = ");
Serial.print(lproxval);
Serial.print("\tRightProx = ");
Serial.println(rproxval);
}

void move(void){
if(braking == 0 || lproxval == 0 || rproxval == 0)
{
  Serial.println("Stopping");
  analogWrite(RPWM, 0);
  analogWrite(LPWM, 0);
}else if(driveIntensity > 10 && steerIntensity > 10)
{
  Serial.println("Moving Forward Right");
}
else if(driveIntensity > 10 && steerIntensity < -10)
{
  Serial.println("Moving Forward Left");
}
else if(driveIntensity < -10 && steerIntensity > 10)
{
  Serial.println("Moving Reverse Right");
}
else if(driveIntensity < -10 && steerIntensity < -10)
{
  Serial.println("Moving Reverse Left");
}
else if(steerIntensity < -10)
{
  Serial.println("Moving Left");
}
else if(steerIntensity > 10)
{
  Serial.println("Moving Right");
}
else if(driveIntensity < -10)
{
  Serial.println("Moving Reverse");
}
else if(driveIntensity > 10)
{
  Serial.println("Moving Forward");
}
else{
  Serial.println("Stopping!!!! NO Command!");
}
}
void setup() {
  // put your setup code here, to run once:
pinMode(brake, INPUT_PULLUP);
pinMode(Lproximity, INPUT_PULLUP);
pinMode(Rproximity, INPUT_PULLUP);

pinMode(LEN1, OUTPUT);
pinMode(LEN2, OUTPUT);
pinMode(LPWM, OUTPUT);

pinMode(REN1, OUTPUT);
pinMode(REN2, OUTPUT);
pinMode(RPWM, OUTPUT);

Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:
// for(int i = 0; i<256 ; i++)
// {
//   analogWrite(RPWM, i);
//   analogWrite(LPWM, i);
//   delay(10);
// }
// for(int i = 255; i>=0 ; i--)
// {
//   analogWrite(RPWM, i);
//   analogWrite(LPWM, i);
//   delay(10);
// }
// Serial.println(digitalRead(brake));
get_proximity();
get_joystick();
// printInputs();
move();
// delay(100);
}