#include <Servo.h>

/*Paramètres ServoMoteur*/
const int pinServo1 = 8;
const int pinServo2 = 9;
const int pinServo3 = 10;
Servo servo1;  // servomoteur au point A (bras gauche)
Servo servo2;  // servomoteur au point B (bras droit)
Servo servo3;  // servomoteur qui lève et baisse le stylo
/*Paramètres Bouton poussoir*/
const int button1 = 3;  // Pin digital pour le bouton poussoir
const int button2 = 4;
const int button3 = 5;
const int button4 = 6;
int buttonState1;  // état du bouton poussoir (HIGH ou LOW)
int buttonState2;
int buttonState3;
int buttonState4;
/*Paramètres Joystick*/
const int SW_pin = 2;  // Pin digital pour indiquer la postion du bouton poussoir du Joystick
int buttonStateSW;     // état du bouton poussoir du Joystick
int Pen;               // 1 si le stylo touche le papier, 0 sinon
const int X_pin = A0;  // Pin analogique pour la coordonnée X
const int Y_pin = A1;  // Pin analogique pour la coordonnée Y
float joyX, joyY;      // Variables pour les valeurs données par le joystick à potentiomètre 2 axes
/*Paramètres du Modèle Géométrique*/
float Ex, Ey;                 // coordonnées cartésiennes du stylo
float theta1, theta2;         // correspondent respectivement à l'angle theta A et theta B du modèle géométrique
const float angleHIGH = 100;  // angles de servo3 correspondants aux positions basses et hautes du stylo
const float angleLOW = 110;
const float D = 80;        // distance entre deux servomoteurs 1 et 2 en mm
const float L = 90;        // longueur d'un bras en mm
const float centerX = 40;  // coordonnées du centre du carré 50mm x 50mm
const float centerY = 140;
const float R = 25;              // rayon de cercle en mm
const float Xmin = centerX - R;  // cadrer un carré de 50mm x 50mm
const float Xmax = centerX + R;
const float Ymin = centerY - R;
const float Ymax = centerY + R;
/*Paramètres Vitesse*/
String message;               // chaîne de caractères pour stocker la commande reçue à partir du moniteur série
int speed;                    // vitesse de mouvement pendant le mode Joystick
                              // allons de 0 à 5 // 0 : pas de mouvement, 1 : très lent, 3 : moyen, 5 : très rapide
int pause;                    // le temps de pause à chaque mouvement pendant le mode Joystick
                              // plus le nombre est petit, plus la rotation est rapide
const int autoPause = 50;     // la pause pendant les modes automatiques
const int transitTime = 300;  // le temps de passage de servo1,2.write() à servo3.write() (et inversement)

void setup() {
  /*Initialisation Servomoteur*/
  pinMode(pinServo1, OUTPUT);
  pinMode(pinServo2, OUTPUT);
  pinMode(pinServo3, OUTPUT);
  servo1.attach(pinServo1);
  servo2.attach(pinServo2);
  servo3.attach(pinServo3);
  servo3.write(90);
  delay(transitTime);
  theta1 = servo1.read();
  theta2 = servo2.read();
  calculatePosition(theta1, theta2);
  GoToPoint(centerX, centerY);
  /*Initialisation Bouton poussoir*/
  pinMode(button1, INPUT_PULLUP);  // INPUT_PULLUP lie l'entrée à 5V avec une résistance interne de 10kOhm
  pinMode(button2, INPUT_PULLUP);
  pinMode(button3, INPUT_PULLUP);
  pinMode(button4, INPUT_PULLUP);
  buttonState1 = digitalRead(button1);
  buttonState2 = digitalRead(button2);
  buttonState3 = digitalRead(button3);
  buttonState4 = digitalRead(button4);
  /*Initialisation Joystick*/
  pinMode(SW_pin, INPUT_PULLUP);
  buttonStateSW = digitalRead(SW_pin);
  pinMode(X_pin, INPUT);
  pinMode(Y_pin, INPUT);
  /*Configuration du moniteur série*/
  Serial.begin(9600);
  Serial.setTimeout(1);  // définit le nombre maximum de millisecondes d'attente pour les données série
  /*Configuration de la vitesse*/
  Serial.println("\nVeuillez régler la vitesse (0 à 5): ");
  message = "";
  setSpeed();
  servo3.write(angleHIGH);
  Pen = 0;

  delay(1000);
}

void loop() {
  /*Réglage de la vitesse*/
  if (Serial.available() > 0) {
    setSpeed();
  }
  /*Mode de pilotage par Joystick*/
  if (digitalRead(SW_pin) != buttonStateSW)  // si le bouton1 est appuyé
  {
    PenUpDown();
    delay(500);
  }
  joyX = analogRead(X_pin);  // lire les valeurs de potentiomètre
  joyY = analogRead(Y_pin);
  if (joyX > 700 && Ex < Xmax) {
    Ex += 0.1;
  }
  if (joyX < 300 && Ex > Xmin) {
    Ex -= 0.1;
  }
  if (joyY > 700 && Ey > Ymin) {
    Ey -= 0.1;
  }
  if (joyY < 300 && Ey < Ymax) {
    Ey += 0.1;
  }
  calculateAngle(Ex, Ey);
  servo1.write(theta1);
  servo2.write(theta2);
  delay(pause);
  /*Tracer une ligne*/
  if (digitalRead(button1) != buttonState1)  // si le bouton1 est appuyé
  {
    Serial.println("Tracer une ligne");
    servo3.write(angleHIGH);  // lever le stylo
    delay(transitTime);
    GoToPoint(Xmin, Ey);  // aller au point le plus gauche
    delay(transitTime);
    servo3.write(angleLOW);  // baisser le stylo
    delay(transitTime);
    while (Ex < Xmax) {  // déplacer horizontalement le stylo vers la droite par millimètres
      Ex++;
      calculateAngle(Ex, Ey);
      servo1.write(theta1);
      servo2.write(theta2);
      delay(autoPause);
    }
    delay(transitTime);
    servo3.write(angleHIGH);  // lever le stylo
    Pen = 0;
    delay(transitTime);
  }
  /*Tracer un cercle*/
  if (digitalRead(button2) != buttonState2) {
    Serial.println("Tracer un cercle");
    servo3.write(angleHIGH);  // lever le stylo
    delay(transitTime);
    float theta = 0;
    float startX = centerX + R * cos(theta);
    float startY = centerY + R * sin(theta);
    GoToPoint(startX, startY);  // aller au point de l'angle de 0 rad
    delay(transitTime);
    servo3.write(angleLOW);  // baisser le stylo
    delay(transitTime);
    while (theta < 2 * PI) {  // allons de 0 rad à 2*pi rad
      theta = theta + 0.1;    // augmenter l'angle de 0.1 rad
      Ex = centerX + R * cos(theta);
      Ey = centerY + R * sin(theta);
      calculateAngle(Ex, Ey);
      servo1.write(theta1);
      servo2.write(theta2);
      delay(autoPause);
    }
    delay(transitTime);
    servo3.write(angleHIGH);  // baisser le stylo
    Pen = 0;
    delay(transitTime);
  }
  /*Tracer une ligne pointillée*/
  if (digitalRead(button3) != buttonState3) {
    Serial.println("Tracer une ligne pointillée");
    servo3.write(angleHIGH);
    delay(transitTime);
    GoToPoint(Xmin, Ey);
    Ex = Xmin;
    delay(transitTime);
    servo3.write(angleLOW);
    Pen = 1;
    delay(transitTime);
    while (Ex < Xmax) {
      Ex++;
      if ((int)Ex % 5 == 0)  // lever ou baisser le stylo tous les 5 mm
      {
        PenUpDown();
      }
      calculateAngle(Ex, Ey);
      servo1.write(theta1);
      servo2.write(theta2);
      delay(autoPause);
    }
    delay(transitTime);
    servo3.write(angleHIGH);
    Pen = 0;
    delay(transitTime);
  }
  /*Tracer un cercle pointillé*/
  if (digitalRead(button4) != buttonState4) {
    Serial.println("Tracer un cercle pointillé");
    servo3.write(angleHIGH);
    delay(transitTime);
    float theta = 0;
    float startX = centerX + R * cos(theta);
    float startY = centerY + R * sin(theta);
    GoToPoint(startX, startY);
    delay(transitTime);
    servo3.write(angleLOW);
    Pen = 1;
    delay(transitTime);
    while (theta < 360) {          // allons de 0° à 360°
      theta = theta + 1;           // augmenter l'angle de 1°
      if ((int)theta % 15 == 0) {  // lever ou baisser le stylo tous les 15°
        PenUpDown();
      }
      Ex = centerX + R * cos(radians(theta));
      Ey = centerY + R * sin(radians(theta));
      calculateAngle(Ex, Ey);
      servo1.write(theta1);
      servo2.write(theta2);
      delay(15);
    }
    delay(transitTime);
    servo3.write(angleHIGH);
    Pen = 0;
    delay(transitTime);
  }
}

/* Loi entrée/sortie du modèle géométrique direct*/
void calculatePosition(float x, float y) {
  /*Ajuster à l'angle des servos*/
  x = 180 - x;
  y = 180 - y;

  x = radians(x);
  y = radians(y);
  float Dx = L * cos(x);
  float Dy = L * sin(x);
  float Cx = D + L * cos(y);
  float Cy = L * sin(y);
  float P = sqrt(pow(Cx - Dx, 2) + pow(Cy - Dy, 2));
  float alpha1 = acos(P / (2 * L));
  float alpha2 = atan((Cy - Dy) / (Cx - Dx));
  Ex = Dx + L * cos(alpha1 + alpha2);
  Ey = Dy + L * sin(alpha1 + alpha2);
}

/* Loi entrée/sortie du modèle géométrique inverse*/
void calculateAngle(float x, float y) {
  float n = sqrt(pow(x, 2) + pow(y, 2));
  float m = sqrt(pow(D - x, 2) + pow(y, 2));
  float alpha1 = acos(n / (2 * L));
  float alpha2 = acos(m / (2 * L));
  float beta1 = atan(y / x);
  float beta2 = atan(y / (D - x));
  theta1 = alpha1 + beta1;
  theta2 = PI - (alpha2 + beta2);
  theta1 = degrees(theta1);
  theta2 = degrees(theta2);

  /*Ajuster à l'angle des servos*/
  theta1 = 180 - theta1;
  theta2 = 180 - theta2;
}
/*Lever ou baisser le stylo*/
void PenUpDown() {
  if (Pen == 1)  // si le stylo touche le papier
  {
    servo3.write(angleHIGH);  // lever le stylo
    Pen = 0;
  } else  // sinon
  {
    servo3.write(angleLOW);  // baisser le stylo
    Pen = 1;
  }
}
/*Réglage de vitesse*/
void setSpeed() {
  speed = 0;
  while (speed == 0) {
    while (!Serial.available()) {
      delay(1);  // attandre de recevoir la commande de l'utilisateur
    }
    while (Serial.available()) {
      delay(2);  // délai pour permettre au buffer de se remplir
      if (Serial.available() > 0) {
        char c = Serial.read();
        message += c;
      }
    }
    Serial.print("La vitesse : ");
    Serial.print(message);
    if (message != "0\n") {  // si la vitesse commandée n'est pas de 0
      speed = message.toInt();
      pause = (int)(10 - speed * 2);
      Serial.println(pause);
    }
    message = "";
  }
}
/*Aller au point désigné*/
void GoToPoint(float x, float y) {
  while (Ex < x - 0.1 || Ex > x + 0.1 || Ey < y - 0.1 || Ey > y + 0.1) {
    if (Ex < x && Ex < Xmax) {
      Ex += 0.1;
    }
    if (Ex > x && Ex > Xmin) {
      Ex -= 0.1;
    }
    if (Ey < y && Ey < Ymax) {
      Ey += 0.1;
    }
    if (Ey > y && Ey > Ymin) {
      Ey -= 0.1;
    }
    calculateAngle(Ex, Ey);
    servo1.write(theta1);
    servo2.write(theta2);
    delay(1);
  }
}
/*Vérification de fonctionnalités*/
void check() {
  Serial.println("Coordonnées");
  Serial.println(Ex, 4);
  Serial.println(Ey, 4);
  Serial.println(theta1, 4);
  Serial.println(theta2, 4);
  Serial.println("Etats des boutons");
  Serial.println(digitalRead(button1));
  Serial.println(digitalRead(button2));
  Serial.println(digitalRead(button3));
  Serial.println(digitalRead(button4));
  Serial.println("Etat du joystick");
  Serial.println(digitalRead(SW_pin));
  Serial.println(analogRead(X_pin));
  Serial.println(analogRead(Y_pin));
  delay(1000);
}