#define DUREE_MVT_TRANSLATEUR_MS 1000
enum translateur_action_t{
  TRANSLATEUR_INIT,
  TRANSLATEUR_AVANCE,
  TRANSLATEUR_RECULE,
}translateur_action = TRANSLATEUR_INIT;

unsigned long temps_debut_action = 0;

void Translateur_init(void){
// Moteur
  /*ledcAttach(3, 500, 8);
  ledcWrite(3, 0);
  ledcAttach(4, 500, 8);
  ledcWrite(4, 0);*/
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  // Etat des contacteurs de fin de course
  // relecture de 3 sur 2 (Avant)
  // relecture de 4 sur 10 (Arrière)
  pinMode(2, INPUT);
  pinMode(10, INPUT);
}


enum etat_action_t Translateur_etat(void){
  if(temps_debut_action + DUREE_MVT_TRANSLATEUR_MS < millis()){
    if(translateur_action == TRANSLATEUR_AVANCE){
      if(digitalRead(2) == 0){
          return ACTION_TERMINEE;
      }
    }else  if(translateur_action == TRANSLATEUR_RECULE){
      if(digitalRead(10) == 0){
        return ACTION_TERMINEE;
      }
    }else{
        return ACTION_TERMINEE;
    }
  }
  return ACTION_EN_COURS;
}


void Translateur_cycle(void){
  if(Translateur_etat() == ACTION_TERMINEE){
    if(translateur_action == TRANSLATEUR_RECULE){
      Translateur_avance();
    }else{
      Translateur_recule();
    }
  }
}


void Translateur_avance(){
  Moteur_set(-255);
  temps_debut_action = millis();
  translateur_action = TRANSLATEUR_AVANCE;
}

void Translateur_recule(){
  Moteur_set(255);
  temps_debut_action = millis();
  translateur_action = TRANSLATEUR_RECULE;
}

/// @brief pilote la vitesse des moteurs
/// @param vitesse vitesse signée, sera saturée à 255
void Moteur_set(int vitesse){
  if(vitesse < 0){
    digitalWrite(3, 1);
    digitalWrite(4, 0);
  }else{
    digitalWrite(3, 0);
    digitalWrite(4, 1);
  }
  /*ledcWrite(3, 0);
  ledcWrite(4, 0);
  if(vitesse < 0){
    vitesse = -vitesse;
    if(vitesse > 255){
      vitesse = 255;
    }
    ledcWrite(3, vitesse);
  }else{
    if(vitesse > 255){
      vitesse = 255;
    }
    ledcWrite(4, vitesse);
  }*/
}

