/*******************************************
 Initialisation et fonctions de déplacement
*******************************************/

/**
 * @brief Applique une configuration aux moteurs.
 * @brief Chaque configuration est sur 1 octet.
 *
 * Exemple
 * ```
 * writeMotor(0b0010, 0b1100, 0b0000);
 * ```
 *
 * @param left Configuration du moteur droit (ex: `0b1100`).
 * @param right Configuration du moteur droit (ex: `0b0101`).
 * @param reader Configuration du moteur de lecture des gommettes (ex: `0b0101`).
 */
void writeMotor(uint8_t left, uint8_t right, uint8_t reader)
{
    digitalWrite(IN4a, left & 0b1000);
    digitalWrite(IN3a, left & 0b0100);
    digitalWrite(IN2a, left & 0b0010);
    digitalWrite(IN1a, left & 0b0001);
    digitalWrite(IN4b, right & 0b1000);
    digitalWrite(IN3b, right & 0b0100);
    digitalWrite(IN2b, right & 0b0010);
    digitalWrite(IN1b, right & 0b0001);
    digitalWrite(IN4r, reader & 0b1000);
    digitalWrite(IN3r, reader & 0b0100);
    digitalWrite(IN2r, reader & 0b0010);
    digitalWrite(IN1r, reader & 0b0001);
}

/**
 * @brief Commande pour faire avancer le robot.
 *
 */
void forward()
{
    writeMotor(0b1000, 0b1000, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0100, 0b0100, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0010, 0b0010, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0001, 0b0001, 0b0000);
    delay(DELAY_TIME);
}

/**
 * @brief Commande pour faire reculer le robot.
 *
 */
void backward()
{
    writeMotor(0b0001, 0b0001, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0010, 0b0010, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0100, 0b0100, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b1000, 0b1000, 0b0000);
    delay(DELAY_TIME);
}

/**
 * @brief Commande pour faire tourner le robot à droite.
 *
 */
void right()
{
    writeMotor(0b1000, 0b0001, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0100, 0b0010, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0010, 0b0100, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0001, 0b1000, 0b0000);
    delay(DELAY_TIME);
}

/**
 * @brief Commande pour faire tourner le robot à gauche.
 *
 */
void left()
{
    writeMotor(0b0001, 0b1000, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0010, 0b0100, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b0100, 0b0010, 0b0000);
    delay(DELAY_TIME);
    writeMotor(0b1000, 0b0001, 0b0000);
    delay(DELAY_TIME);
}

/**
 * @brief Commande pour faire avancer la page de gommettes.
 *
 */
void forwardReader()
{
    writeMotor(0b0000, 0b0000, 0b0001);
    delay(DELAY_TIME);
    writeMotor(0b0000, 0b0000, 0b0010);
    delay(DELAY_TIME);
    writeMotor(0b0000, 0b0000, 0b0100);
    delay(DELAY_TIME);
    writeMotor(0b0000, 0b0000, 0b1000);
    delay(DELAY_TIME);
}

/**
 * @brief Comande pour activer la pompe à eau
 */
void pump()
{
    digitalWrite(IN1p, HIGH);
    delay(DELAY_TIME);
    Serial.println("pompe activée !");
}


/**
 * @brief Commande pour arrêter les  moteurs du robot.
 *
 */
void stopMotors()
{
    writeMotor(0b0000, 0b0000, 0b0000);
    digitalWrite(IN1p, LOW);
}

