//Définition des ports pour controler les moteurs
//Moteur gauche :
#define IN1a 27
#define IN2a 14
#define IN3a 12
#define IN4a 13
//Moteur droit :
#define IN1b 5
#define IN2b 18
#define IN3b 19
#define IN4b 21

#define BUTTON_PIN 0

//Temps d'attente entre deux impulsions :
int delayTime = 3;

//Code éxécuté au démarrage (paramétrage) :
void setup() {
  //On indique que les ports de controle des moteurs sont des sorties (output)
  pinMode(IN1a, OUTPUT);
  pinMode(IN2a, OUTPUT);
  pinMode(IN3a, OUTPUT);
  pinMode(IN4a, OUTPUT);

  pinMode(IN1b, OUTPUT);
  pinMode(IN2b, OUTPUT);
  pinMode(IN3b, OUTPUT);
  pinMode(IN4b, OUTPUT);

  pinMode(BUTTON_PIN, INPUT_PULLUP);

  Serial.begin(115200);  //Démarrage d'une communication Série avec l'ordinateur s'il est connecté
}

//Code éxécuté en boucle jusqu'à l'extinction du robot
void loop() {

  int buttonState = digitalRead(BUTTON_PIN);

  if (buttonState == LOW) {
    delay(1000);
    
    // Fonctions disponibles :
    // - forward(<distance en centimètre>);
    // - backward(<distance en centimètre>);
    // - left(<rotation en degré>);
    // - right(<rotation en degré>);

    // --- DEBUT DU CODE --- //

    

    // ---- FIN DU CODE ---- //
  }
}



//Fonction pour convertir une longueur en cm vers un nombre de pas moteur.
int convertLengthToSteps(float length) {
  float result = length * 512 / (4 * 3.1415);
  return int(result);
}

//fonction pour convertir une rotation en degrés vers un nombre de pas moteur
int convertRotToSteps(int rotation) {
  int result = convertLengthToSteps(rotation * 3.1415 / 180 * 7.8);
  return result;
}


void forward(int length) {
  for (int i = 0; i < convertLengthToSteps(length); i++) {
    digitalWrite(IN4a, HIGH);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, HIGH);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, HIGH);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, HIGH);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, HIGH);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, HIGH);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, HIGH);
    digitalWrite(IN4b, HIGH);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
  }
}

void backward(int length) {
  for (int i = 0; i < convertLengthToSteps(length); i++) {
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, HIGH);
    digitalWrite(IN4b, HIGH);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, HIGH);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, HIGH);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, HIGH);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, HIGH);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, HIGH);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, HIGH);
    delay(delayTime);
  }
}

void right(int deg) {
  for (int i = 0; i < convertRotToSteps(deg); i++) {
    digitalWrite(IN4a, HIGH);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, HIGH);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, HIGH);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, HIGH);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, HIGH);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, HIGH);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, HIGH);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, HIGH);
    delay(delayTime);
  }
}

void left(int deg) {
  for (int i = 0; i < convertRotToSteps(deg); i++) {
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, HIGH);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, HIGH);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, HIGH);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, HIGH);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, HIGH);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, HIGH);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, HIGH);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, HIGH);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
  }
}


void stopMotors(void) {
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);

  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
}