/*
 Controlling a servo position using a potentiometer (variable resistor)
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Knob
*/

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

Servo myservo1;
Servo myservo2;
Servo myservoZ;

int potx = A2;  //potentiomètre en x
int poty = A3;  //potentiometre en y
int servo1 = 6;
int servo2 = 5;
int servoZ = 3;
int bouton_mode = 2;   //bouton permettant le changement de mode
int boutonstylo = 9;  //bouton permettant de lever ou d'abaisser le stylo
int bouton_dessin = 7 ;
int bouton_lancerdessin = 8 ;
int ledmode = 12;
int led_dessin = 10;
int led_pointille = 11;
int calibrage;
float a1 = 6.5;  //LONGUEUR BRAS 1
float a2 = 8.7;  //LONGUEUR BRAS 2

//------------------------------------------------------------------------------------------------
void setup() {
  //initialisation des entrées et sorites
  pinMode(potx, INPUT);
  pinMode(poty, INPUT);
  pinMode(bouton_mode, INPUT);
  pinMode(boutonstylo, INPUT);
  pinMode(bouton_dessin, INPUT);
  pinMode(bouton_lancerdessin, INPUT);
  pinMode(ledmode, OUTPUT);
  pinMode(led_dessin, OUTPUT);
  pinMode(led_pointille, OUTPUT);
  Serial.begin(9600);
  myservo1.attach(servo1);
  myservo2.attach(servo2);
  myservoZ.attach(servoZ);
  calibrage = 0;
}
//------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------
/*Calcul du modèle géométrique*/
void Goto(float x, float y) {
  float h2 = pow(x, 2) + pow(y, 2.0);
  float theta2 = M_PI - acos((pow(a1, 2.0) + pow(a2, 2.0) - h2) / (2.0 * a1 * a2));
  float thetap = atan2(x, y);
  float alpha = acos((pow(a1, 2.0) + h2 - pow(a2, 2.0)) / (2.0 * a1 * sqrt(h2)));
  float theta1 = (thetap + alpha);
  float theta1deg = (theta1 / M_PI) * 180.0;
  float theta2deg = (theta2 / M_PI) * 180.0;
  myservo2.writeMicroseconds(map(theta2deg, 0, 180, 1000, 1600));
  myservo1.write((int)theta1deg);
  //myservo1.writeMicroseconds(map(theta1deg, 0, 180, 1000, 2000));
}
//------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------
void loop() {
  int mode = 1;
  lever_stylo(1);
  Goto(0,a1 +a2);
  while(calibrage == 0){
    if(digitalRead(bouton_mode)){
      calibrage = 1;
      delay(500);
    }
  }
  while (true) {
    if (mode) {
      allumer_led(1);
      digitalWrite(ledmode, HIGH);
      joystick();
      mode = 0;
      delay(500);
    } else if (!mode) {
      digitalWrite(ledmode, LOW);
      dessin();
      mode = 1;
      delay(500);
    }
  }
}
//------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------
/*Dessin d'un trait de 5cm*/
void trait() {
  float x;
  float y = 8.0;
  Goto(x, y);
  delay(500);
  lever_stylo(0);
  for (x = -2.5; x <= 2.50; x += 0.1) {
    Goto(x, y);
    delay(200);
  }
  lever_stylo(1);
}
//------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------
/*Fonction permettant de lever et baisser le stylo avec le servo moteur*/
void lever_stylo(int i) {
  if (i) {
    myservoZ.writeMicroseconds(1150);
  } else {
    myservoZ.writeMicroseconds(1220);
  }
}
//------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------
/*Dessin trait pointillé de 5cm*/
void trait_pointille() {
  float x = -2.5;
  float y = 8.0;
  int i = 0;
  int stylo = 0;
  Goto(x, y);
  delay(500);
  lever_stylo(0);
  for (x = -2.5; x <= 2.5; x += 0.1) {
    Goto(x, y);
    i++;
    if (i == 2) {
      if (stylo == 1) {
        stylo = 0;
      } else {
        stylo = 1;
      }
      lever_stylo(stylo);
      i = 0;
    }
    delay(200);
  }
  lever_stylo(1);
}
//------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------
/*Dessin d'un cercle que rayon 2.5cm*/
void cercle() {
  float r = 2.5;
  float x_centre = 0;
  float y_centre = 8.50;
  float xbegin = x_centre - r;
  float xend = x_centre + r;
  float x = xbegin;
  float y = y_centre + 1.2 * sqrt(pow(r, 2) - pow((xbegin - x_centre), 2));
  Goto(x, y);
  delay(800);
  lever_stylo(0);
  delay(400);
  for (x = xbegin; x <= xend; x += 0.1) {
    y = y_centre + 1.2 * sqrt(pow(r, 2) - pow((x - x_centre), 2));
    Goto(x, y);
    delay(100);
  }
  delay(100);
  for (x = xend; x >= xbegin; x -= 0.1) {
    y = y_centre - 1.2 * sqrt(pow(r, 2) - pow((x - x_centre), 2));
    Goto(x, y);
    delay(100);
  }
  lever_stylo(1);
}
//------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------
/*Dessin d'un cercle pointillé de rayon 2.5cm*/
void cercle_pointille() {
  float r = 2.5;
  float x_centre = 0;
  float y_centre = 8.50;
  float xbegin = x_centre - r;
  float xend = x_centre + r;
  float x = xbegin;
  float y = y_centre + 1.2 * sqrt(pow(r, 2) - pow((xbegin - x_centre), 2));
  int stylo = 0;
  int i = 0;
  Goto(x, y);
  delay(400);
  lever_stylo(0);
  delay(400);
  for (x = xbegin; x <= xend; x += 0.1) {
    y = y_centre + 1.2 * sqrt(pow(r, 2) - pow((x - x_centre), 2));
    Goto(x, y);
    i++;
    if (i == 5) {
      if (stylo == 1) {
        stylo = 0;
      } else {
        stylo = 1;
      }
      lever_stylo(stylo);
      i = 0;
    }
    delay(100);
  }
  delay(100);
  for (x = xend; x >= xbegin; x -= 0.1) {
    y = y_centre - 1.2 * sqrt(pow(r, 2) - pow((x - x_centre), 2));
    Goto(x, y);
    i++;
    if (i == 5) {
      if (stylo == 1) {
        stylo = 0;
      } else {
        stylo = 1;
      }
      lever_stylo(stylo);
      i = 0;
    }
    delay(100);
  }
  lever_stylo(1);
}
//------------------------------------------------------------------------------------------------
void allumer_led(int n){
  if(n == 1){
    digitalWrite(led_dessin, LOW);
    digitalWrite(led_pointille, LOW);
  }
  if(n == 2){
    digitalWrite(led_dessin, LOW);
    digitalWrite(led_pointille, HIGH);
  }
  if(n == 3){
    digitalWrite(led_dessin, HIGH);
    digitalWrite(led_pointille, LOW);
  }
  if(n == 4){
    digitalWrite(led_dessin, HIGH);
    digitalWrite(led_pointille, HIGH);
  }
}
//------------------------------------------------------------------------------------------------
/*Fontion de dessin*/
void dessin() {
  int dessin = 1;
  int dessiner = 0;
  allumer_led(dessin);
  while (true) {
    if(digitalRead(bouton_lancerdessin)){
      dessiner = 1;
    }
    if(digitalRead(bouton_dessin)){
      delay(500);
      if(dessin < 4){
        dessin++;
      }
      else{
        dessin = 1;
      }
      allumer_led(dessin);
    }
    if(dessiner){
      if(dessin == 1){
        trait();
      }
      if(dessin == 2){
        trait_pointille();
      }
      if(dessin == 3){
        cercle();
      }
      if(dessin == 4){
        cercle_pointille();
      }
      dessiner = 0;
    }
    if (digitalRead(bouton_mode) == 1) {
      break;
    }
  }
}
//------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------
/*Controle du dessin avec joystick*/
void joystick() {
  int valy;
  int valx;
  float x = 0;
  float y = 10;
  int stylo = 1;
  lever_stylo(1);
  Goto(x, y);
  while (true) {
    valx = analogRead(potx);
    valy = analogRead(poty);
    Serial.println(stylo);
    if (digitalRead(boutonstylo)) {
      if (stylo == 1) {
        stylo = 0;
      } else {
        stylo = 1;
      }
      lever_stylo(stylo);
      delay(50);
    }
    if (valx < 200 && x <= 5) {
      x += 0.1;
      Goto(x, y);
    }
    if (valx > 900 && x >= -5) {
      x -= 0.1;
      Goto(x, y);
    }
    if (valy > 900 && y >= 5) {
      y -= 0.1;
      Goto(x, y);
    }
    if (valy < 200 && y <= 12) {
      y += 0.1;
      Goto(x, y);
    }
    if (digitalRead(bouton_mode) == 1) {
      break;
    }
    delay(100);
  }
  lever_stylo(1);
}
//------------------------------------------------------------------------------------------------