/*
This is the basic program for getting the input from joystick and making LED's symbolizing the wheels light up 
*/
const int SW_pin = 2;
const int Water_eject = 12;
const int X_pin = 0;
const int Y_pin = 2;
const int Left_Forward = 11;
const int Left_Backward = 10;
const int Right_Forward = 6;
const int Right_Backward = 5;
const int sleeptime = 50;

int brightFactor(int inputForce){ //this funcion enables more precise driving
  float brightness;
  if (inputForce > 505){
    brightness = ((inputForce - 512) / 2);
  }else{
    brightness = (255-(inputForce / 2));
  }
  
    return brightness;
}

float biasFactorRight(float biasForce){ //this factor gives the ability to make a slight turns while moving
  float biasRight;
  if(biasForce > 520){
  biasRight = 1 - ((biasForce-512)/512);
  }else{
    biasRight = 1;
  }
  if (biasForce>970){
    biasRight = 0.1;
  }
  
  return biasRight;
}

float biasFactorLeft(float biasForce){ //this factor gives the ability to make a slight turns while moving
  float biasLeft;
  if(biasForce < 490){
  biasLeft = biasForce/512;
  }else{
    biasLeft = 1;
  }
  if (biasForce<50){
    biasLeft = 0.1;
  }
  
  return biasLeft;
}

int buttonState = 1;
int X_beg_value = 512;
int Y_beg_value = 501;
int X_value = 512;
int Y_value = 512;
void setup() {
    pinMode(SW_pin, INPUT);
    pinMode(Water_eject, OUTPUT);
    digitalWrite(SW_pin, HIGH);
    pinMode(Left_Forward, OUTPUT);
    pinMode(Left_Backward, OUTPUT);
    pinMode(Right_Forward, OUTPUT);
    pinMode(Right_Backward, OUTPUT);
    Serial.begin(9600);
    
    
}

void loop() {
  buttonState = digitalRead(SW_pin);
  X_value = analogRead(X_pin);
  Y_value = analogRead(Y_pin);
  Serial.print("\nX_value:"); //this part shows the current parameters and was used during testing
  Serial.print(X_value);
  Serial.print("\nY_value:");
  Serial.print(Y_value);
  Serial.print("\n");
  Serial.print("\nbrightFactor(X_value):");
  Serial.print(brightFactor(X_value));
  Serial.print("\nbrightFactor(Y_value):");
  Serial.print(brightFactor(Y_value));
  Serial.print("\nbiasFactorRight(Y_value)");
  Serial.print(biasFactorRight(Y_value));
  Serial.print("\nbiasFactorLeft(Y_value)");
  Serial.print(biasFactorLeft(Y_value));
  
  
  if (buttonState == LOW) { //this button controls the water tower
    // turn LED on:
    digitalWrite(Water_eject, HIGH);
  } else {
    // turn LED off:
    digitalWrite(Water_eject, LOW);
  }
  
  
  
  if(X_value > 542 || X_value < 482 || Y_value > 532 || Y_value < 472){ //condition for the movement
  
  //movement beggining
  if(X_value > 530 || X_value < 500){ //this part governs the x axis motion
    //actual driving part
  if (X_value > 524) {
    //movement forward
    analogWrite(Left_Forward, brightFactor(X_value)*biasFactorLeft(Y_value));
    analogWrite(Left_Backward, 0);
    analogWrite(Right_Forward, brightFactor(X_value)*biasFactorRight(Y_value));
    analogWrite(Right_Backward, 0);
    delay(sleeptime);
  } else {
    //movement backward
    analogWrite(Left_Forward, 0);
    analogWrite(Left_Backward, brightFactor(X_value)*biasFactorLeft(Y_value));
    analogWrite(Right_Forward, 0);
    analogWrite(Right_Backward, brightFactor(X_value)*biasFactorRight(Y_value));
    delay(sleeptime);
  }
    //which ends here
  }else {
    if(Y_value > 540 || Y_value < 490){
       
    if (Y_value > 540) {
    //movement forward
    analogWrite(Left_Forward, brightFactor(Y_value));
    analogWrite(Left_Backward, 0);
    analogWrite(Right_Forward, 0);
    analogWrite(Right_Backward, brightFactor(Y_value));
    delay(sleeptime);
  } else {
    //movement backward
    analogWrite(Left_Forward, 0);
    analogWrite(Left_Backward, brightFactor(Y_value));
    analogWrite(Right_Forward, brightFactor(Y_value));
    analogWrite(Right_Backward, 0);
    delay(sleeptime);
  }
    //which ends here
  
  
}
} 
  
}else {
    digitalWrite(Left_Forward, 0); //if the input is too weak to trigger the movement, there is no movement
    digitalWrite(Left_Backward, 0);
    digitalWrite(Right_Forward, 0);
    digitalWrite(Right_Backward, 0);
    delay(sleeptime);
  }
}