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

// 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 = D6; // Bouton du joystick

// 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;

//115 position de base servo3 
// 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.03;

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

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

  //Position initiale du robot
  // servo1.write(45);
  // servo2.write(45);
  // delay(1000);

  //Bouger le robot à une position (xp,yp) désirée POSITION DE DEPART LIGNE
    // double x_p = -30.0;
    // double y_p = 100.0;

  // 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 de radians en degrés
  // int angleServo1 = int(theta_1 * 180 / M_PI);
  // int angleServo2 = 180 - int(theta_2 * 180 / M_PI);

  // Serial.print("Theta_1 = ");
  // Serial.println(theta_1 * 180 / M_PI);
  // Serial.print("Theta_2 = ");
  // Serial.println(theta_2 * 180 / M_PI);

  // servo1.write(angleServo1);
  // servo2.write(angleServo2);
  //servo3.write(130);

  // 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
    // move_in_line(x_p, y_p, line_length, steps, l_1, l_2, l_3, l_4, l_5);
  //  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;

    // 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);

    // Serial.print("Theta_1 = ");
    // Serial.println(theta_1 * 180 / M_PI);
    // Serial.print("Theta_2 = ");
    // Serial.println(theta_2 * 180 / M_PI);

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

    delay(100);
  
}


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) {
  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(115); // Abaisser le stylo
    } else {
      servo3.write(130); // 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(115);  // Stylo en bas
    } else {
      servo3.write(130); // 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(115);  // Stylo en bas
    } else {
      servo3.write(130); // 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);
}