#include <Servo.h>

Servo myservoX;  // create servo object to control the X servo
int lumen;    // variable to read the value from the analog pin
int switchStateOpen = 1; // int to store the pin value of the joystick button
int switchStateOpen = 0;

// variables will change:
int targetX = 90; // target X value of the servo <=> X value of the joystick
int currentX = 90; // current value of the X servo
int sensX =  COMMAND_OPEN;

int switchState = 1; // int to store the pin value of the joystick button

int interval = 14;
int minDegree=5;
int maxDegree = 175;

// time for 60° = o.20 sec


////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////


void setup() {
  // initialize serial communication
  Serial.begin(115200);
  
  // attach all components to correct pin
  myservoX.attach(pinServoX);   // attaches the horizontal servo on pin 9 to the servo object
  myservoY.attach(pinServoY);  // attaches the vertical servo on pin 10 to the servo object
  pinMode(pinJoyX, INPUT); // initializa the X axis of the joystick as an input
  pinMode(pinJoyY, INPUT); // initializa the Y axis of the joystick as an input
  pinMode(swPin, INPUT); // initializa the pushbutton ofnthe joystick as an input
  digitalWrite(swPin, HIGH); // set the pushbutton of the joystick to HIGH Value
  pinMode(buttonPin, INPUT); // initialize the pushbutton pin as an input
  }


void loop() {

  // converts the analog value to commands
  // reset commands
  commandX = COMMAND_NO;
  commandY = COMMAND_NO;

  // read value of 2 buttons
  buttonState = digitalRead(buttonPin); // read the value of the pushbutton
  switchState = digitalRead(swPin); // read the value of the joystick button
  

  // modify value of mode if a button is pressed
    if (buttonState != 0 ) {
      switch (mode) {
        case 2:
          buttonState = 0;
          mode = 0;
          targetX = 90;
          myservoX.write(targetX);
          targetY = 90;
          myservoY.write(targetY);
          Serial.println("mode : " + String(mode));
          delay(350);
          break;
        default:
          buttonState = 0;
          mode = mode + 1;
          targetX = 90;
          myservoX.write(targetX);
          targetY = 90;
          myservoY.write(targetY);
          Serial.println("mode : " + String(mode));
          delay(350);
        break;
    }
  }
    
    // modify value of step if a joystick button is pressed
    if (switchState != 1){
      switch (step) {
        case 5:
          switchState = 1;
          step = 1;
          targetX = 90;
          myservoX.write(targetX);
          targetY = 90;
          myservoY.write(targetY);
          Serial.println("step : " + String(step));
          delay(350);
          break;

        default:
          switchState = 1;
          step = step + 1;
          targetX = 90;
          myservoX.write(targetX);
          targetY = 90;
          myservoY.write(targetY);
          Serial.println("step : " + String(step));
          delay(350);
          break;
    }
  }

  // read value from the joystick
  joyX = analogRead(pinJoyX); // read the value of the X axis of the joystick
  joyY = analogRead(pinJoyY); // read the value of the Y axis of the joystick

  // we determine in which direction the tail should go in auto mode

  // check left/right commands
  if (joyX < LEFT_THRESHOLD)
    {commandX = COMMAND_LEFT;}
  else if (joyX > RIGHT_THRESHOLD)
    {commandX = COMMAND_RIGHT;}

  // check up/down commands
  if (joyY < DOWN_THRESHOLD)
    {commandY = COMMAND_DOWN;}
  else if (joyY > UP_THRESHOLD)
    {commandY = COMMAND_UP;}


  // we determine in which direction the tail should go in mode 1
  if (mode == 1) {
    // set commandY value
    commandY = sensY;}
  // we determine in which direction the tail should go in mode 2
  else if (mode == 2) {
    // set commandX value
    commandX = sensX;}


  // we read the current value of the servo
  currentX = myservoX.read();
  currentY = myservoY.read(); 
  //Serial.println("X value : " + String(currentX));
  //Serial.println("Y value : " + String(currentY));


  switch (commandX) {
    case COMMAND_NO:
      break;
    case COMMAND_RIGHT:
      if (currentX + step <= maxDegree){
        targetX = currentX + step;
        myservoX.write(targetX);
        delay(interval);
      
      if (targetX == maxDegree){
        sensX = COMMAND_LEFT;
      }}
      else if (currentX + step > maxDegree){
        targetX =maxDegree;
        myservoX.write(targetX);
        delay((interval/step) * (maxDegree - currentX));
        sensX = COMMAND_LEFT;
      }
      break;
    case COMMAND_LEFT:
      if (currentX - step >= minDegree){
        targetX = currentX - step;
        myservoX.write(targetX);
        delay(interval);
      
      if (targetX == minDegree){
        sensX = COMMAND_RIGHT;
      }}
      else if (currentX - step < minDegree) {
        targetX = minDegree;
        myservoX.write(minDegree);
        delay((interval/step) * (currentX - minDegree));
        sensX = COMMAND_RIGHT;
      }
      break;
  }

  while  (currentX < targetX-5 || currentX > targetX+5) {
  delay(50); 
  currentX = myservoX.read();
  Serial.println("X value : " + String(currentX));
  Serial.println("X target : " + String(targetX));
  }

  switch (commandY) {
    case COMMAND_NO:
      break;
    case COMMAND_UP:
      if (currentY + step <= maxDegree){
        targetY = currentY + step;
        myservoY.write(targetY);
        delay(interval);
      
      if (targetY == maxDegree){
        sensY = COMMAND_DOWN;
      }}

      else if (currentY + step > maxDegree){
        targetY = maxDegree;
        myservoY.write(targetY);
        delay((interval/step) * (maxDegree - currentY));
        sensY = COMMAND_DOWN;
      }
      break;
    case COMMAND_DOWN:
      if (currentY - step >= minDegree){
        targetY = currentY - step;
        myservoY.write(targetY);
        delay(interval);

        if (targetY == minDegree){
        sensY = COMMAND_UP;
        }        }

      else if (currentY - step < minDegree ) {
        targetY = minDegree;
        myservoY.write(targetY);
        delay((interval/step) * (currentY - minDegree));
        sensY = COMMAND_UP;
      }
      break;
  }

  while  ( currentY < targetY-5 || currentY > targetY+5 ) {
  delay(50); 
  currentY = myservoY.read();
  Serial.println("Y value : " + String(currentY)); 
  Serial.println("Y target : " + String(targetY));}


}