#include <Servo.h>
#include <Arduino.h>
#include <math.h>

#define rouge 3
#define vert 5
#define bleu 6

// Déclaration des objets servo pour chaque servo
Servo servo1;
Servo servo2;
Servo servo3;

// Broches du joystick
const int VrxPin = A4;
const int VryPin = A5;
const int pinJoystickButton = 7;  // Bouton du joystick
//Bouton choix mode
const int buttonPin = 2;
int etat_bouton = 0, val_bouton = 0;

// Bornes de lecture du joystick
const int joystickMin = 0;
const int joystickMax = 1023;

// Bornes de mouvement du robot
const double minX = -100;  // Valeur minimale de coordonnée X
const double maxX = 50;    // Valeur maximale de coordonnée X
const double minY = 70;    // Valeur minimale de coordonnée Y
const double maxY = 130;   // Valeur maximale de coordonnée Y

// Tolérance pour détecter le recentrage du joystick
const int joystickCenterTolerance = 25;  // Ajuster cette valeur

// État du bouton du joystick
int lastButtonState = HIGH;
int currentButtonState;

//  Définir les paramètres géométriques
double l_1 = 50.0;
double l_2 = 50.0;
double l_3 = 50.0;
double l_4 = 50.0;
double l_5 = 57.0;

// Variables pour stocker les positions actuelles
double current_x_p = (minX + maxX) / 2;
double current_y_p = (minY + maxY) / 2;

// Facteur de lissage pour les mouvements
const double smoothingFactor = 0.02;

double line_length = 52.0;  // Longueur de la ligne en mm (mettre 60.0mm pour avoir 5 cm en réalité)
int steps = 50;
double x_p = -30.0;
double y_p = 100.0;

void setup() {
  delay(2000);
  Serial.begin(9600);

  pinMode(buttonPin, INPUT_PULLUP);
  // Attachement des servomoteurs aux broches
  servo1.attach(9);
  servo2.attach(10);
  servo3.attach(11);

  //led de validation de choix
  pinMode(rouge, OUTPUT);
  pinMode(vert, OUTPUT);
  pinMode(bleu, OUTPUT);

  // Position initiale du robot
  servo3.write(130);
  servo1.write(45);
  servo2.write(45);
  delay(500);

  // Tracer une ligne droite
  // double line_length = 52.0; // Longueur de la ligne en mm (mettre 60.0mm pour avoir 5 cm en réalité)
  // int steps = 50;            // Nombre de pas pour tracer la ligne
  // servo3.write(115);
  // move_in_line(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);
  // servo3.write(130);
  //move_in_line_disc(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);

  // Tracer un cercle
  // double center_x = -55.0;
  // double center_y = 100.0;
  // double radius = 28.0;  // Rayon du cercle en mm (2.5 cm) (radius = 25.0mm donne 2cm en réalité, produit en croix pour avoir 2.5cm en réalité)
  // int steps = 50;  // Nombre de pas pour tracer le cercle
  //  move_in_circle_half1(center_x, center_y, radius, steps, l_1, l_2, l_3, l_4, l_5);
  //  move_in_circle_half2(center_x, center_y, radius, steps, l_1, l_2, l_3, l_4, l_5);
  // move_in_circle_disc(center_x, center_y, radius, steps, l_1, l_2, l_3, l_4, l_5);
}

void loop() {
  // Lire les valeurs du joystick
  int vrxValue = analogRead(VrxPin);
  int vryValue = analogRead(VryPin);
  
  // Inverser la valeur du joystick pour l'axe Y
  vryValue = joystickMax - vryValue;

  if (digitalRead(buttonPin) == HIGH && etat_bouton == 0) {
    etat_bouton = 1;
    setColor(250, 250, 250);  // LED en blanc pour indiquer la sélection de mode

    do {
      vrxValue = analogRead(VrxPin);
      vryValue = analogRead(VryPin);

      if (vryValue > 512 + 200) {
        val_bouton = 1;  // Mode 1 (joystick en haut)
      } else if (vryValue < 512 - 200) {
        val_bouton = 2;  // Mode 2 (joystick en bas)
      } else if (vrxValue > 512 + 200) {
        val_bouton = 3;  // Mode 3 (joystick à droite)
      } else if (vrxValue < 512 - 200) {
        val_bouton = 4;  // Mode 4 (joystick à gauche)
      }

      switch (val_bouton) {
        case 1:
          setColor(250, 250, 0);  // LED en jaune

          break;
        case 2:
          setColor(125, 125, 125);  // LED en gris
          break;
        case 3:
          setColor(0, 250, 0);  // LED en vert
          break;
        case 4:
          setColor(250, 0, 250);  // LED en magenta
          break;
      }

      delay(10);

      if (digitalRead(buttonPin) == HIGH && val_bouton != 5) {
        etat_bouton = 2;
      }
    } while (etat_bouton != 2);

    switch (val_bouton) {
      case 1:  // Mode 1
        x_p = -30.0;
        y_p = 100.0;
        line_length = 52.0;  // Longueur de la ligne en mm (mettre 60.0mm pour avoir 5 cm en réalité)
        steps = 50;          // Nombre de pas pour tracer la ligne
        servo3.write(90);
        move_in_line(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);
        servo3.write(100);
        break;
      case 2:  // Mode 2
        x_p = -30.0;
        y_p = 100.0;
        line_length = 52.0;  // Longueur de la ligne en mm (mettre 60.0mm pour avoir 5 cm en réalité)
        steps = 50;          // Nombre de pas pour tracer la ligne
        servo3.write(90);
        move_in_line_disc(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);
        servo3.write(100);
        break;
      case 3:  // Mode 3
        servo3.write(90);
        move_in_circle_half1_disc(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);
        move_in_circle_half2_disc(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);
        servo3.write(100);
        break;
      case 4:  // Mode 4
        servo3.write(90);
        move_in_circle_half1(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);
        move_in_circle_half2(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);
        servo3.write(100);
        break;
    }

    
  }
  // Mode de déplacement classique

    // Vérifier si le joystick est centré (dans la tolérance)
    bool joystickCentered = (abs(vrxValue - (joystickMax / 2)) < joystickCenterTolerance) && (abs(vryValue - (joystickMax / 2)) < joystickCenterTolerance);

    if (!joystickCentered) {
      // Mapper les valeurs du joystick aux coordonnées (xp, yp)
      double target_x_p = map(vrxValue, joystickMin, joystickMax, minX, maxX);
      double target_y_p = map(vryValue, joystickMin, joystickMax, minY, maxY);

      // Lissage des mouvements
      current_x_p += (target_x_p - current_x_p) * smoothingFactor;
      current_y_p += (target_y_p - current_y_p) * smoothingFactor;
    }

    double theta_1 = calculate_theta_1(current_x_p, current_y_p, l_1, l_4, l_5);
    double theta_2 = calculate_theta_2(current_x_p, current_y_p, theta_1, l_1, l_2, l_3, l_4, l_5);

    // Convertir les angles de radians en degrés
    int angleServo1 = int(theta_1 * 180 / M_PI);
    int angleServo2 = 180 - int(theta_2 * 180 / M_PI);

    servo1.write(angleServo1);
    servo2.write(angleServo2);

    setColor(0, 0, 0);  // Éteindre la LED

    // Réinitialisation de l'état et de la valeur du bouton
    etat_bouton = 0;
    val_bouton = 5;
    delay(50);
}



double calculate_theta_1(double x_p, double y_p, double l_1, double l_4, double l_5) {
  double O1P = sqrt(x_p * x_p + y_p * y_p);
  double alpha = acos(constrain((l_1 * l_1 - (l_4 + l_5) * (l_4 + l_5) + O1P * O1P) / (2 * l_1 * O1P), -1.0, 1.0));  // S'assurer que acos reste dans le bon domaine
  double beta = atan2(y_p, -x_p);
  double theta_1 = M_PI - (alpha + beta);
  return theta_1;
}

double calculate_theta_2(double x_p, double y_p, double theta_1, double l_1, double l_2, double l_3, double l_4, double l_5) {
  double xc = (l_4 / (l_4 + l_5)) * x_p + (l_5 / (l_4 + l_5)) * l_1 * cos(theta_1);
  double yc = (l_4 / (l_4 + l_5)) * y_p + (l_5 / (l_4 + l_5)) * l_1 * sin(theta_1);
  double O2C = sqrt(xc * xc + yc * yc);
  double delta = acos(constrain((l_2 * l_2 - l_3 * l_3 + O2C * O2C) / (2 * l_2 * O2C), -1.0, 1.0));  // S'assurer que acos reste dans le bon domaine
  double beta = atan2(y_p, -x_p);
  double gamma = atan2(yc, xc);
  double theta_2 = delta + gamma;
  return theta_2;
}

// Tracer une ligne droite horizontale dans le sens des x négatifs
void move_in_line(double x_start, double y_start, double length, int steps, double l_1, double l_2, double l_3, double l_4, double l_5) {
  double step_size = length / steps;
  // double time_per_step = 10000.0 / steps; // 10 secondes (10000 ms) divisé par le nombre de steps
  unsigned long total_time = 9800;  // Temps total en millisecondes (10 secondes)
  unsigned long time_per_step = total_time / steps;
  unsigned long start_time = millis();  // Start time

  for (int i = 0; i <= steps; i++) {
    double x_p = x_start - i * step_size;  // Tracer la ligne dans le sens des x négatifs
    double y_p = y_start;

    double theta_1 = calculate_theta_1(x_p, y_p, l_1, l_4, l_5);
    double theta_2 = calculate_theta_2(x_p, y_p, theta_1, l_1, l_2, l_3, l_4, l_5);

    // Convertir les angles en degrés
    double theta_1_deg = theta_1 * 180 / M_PI;
    double theta_2_deg = theta_2 * 180 / M_PI;

    // Déplacer les servos aux angles calculés
    servo1.write(theta_1_deg);
    servo2.write(180 - theta_2_deg);

    // Afficher les angles actuels dans le moniteur série
    Serial.print("Step ");
    Serial.print(i);
    Serial.print(": Theta_1 = ");
    Serial.print(theta_1_deg);
    Serial.print(", Theta_2 = ");
    Serial.println(theta_2_deg);

    // delay(time_per_step);  // Délai pour chaque étape

    // Attendre jusqu'au temps approprié pour le prochain pas
    unsigned long current_time = millis();
    unsigned long next_step_time = start_time + (i + 1) * time_per_step;
    while (current_time < next_step_time) {  // Boucle while remplace le delay
      current_time = millis();
    }
  }
  // Vérification finale du temps écoulé
  unsigned long actual_end_time = millis();
  Serial.print("Temps total: ");
  Serial.println(actual_end_time - start_time);
}

// Tracer une ligne droite discontinue
void move_in_line_disc(double x_start, double y_start, double length, int steps, double l_1, double l_2, double l_3, double l_4, double l_5) {
  servo3.write(115);
  double step_size = length / steps;
  unsigned long total_time = 9800;  // Temps total en millisecondes (10 secondes)
  unsigned long time_per_step = total_time / steps;
  unsigned long start_time = millis();  // Start time

  for (int i = 0; i <= steps; i++) {
    double x_p = x_start - i * step_size;  // Tracer la ligne dans le sens des x négatifs
    double y_p = y_start;

    double theta_1 = calculate_theta_1(x_p, y_p, l_1, l_4, l_5);
    double theta_2 = calculate_theta_2(x_p, y_p, theta_1, l_1, l_2, l_3, l_4, l_5);

    // Convertir les angles en degrés
    double theta_1_deg = theta_1 * 180 / M_PI;
    double theta_2_deg = theta_2 * 180 / M_PI;

    // Déplacer les servos aux angles calculés
    servo1.write(theta_1_deg);
    servo2.write(180 - theta_2_deg);

    // Lever et abaisser le stylo pour créer une ligne discontinue
    if (i % 10 < 5) {
      servo3.write(90);  // Abaisser le stylo
    } else {
      servo3.write(100);  // Lever le stylo
    }

    // Afficher les angles actuels dans le moniteur série
    Serial.print("Step ");
    Serial.print(i);
    Serial.print(": Theta_1 = ");
    Serial.print(theta_1_deg);
    Serial.print(", Theta_2 = ");
    Serial.println(theta_2_deg);

    // Attendre jusqu'au temps approprié pour le prochain pas
    unsigned long current_time = millis();
    unsigned long next_step_time = start_time + (i + 1) * time_per_step;
    while (current_time < next_step_time) {  // Boucle while remplace le delay
      current_time = millis();
    }
  }

  // Vérification finale du temps écoulé
  unsigned long actual_end_time = millis();
  Serial.print("Temps total: ");
  Serial.println(actual_end_time - start_time);
}

// Utiliser les équations paramétriques d'un cercle pour calculer les positions des points autour du cercle
void move_in_circle_half1(double center_x, double center_y, double radius, int steps, double l_1, double l_2, double l_3, double l_4, double l_5) {
  unsigned long total_time = 4800;  // Total time in milliseconds (10 seconds)
  unsigned long time_per_step = total_time / steps;
  unsigned long start_time = millis();  // Start time

  for (int i = 0; i <= steps; i++) {
    // Calcul des coordonnées x et y pour le cercle
    double angle = 1 * M_PI * i / steps;  // Angle en radians
    double x_p = center_x + radius * cos(angle);
    double y_p = center_y + radius * sin(angle);

    double theta_1 = calculate_theta_1(x_p, y_p, l_1, l_4, l_5);
    double theta_2 = calculate_theta_2(x_p, y_p, theta_1, l_1, l_2, l_3, l_4, l_5);

    // Convertir les angles en degrés
    double theta_1_deg = theta_1 * 180 / M_PI;
    double theta_2_deg = theta_2 * 180 / M_PI;

    // Déplacer les servos aux angles calculés
    servo1.write(theta_1_deg);
    servo2.write(180 - theta_2_deg);

    // Afficher les angles actuels dans le moniteur série
    Serial.print("Step ");
    Serial.print(i);
    Serial.print(": Theta_1 = ");
    Serial.print(theta_1_deg);
    Serial.print(", Theta_2 = ");
    Serial.println(theta_2_deg);

    // Attendre jusqu'au temps approprié pour le prochain pas
    unsigned long current_time = millis();
    unsigned long next_step_time = start_time + (i + 1) * time_per_step;
    while (current_time < next_step_time) {
      current_time = millis();
    }
    // delay(time_per_step);
  }

  // // Position finale
  // double x_f = center_x + radius * cos(M_PI / 6);
  // double y_f = center_y + radius * sin(M_PI / 6);

  // double theta_1_f = calculate_theta_1(x_f, y_f, l_1, l_4, l_5);
  // double theta_2_f = calculate_theta_2(x_f, y_f, theta_1_f, l_1, l_2, l_3, l_4, l_5);

  // // Convertir les angles en degrés
  // double theta_1_deg_f = theta_1_f * 180 / M_PI;
  // double theta_2_deg_f = theta_2_f * 180 / M_PI;

  // // Déplacer les servos aux angles calculés
  // servo1.write(theta_1_deg_f);
  // servo2.write(180 - theta_2_deg_f);

  // Vérification finale du temps écoulé
  unsigned long actual_end_time = millis();
  Serial.print("Temps total: ");
  Serial.println(actual_end_time - start_time);
}

// Utiliser les équations paramétriques d'un cercle pour calculer les positions des points autour du cercle
void move_in_circle_half2(double center_x, double center_y, double radius, int steps, double l_1, double l_2, double l_3, double l_4, double l_5) {
  unsigned long total_time = 4800;  // Total time in milliseconds (10 seconds)
  unsigned long time_per_step = total_time / steps;
  unsigned long start_time = millis();  // Start time

  for (int i = 0; i <= steps; i++) {
    // Calcul des coordonnées x et y pour le cercle
    double angle = 1 * M_PI * i / steps;  // Angle en radians
    double x_p = center_x - radius * cos(angle);
    double y_p = center_y - radius * sin(angle);

    double theta_1 = calculate_theta_1(x_p, y_p, l_1, l_4, l_5);
    double theta_2 = calculate_theta_2(x_p, y_p, theta_1, l_1, l_2, l_3, l_4, l_5);

    // Convertir les angles en degrés
    double theta_1_deg = theta_1 * 180 / M_PI;
    double theta_2_deg = theta_2 * 180 / M_PI;

    // Déplacer les servos aux angles calculés
    servo1.write(theta_1_deg);
    servo2.write(180 - theta_2_deg);

    // Afficher les angles actuels dans le moniteur série
    Serial.print("Step ");
    Serial.print(i);
    Serial.print(": Theta_1 = ");
    Serial.print(theta_1_deg);
    Serial.print(", Theta_2 = ");
    Serial.println(theta_2_deg);

    // Attendre jusqu'au temps approprié pour le prochain pas
    unsigned long current_time = millis();
    unsigned long next_step_time = start_time + (i + 1) * time_per_step;
    while (current_time < next_step_time) {
      current_time = millis();
    }
    // delay(time_per_step);
  }

  // // Position finale
  double x_f = center_x - radius * cos(M_PI / 6);
  double y_f = center_y - radius * sin(M_PI / 6);

  double theta_1_f = calculate_theta_1(x_f, y_f, l_1, l_4, l_5);
  double theta_2_f = calculate_theta_2(x_f, y_f, theta_1_f, l_1, l_2, l_3, l_4, l_5);

  // Convertir les angles en degrés
  double theta_1_deg_f = theta_1_f * 180 / M_PI;
  double theta_2_deg_f = theta_2_f * 180 / M_PI;

  // Déplacer les servos aux angles calculés
  servo1.write(theta_1_deg_f);
  servo2.write(180 - theta_2_deg_f);

  // Vérification finale du temps écoulé
  unsigned long actual_end_time = millis();
  Serial.print("Temps total: ");
  Serial.println(2 * (actual_end_time - start_time));
}

void move_in_circle_half1_disc(double center_x, double center_y, double radius, int steps, double l_1, double l_2, double l_3, double l_4, double l_5) {
  unsigned long total_time = 4800;
  unsigned long time_per_step = total_time / steps;
  unsigned long start_time = millis();

  for (int i = 0; i <= steps; i++) {
    double angle = 1 * M_PI * i / steps;
    double x_p = center_x + radius * cos(angle);
    double y_p = center_y + radius * sin(angle);

    double theta_1 = calculate_theta_1(x_p, y_p, l_1, l_4, l_5);
    double theta_2 = calculate_theta_2(x_p, y_p, theta_1, l_1, l_2, l_3, l_4, l_5);

    double theta_1_deg = theta_1 * 180 / M_PI;
    double theta_2_deg = theta_2 * 180 / M_PI;

    servo1.write(theta_1_deg);
    servo2.write(180 - theta_2_deg);

    if (i % 10 < 5) {
      servo3.write(90);  // Stylo en bas
    } else {
      servo3.write(100);  // Stylo en haut
    }

    Serial.print("Step ");
    Serial.print(i);
    Serial.print(": Theta_1 = ");
    Serial.print(theta_1_deg);
    Serial.print(", Theta_2 = ");
    Serial.println(theta_2_deg);

    unsigned long current_time = millis();
    unsigned long next_step_time = start_time + (i + 1) * time_per_step;
    while (current_time < next_step_time) {
      current_time = millis();
    }
  }

  unsigned long actual_end_time = millis();
  Serial.print("Temps total: ");
  Serial.println(actual_end_time - start_time);
}

void move_in_circle_half2_disc(double center_x, double center_y, double radius, int steps, double l_1, double l_2, double l_3, double l_4, double l_5) {
  unsigned long total_time = 4800;
  unsigned long time_per_step = total_time / steps;
  unsigned long start_time = millis();

  for (int i = 0; i <= steps; i++) {
    double angle = M_PI + M_PI * i / steps;
    double x_p = center_x + radius * cos(angle);
    double y_p = center_y + radius * sin(angle);

    double theta_1 = calculate_theta_1(x_p, y_p, l_1, l_4, l_5);
    double theta_2 = calculate_theta_2(x_p, y_p, theta_1, l_1, l_2, l_3, l_4, l_5);

    double theta_1_deg = theta_1 * 180 / M_PI;
    double theta_2_deg = theta_2 * 180 / M_PI;

    servo1.write(theta_1_deg);
    servo2.write(180 - theta_2_deg);

    if (i % 10 < 5) {
      servo3.write(90);  // Stylo en bas
    } else {
      servo3.write(100);  // Stylo en haut
    }

    Serial.print("Step ");
    Serial.print(i);
    Serial.print(": Theta_1 = ");
    Serial.print(theta_1_deg);
    Serial.print(", Theta_2 = ");
    Serial.println(theta_2_deg);

    unsigned long current_time = millis();
    unsigned long next_step_time = start_time + (i + 1) * time_per_step;
    while (current_time < next_step_time) {
      current_time = millis();
    }
  }

  unsigned long actual_end_time = millis();
  Serial.print("Temps total: ");
  Serial.println(actual_end_time - start_time);
}

void move_in_circle_disc(double center_x, double center_y, double radius, int steps, double l_1, double l_2, double l_3, double l_4, double l_5) {
  move_in_circle_half1_disc(center_x, center_y, radius, steps / 2, l_1, l_2, l_3, l_4, l_5);
  move_in_circle_half2_disc(center_x, center_y, radius, steps / 2, l_1, l_2, l_3, l_4, l_5);
}

void setColor(int redValue, int greenValue, int blueValue) {
  // Régler l'intensité lumineuse de chaque couleur
  analogWrite(rouge, redValue);
  analogWrite(vert, greenValue);
  analogWrite(bleu, blueValue);
}