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

void deplacement(int coordX, int coordY);

int axey = A1;
int axex = A0;
int valeurX;
int valeurY;
int BoutonJoystick;

Servo servo1;
Servo servo2;
Servo miniservo;

float L = 3.0; // longueur du bras
float plageMvt = 10.0;
void setup()
{
  // put your setup code here, to run once:
  servo1.attach(9);
  servo2.attach(10);
  miniservo.attach(11);

  pinMode(2, INPUT_PULLUP);
  Serial.begin(9600);
}

void loop()
{
  // lecture joysitck
  BoutonJoystick = digitalRead(2);
  valeurX = analogRead(axex);
  valeurY = analogRead(axey);

  // etat bouton
  if (BoutonJoystick == LOW)
  {
    miniservo.write(180);
    delay(800);
  }
  else
  {
    miniservo.write(0);
    delay(50);
  }

  // affichage paramètre servo

  Serial.print("x = ");
  Serial.print(valeurX);
  Serial.print("   Y = ");
  Serial.print(valeurY);
  // Serial.print("  bouton = ");
  // Serial.print(BoutonJoystick);

  deplacement(valeurX, valeurY);
}

void deplacement(int coordX, int coordY)
{
  // Calcul de la longueur du bras
  coordY = map(valeurY, 0, 1023, 0, plageMvt);
  coordX = map(valeurX, 0, 1023, 0, plageMvt);
  float l = sqrt(coordX * coordX + coordY * coordY);

  // Calcul des angles en fonction des coordonnées X et Y
  float alpha1 = degrees((coordX, coordY));
  float alpha3 = degrees(((2 * L * L - l * l) / (2 * L * L)));
  float alpha2 = 90 - alpha3 / 2;

  float theta1 = alpha2 - alpha1;
  float theta2 = alpha2 + alpha1;

  // Écriture des angles pour déplacer le bras
  /*
  servo1.write(theta1);
  servo2.write(theta2);
  delay(10);
  */

  Serial.print(" theta1 = ");
  Serial.print(theta1);
  Serial.print("   theta2 = ");
  Serial.print(theta2);
  Serial.print("   l = ");
  Serial.println(l);
}