/*
  Tutorials used:
  https://www.youtube.com/watch?v=7woqNH_qby4&list=PLHvJ4bw0xno5u_Hv2nf9Hj3DWW5jLdwMz&index=4
  https://www.youtube.com/watch?v=T338v4mxCvY
  https://www.youtube.com/watch?v=J_kbyAY1rLM
  https://mechatrofice.com/arduino/servo-motor-push-button-control
  https://forum.arduino.cc/t/ramping-easing-servo-between-points/101503
  https://www.youtube.com/watch?v=lZRJEMEerMw
  https://forum.arduino.cc/t/simple-joystick-servo-demo/325903
 */

String readString;
#include <Servo.h>

// Define pin locations
#define xPin A0
#define servo 9
#define CENTER 2
#define LEFT 3
#define RIGHT 4
#define fullRIGHT 5
#define fullLEFT 6

Servo myservo;  // create servo object to control a servo

int xMaxAngle = 120, xMinAngle = 60, angle;
float xSpeed = 2, xSpeedManual = 5, xSpeedCoeff = 0.1;
int statusMonitor = 1;

void setup() { //setup code 
  Serial.begin(9600);  //Initilize serial communication
  Serial.println("Starting Direction Control Sequence");
  myservo.attach(servo);  // attaches the servo on pin 9 to the servo object
  myservo.write(90);

  pinMode(CENTER, INPUT_PULLUP);   // pullup used for buttons (high and low positions)
  pinMode(LEFT, INPUT_PULLUP);
  pinMode(RIGHT, INPUT_PULLUP);
  pinMode(fullLEFT, INPUT_PULLUP);
  pinMode(fullRIGHT, INPUT_PULLUP);
}

void loop() { //main code, runs repeatedly

  readJoystick();
  readLEFT();
  readRIGHT();
  readFullLeft();
  readFullRight();
  readCENTER();
  myservo.write(angle);
  //delay(50);

  Serial.print("ServoPos ");
  Serial.println(angle);
  Serial.print("StatusMonitor ");
  Serial.println(statusMonitor);
}

void readJoystick(){
 int xValue = analogRead(xPin);    // reads the value of the analog pins (value between 0 and 1023)
 int xMap = map(xValue,0, 1023, -500, 500);
 
 Serial.print("MAP        ");
 Serial.print(xMap);
 Serial.print("       X Value");
 Serial.println(xValue);
 
 
 if (angle <= xMinAngle) { 
  angle = xMinAngle;
  xSpeed = 0;
  xSpeedCoeff = 0;
  statusMonitor = 6;
 }
 if (xValue < 450){
    xSpeedCoeff = abs(xMap/125);
    xSpeed = 2;
    xSpeed = xSpeed * xSpeedCoeff;
                                        if (angle > 60) {
                                        angle -= xSpeed;
                                        }
   statusMonitor = 2;
 }
 if (angle >= xMaxAngle){
   angle = xMaxAngle;
   xSpeed = 0;
   xSpeedCoeff = 0;
   statusMonitor = 3;
  }
 if (xValue > 550) {     // Higher dead zone limit.
   xSpeedCoeff = abs(xMap/125);
   xSpeed = 2;
    xSpeed = xSpeed * xSpeedCoeff;
                                        if (angle < 120) {
                                        angle += xSpeed;
                                        }
    
  
   statusMonitor = 4;
 }
  Serial.print("                              XSpeed ");
  Serial.print(xSpeed);
  Serial.print("      xSpeedCoeff    ");
  Serial.println(xSpeedCoeff);
 
}

void readLEFT(){
 int leftOutput = digitalRead(LEFT);
   if(leftOutput == LOW) {
      angle -= xSpeedManual;
      if (angle <= xMinAngle) {
          angle = xMinAngle;
  }
   }
   }

void readRIGHT(){
 int rightOutput = digitalRead(RIGHT);
   if(rightOutput == LOW) {
      angle += xSpeedManual;
      if (angle > xMaxAngle) {
          angle = xMaxAngle;
  }
   }
}

void readFullLeft(){
 int fullLEFTOutput = digitalRead(fullLEFT);
 if(fullLEFTOutput == LOW) {
   angle = xMinAngle;
   }
}

void readFullRight(){
 int fullRIGHTOutput = digitalRead(fullRIGHT);
 if(fullRIGHTOutput == LOW) {
   angle = xMaxAngle;
   }
}

void readCENTER() {
 int centerOutput = digitalRead(CENTER);
 if(centerOutput == LOW) {
   angle = 90;
   }
}