#include <stdio.h>
#include <string.h>
#include "driver/uart.h"
#include "driver/gpio.h"
#include "esp_system.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/adc.h"
#include "esp_adc/adc_oneshot.h"
#include "esp_crc.h"
#include "esp_log.h"
#include "driver/ledc.h"

// Paramètres du port série
#define UART_NUM       UART_NUM_0
#define TXD_PIN        GPIO_NUM_25
#define RXD_PIN        GPIO_NUM_26
#define BUF_SIZE       1024
#define BAUD_RATE      115200

// Définition de la broche du bouton
#define LED_COMM_PIN     12

// Définition PWM
#define PWM_FREQ_HZ      5000 // Fréquence du PWM (5 kHz)
#define PWM_RESOLUTION   LEDC_TIMER_13_BIT // Résolution sur 13 bits (0-8191)

// Définition des broches moteurs direction
#define MOTOR1_IN1       GPIO_NUM_32
#define MOTOR1_IN2       GPIO_NUM_33
#define MOTOR2_IN1       GPIO_NUM_25
#define MOTOR2_IN2       GPIO_NUM_26

// Canaux PWM moteurs direction
#define MOTOR1_IN1_PWM_CHANNEL   LEDC_CHANNEL_0
#define MOTOR1_IN2_PWM_CHANNEL   LEDC_CHANNEL_1
#define MOTOR2_IN1_PWM_CHANNEL   LEDC_CHANNEL_2
#define MOTOR2_IN2_PWM_CHANNEL   LEDC_CHANNEL_3

// Définition des capteurs position chariot
#define SENSOR_N0 23
#define SENSOR_N1 22
#define SENSOR_N2 21
#define SENSOR_N3 19

// Définition des boutons target chariot sur robot
#define BTN_N0 5
#define BTN_N1 18
#define BTN_N2 27
#define BTN_N3 14

// Définition des actionneurs
#define INCR_POINT 17

// Définition des commandes moteur chariot
#define MOTOR_UP GPIO_NUM_10
#define MOTOR_DOWN GPIO_NUM_9

// Définition des commandes pousse planches
#define POUSSE_PLANCHE_ON 16
#define POUSSE_PLANCHE_OFF 17

// Définition des commandes ouverture affiche
#define OUVERTURE_AFFICHE_ON 0
#define OUVERTURE_AFFICHE_OFF 4

// Définition des commandes PINCES EXT
#define PINCE_EXT_ON 15
#define PINCE_EXT_OFF 2

// Définition des commandes PINCES INT et BLOQUE PLANCHE
#define PINCE_INT_BP_ON GPIO_NUM_7
#define PINCE_INT_BP_OFF GPIO_NUM_8

void init_pwm() {
    // Configuration du timer
    ledc_timer_config_t ledc_timer = {
        .duty_resolution = PWM_RESOLUTION,
        .freq_hz = PWM_FREQ_HZ,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .timer_num = LEDC_TIMER_0
    };
    ledc_timer_config(&ledc_timer);

    // Configuration des canaux PWM pour moteur 1 (IN1 et IN2)
    ledc_channel_config_t motor1_in1_pwm = {
        .channel    = MOTOR1_IN1_PWM_CHANNEL,
        .duty       = 0,
        .gpio_num   = MOTOR1_IN1,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .hpoint     = 0,
        .timer_sel  = LEDC_TIMER_0
    };
    ledc_channel_config(&motor1_in1_pwm);

    ledc_channel_config_t motor1_in2_pwm = {
        .channel    = MOTOR1_IN2_PWM_CHANNEL,
        .duty       = 0,
        .gpio_num   = MOTOR1_IN2,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .hpoint     = 0,
        .timer_sel  = LEDC_TIMER_0
    };
    ledc_channel_config(&motor1_in2_pwm);

    // Configuration des canaux PWM pour moteur 2 (IN1 et IN2)
    ledc_channel_config_t motor2_in1_pwm = {
        .channel    = MOTOR2_IN1_PWM_CHANNEL,
        .duty       = 0,
        .gpio_num   = MOTOR2_IN1,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .hpoint     = 0,
        .timer_sel  = LEDC_TIMER_0
    };
    ledc_channel_config(&motor2_in1_pwm);

    ledc_channel_config_t motor2_in2_pwm = {
        .channel    = MOTOR2_IN2_PWM_CHANNEL,
        .duty       = 0,
        .gpio_num   = MOTOR2_IN2,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .hpoint     = 0,
        .timer_sel  = LEDC_TIMER_0
    };
    ledc_channel_config(&motor2_in2_pwm);
}

// Initialiser l'UART
void init_uart() {
    // Configuration du port série UART0
    uart_config_t uart_config = {
        .baud_rate = BAUD_RATE,
        .data_bits = UART_DATA_8_BITS,           // 8 bits de données
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE
    };
    uart_param_config(UART_NUM, &uart_config);
    uart_set_pin(UART_NUM, TXD_PIN, RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
    uart_driver_install(UART_NUM, BUF_SIZE, 0, 0, NULL, 0);
}

// Initialiser les leds et entrées boutons
void init_gpio() {
    gpio_set_direction(LED_COMM_PIN, GPIO_MODE_OUTPUT);
    gpio_set_level(LED_COMM_PIN, 0);  // LED éteinte au démarrage

    // Configuration du GPIO pour le bouton avec résistance pull-up interne
    gpio_set_direction(SENSOR_N0, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(SENSOR_N1, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(SENSOR_N2, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(SENSOR_N3, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_pullup_en(SENSOR_N0);                       // Activer la résistance pull-up
    gpio_pullup_en(SENSOR_N1);                       // Activer la résistance pull-up
    gpio_pullup_en(SENSOR_N2);                       // Activer la résistance pull-up
    gpio_pullup_en(SENSOR_N3);                       // Activer la résistance pull-up

    // Configuration du GPIO pour le bouton avec résistance pull-up interne
    gpio_set_direction(BTN_N0, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(BTN_N1, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(BTN_N2, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(BTN_N3, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_pullup_en(BTN_N0);                       // Activer la résistance pull-up
    gpio_pullup_en(BTN_N1);                       // Activer la résistance pull-up
    gpio_pullup_en(BTN_N2);                       // Activer la résistance pull-up
    gpio_pullup_en(BTN_N3);                       // Activer la résistance pull-up

    // Configuration actionneurs
    gpio_set_direction(INCR_POINT, GPIO_MODE_OUTPUT);

    // Configurer les moteurs en sortie
    gpio_set_direction(MOTOR_UP, GPIO_MODE_OUTPUT);
    gpio_set_direction(MOTOR_DOWN, GPIO_MODE_OUTPUT);
    gpio_set_direction(POUSSE_PLANCHE_ON, GPIO_MODE_OUTPUT);
    gpio_set_direction(POUSSE_PLANCHE_OFF, GPIO_MODE_OUTPUT);
    gpio_set_direction(OUVERTURE_AFFICHE_ON, GPIO_MODE_OUTPUT);
    gpio_set_direction(OUVERTURE_AFFICHE_OFF, GPIO_MODE_OUTPUT);
    gpio_set_direction(PINCE_EXT_ON, GPIO_MODE_OUTPUT);
    gpio_set_direction(PINCE_EXT_OFF, GPIO_MODE_OUTPUT);
    gpio_set_direction(PINCE_INT_BP_ON, GPIO_MODE_OUTPUT);
    gpio_set_direction(PINCE_INT_BP_OFF, GPIO_MODE_OUTPUT);

}

// Fonction pour calculer le CRC32 et ajouter le CRC à la trame
size_t create_frame_with_crc(uint8_t *frame_out) {
    // Lire l'état des boutons (0 si appuyé, 1 si relâché)
    int sensorN0_state = gpio_get_level(SENSOR_N0);
    int sensorN1_state = gpio_get_level(SENSOR_N1);
    int sensorN2_state = gpio_get_level(SENSOR_N2);
    int sensorN3_state = gpio_get_level(SENSOR_N3);

    // Création de la trame à envoyer
    char message[128];
    int message_length = snprintf(message, sizeof(message),
                          "\nR%s%s%s%s",
                          sensorN0_state == 0 ? "1" : "0",
                          sensorN1_state == 0 ? "1" : "0",
                          sensorN2_state == 0 ? "1" : "0",
                          sensorN3_state == 0 ? "1" : "0");

    // Calculer le CRC32
    memcpy(frame_out, message, message_length);
    uint32_t crc = esp_crc32_le(0, (const uint8_t *)message, message_length);

    // Ajouter le CRC à la fin de la trame (little-endian)
    frame_out[message_length] = (uint8_t)(crc & 0xFF);
    frame_out[message_length + 1] = (uint8_t)((crc >> 8) & 0xFF);
    frame_out[message_length + 2] = (uint8_t)((crc >> 16) & 0xFF);
    frame_out[message_length + 3] = (uint8_t)((crc >> 24) & 0xFF);

    // Retourner la longueur totale de la trame (données + CRC)
    return message_length + 4;
}

// Fonction pour vérifier le CRC32 d'une trame
bool verify_crc32(const uint8_t *frame, size_t length) {
    if (length < 4) {
        // La longueur doit être au moins 4 octets (pour contenir un CRC32)
        return false;
    }

    // Extraire le CRC32 reçu (les 4 derniers octets de la trame)
    uint32_t received_crc = (uint32_t)frame[length - 4] |
                            ((uint32_t)frame[length - 3] << 8) |
                            ((uint32_t)frame[length - 2] << 16) |
                            ((uint32_t)frame[length - 1] << 24);

    // Calculer le CRC32 des données, sans inclure les 4 derniers octets
    uint32_t calculated_crc = esp_crc32_le(0, frame, length - 4);

    // Comparer le CRC reçu avec le CRC calculé
    return (calculated_crc == received_crc);
}

// Fonction pour lire la trame et allumer la LED si elle commence par "R"
void check_uart_data(uint32_t *joystick1A, uint32_t *joystick1R, uint32_t *joystick2A,
          uint32_t *joystick2R, int *target_level,
          int *memo_pinces_interieur_bloque_planches, int *pinces_interieur_bloque_planches,
          int *memo_pinces_exterieur, int *pinces_exterieur,
          int *memo_pousse_planches, int *pousse_planches,
          int *memo_ouvre_affiches, int *ouvre_affiches,
          int *increment_point) {
    uint8_t data[BUF_SIZE];
    int length = uart_read_bytes(UART_NUM, data, BUF_SIZE, 20 / portTICK_PERIOD_MS);

    if (length > 0) {
        //ESP_LOGI("UART", "Données reçues : %.*s", length, data);
        if (verify_crc32(data, length))
        {
          // Vérifier si la trame commence par 'T'
          if (data[0] == 'T') {
              ESP_LOGI("UART", "Trame valide reçue, allumer la LED.");
              gpio_set_level(LED_COMM_PIN, 1);  // Allumer la LED
          } else {
              gpio_set_level(LED_COMM_PIN, 0);  // Éteindre la LED
          }
          if (data[1] == '1') {
              ESP_LOGI("UART", "Pinces interieures et bloc planche");
              if(*memo_pinces_interieur_bloque_planches==0) {
                if (*pinces_interieur_bloque_planches==0) {
                  *pinces_interieur_bloque_planches = 1;
                } else if (*pinces_interieur_bloque_planches==1) {
                  *pinces_interieur_bloque_planches = 0;
                }
              }
              *memo_pinces_interieur_bloque_planches = 1;
          } else if (data[1] == '0') {
              *memo_pinces_interieur_bloque_planches = 0;
          }

          if (data[2] == '1') {
              ESP_LOGI("UART", "Pinces exterieures");
              if(*memo_pinces_exterieur==0) {
                if (*pinces_exterieur==0) {
                  *pinces_exterieur = 1;
                } else if (*pinces_exterieur==1) {
                  *pinces_exterieur = 0;
                }
              }
              *memo_pinces_exterieur = 1;
          } else if (data[2] == '0') {
              *memo_pinces_exterieur = 0;
          }
          
          if (data[3] == '1') {
              ESP_LOGI("UART", "Pousse planche");
              if(*memo_pousse_planches==0) {
                if (*pousse_planches==0) {
                  *pousse_planches = 1;
                } else if (*pousse_planches==1) {
                  *pousse_planches = 0;
                }
              }
              *memo_pousse_planches = 1;
          } else if (data[3] == '0') {
              *memo_pousse_planches = 0;
          }

          if (data[4] == '1') {
              ESP_LOGI("UART", "N0");
              *target_level = 0;
          }
          if (data[5] == '1') {
              ESP_LOGI("UART", "N1");
              *target_level = 1;
          }
          if (data[6] == '1') {
              ESP_LOGI("UART", "N2");
              *target_level = 2;
          }
          if (data[7] == '1') {
              ESP_LOGI("UART", "N3");
              *target_level = 3;
          }
          if (data[8] == '1') {
              ESP_LOGI("UART", "Tempo PAMI");
          }
          if (data[9] == '1') {
              ESP_LOGI("UART", "Incréments points");
              // On incremente que si la variable est a l'etat initial
              if (*increment_point==0) *increment_point = 1;
          }
          if (data[10] == '1') {
              ESP_LOGI("UART", "Ouverture affiche");
              if(*memo_ouvre_affiches==0) {
                if (*ouvre_affiches==0) {
                  *ouvre_affiches = 1;
                } else if (*ouvre_affiches==1) {
                  *ouvre_affiches = 0;
                }
              }
              *memo_ouvre_affiches = 1;
          } else if (data[10] == '0') {
              *memo_ouvre_affiches = 0;
          }
          *joystick1A = (uint32_t)data[11] |
                            ((uint32_t)data[12] << 8) |
                            ((uint32_t)data[13] << 16) |
                            ((uint32_t)data[14] << 24);
          *joystick1R = (uint32_t)data[15] |
                            ((uint32_t)data[16] << 8) |
                            ((uint32_t)data[17] << 16) |
                            ((uint32_t)data[18] << 24);
          *joystick2A = (uint32_t)data[19] |
                            ((uint32_t)data[20] << 8) |
                            ((uint32_t)data[21] << 16) |
                            ((uint32_t)data[22] << 24);
          *joystick2R = (uint32_t)data[23] |
                            ((uint32_t)data[24] << 8) |
                            ((uint32_t)data[25] << 16) |
                            ((uint32_t)data[26] << 24);

        } else {
          ESP_LOGI("UART", "CRC trame invalide");
          gpio_set_level(LED_COMM_PIN, 0);  // Éteindre la LED
        }
    } else {
      gpio_set_level(LED_COMM_PIN, 0);  // Éteindre la LED
    }
}

void process_joystick_value(uint32_t *raw_value) {
    const int32_t CENTER = 2048;   // Position centrale du joystick
    const int32_t DEADZONE = 200; // Largeur de la zone morte (±200 autour de 2048)
    const int32_t MAX_RAW = 4096; // Valeur maximale brute
    const int32_t MIN_RAW = 0;    // Valeur minimale brute
    const int32_t MAX_OUTPUT = 4096; // Valeur maximale de sortie

    if (*raw_value >= CENTER - DEADZONE && *raw_value <= CENTER + DEADZONE) {
        // Zone morte : aucune sortie
        *raw_value = 0;
    } else if (*raw_value > CENTER + DEADZONE) {
        // Au-dessus de la zone morte : mappe vers [0, 4096]
        *raw_value = (*raw_value - (CENTER + DEADZONE)) * MAX_OUTPUT / (MAX_RAW - (CENTER + DEADZONE));
    } else {
        // En dessous de la zone morte : mappe vers [0, -4096]
        *raw_value = -((CENTER - DEADZONE) - *raw_value) * MAX_OUTPUT / ((CENTER - DEADZONE) - MIN_RAW);
    }
}

void drive_robot(uint32_t *joystick1A, uint32_t *joystick1R)
{
    // Calcul des vitesses des moteurs en combinant avance et tourne
    int32_t moteur1 = *joystick1A - *joystick1R; // Moteur gauche
    int32_t moteur2 = *joystick1A + *joystick1R; // Moteur droit

    // Limite les valeurs à la plage autorisée (-4096 à 4096)
    moteur1 = (moteur1 > 4096) ? 4096 : (moteur1 < -4096) ? -4096 : moteur1;
    moteur2 = (moteur2 > 4096) ? 4096 : (moteur2 < -4096) ? -4096 : moteur2;

    // Configuration pour le moteur 1
    if (moteur1 > 0) {
        // Avant : PWM sur IN1, IN2 à LOW
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, MOTOR1_IN1_PWM_CHANNEL, moteur1 * 8191 / 4096);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, MOTOR1_IN1_PWM_CHANNEL);

        ledc_set_duty(LEDC_HIGH_SPEED_MODE, MOTOR1_IN2_PWM_CHANNEL, 0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, MOTOR1_IN2_PWM_CHANNEL);
    } else {
        // Arrière : PWM sur IN2, IN1 à LOW
        moteur1 = -moteur1; // Valeur absolue pour le PWM
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, MOTOR1_IN1_PWM_CHANNEL, 0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, MOTOR1_IN1_PWM_CHANNEL);

        ledc_set_duty(LEDC_HIGH_SPEED_MODE, MOTOR1_IN2_PWM_CHANNEL, moteur1 * 8191 / 4096);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, MOTOR1_IN2_PWM_CHANNEL);
    }

    // Configuration pour le moteur 2
    if (moteur2 > 0) {
        // Avant : PWM sur IN1, IN2 à LOW
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, MOTOR2_IN1_PWM_CHANNEL, moteur2 * 8191 / 4096);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, MOTOR2_IN1_PWM_CHANNEL);

        ledc_set_duty(LEDC_HIGH_SPEED_MODE, MOTOR2_IN2_PWM_CHANNEL, 0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, MOTOR2_IN2_PWM_CHANNEL);
    } else {
        // Arrière : PWM sur IN2, IN1 à LOW
        moteur2 = -moteur2; // Valeur absolue pour le PWM
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, MOTOR2_IN1_PWM_CHANNEL, 0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, MOTOR2_IN1_PWM_CHANNEL);

        ledc_set_duty(LEDC_HIGH_SPEED_MODE, MOTOR2_IN2_PWM_CHANNEL, moteur2 * 8191 / 4096);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, MOTOR2_IN2_PWM_CHANNEL);
    }

}

void drive_chariot(int *chariot_current_level, int *chariot_last_level, int *chariot_target_level)
{
  if (*chariot_current_level == *chariot_target_level) {
      printf("Target level atteint\n");
      //stop_motor();
      gpio_set_level(MOTOR_UP, 0);
      gpio_set_level(MOTOR_DOWN, 0);
  } else {
    // Vérifier si le capteur du niveau cible est activé
    if ((*chariot_target_level == 0 && gpio_get_level(SENSOR_N0)==0) ||
        (*chariot_target_level == 1 && gpio_get_level(SENSOR_N1)==0) ||
        (*chariot_target_level == 2 && gpio_get_level(SENSOR_N2)==0) ||
        (*chariot_target_level == 3 && gpio_get_level(SENSOR_N3)==0)) {
        printf("Niveau cible %d atteint\n", *chariot_target_level);
        //stop_motor();
        gpio_set_level(MOTOR_UP, 0);
        gpio_set_level(MOTOR_DOWN, 0);

        *chariot_current_level = *chariot_target_level;
    } else {
      if (*chariot_last_level < *chariot_target_level) {
          // Monter
          printf("Monter\n");
          gpio_set_level(MOTOR_UP, 1);
          gpio_set_level(MOTOR_DOWN, 0);
      } else if (*chariot_last_level > *chariot_target_level) {
          // Descendre
          printf("Descendre\n");
          gpio_set_level(MOTOR_UP, 0);
          gpio_set_level(MOTOR_DOWN, 1);
      }
    }

  }
}

void read_chariot_target_level(int *chariot_target_level) {
    // Lire l'état des boutons (0 si appuyé, 1 si relâché)
    if (gpio_get_level(BTN_N0)==0)
    {
      *chariot_target_level = 0;
    } else {
      if (gpio_get_level(BTN_N1)==0)
      {
        *chariot_target_level = 1;
      } else {
        if (gpio_get_level(BTN_N2)==0)
        {
          *chariot_target_level = 2;
        } else {
          if (gpio_get_level(BTN_N3)==0)
          {
            *chariot_target_level = 3;
          }
        }
      }
    }
}

void read_chariot_current_level(int *chariot_current_level, int *chariot_last_level) {
    if (gpio_get_level(SENSOR_N0)==0)
    {
      *chariot_current_level = 0;
    } else {
      if (gpio_get_level(SENSOR_N1)==0)
      {
        *chariot_current_level = 1;
      } else {
        if (gpio_get_level(SENSOR_N2)==0)
        {
          *chariot_current_level = 2;
        } else {
          if (gpio_get_level(SENSOR_N3)==0)
          {
            *chariot_current_level = 3;
          } else {
            *chariot_current_level = -1; // Aucun capteur actif
          }
        }
      }
    }
    if (*chariot_current_level!=-1)
    {
      *chariot_last_level = *chariot_current_level;
    }
}

void incremente_points(int *increment_point) {
  if (*increment_point==-1) {
    gpio_set_level(INCR_POINT, 0);
    *increment_point = 0;
  }
  if (*increment_point==1) {
    gpio_set_level(INCR_POINT, 1);
    *increment_point = -1;
  }
}

void drive_actionneurs(int *pousse_planches, int *pinces_interieur_bloque_planches, int *pinces_exterieur, int *ouvre_affiches)
{
  if (*pousse_planches==1) {
    gpio_set_level(POUSSE_PLANCHE_ON, 1);
    gpio_set_level(POUSSE_PLANCHE_OFF, 0);
  } else {
    gpio_set_level(POUSSE_PLANCHE_ON, 0);
    gpio_set_level(POUSSE_PLANCHE_OFF, 1);
  }
  
  if (*pinces_interieur_bloque_planches==1) {
    gpio_set_level(PINCE_INT_BP_ON, 1);
    gpio_set_level(PINCE_INT_BP_OFF, 0);
  } else {
    gpio_set_level(PINCE_INT_BP_ON, 0);
    gpio_set_level(PINCE_INT_BP_OFF, 1);
  }
  
  if (*pinces_exterieur==1) {
    gpio_set_level(PINCE_EXT_ON, 1);
    gpio_set_level(PINCE_EXT_OFF, 0);
  } else {
    gpio_set_level(PINCE_EXT_ON, 0);
    gpio_set_level(PINCE_EXT_OFF, 1);
  }
  
  if (*ouvre_affiches==1) {
    gpio_set_level(OUVERTURE_AFFICHE_ON, 1);
    gpio_set_level(OUVERTURE_AFFICHE_OFF, 0);
  } else {
    gpio_set_level(OUVERTURE_AFFICHE_ON, 0);
    gpio_set_level(OUVERTURE_AFFICHE_OFF, 1);
  }

}

void app_main(void)
{
    init_uart();
    init_gpio();
    init_pwm();

    int chariot_current_level = -1;
    int chariot_last_level = -1;
    int chariot_target_level = 0;

    int pinces_interieur_bloque_planches = 0;
    int memo_pinces_interieur_bloque_planches = 0;

    int pinces_exterieur = 0;
    int memo_pinces_exterieur = 0;

    int pousse_planches = 0;
    int memo_pousse_planches = 0;

    int ouvre_affiches = 0;
    int memo_ouvre_affiches = 0;

    int increment_point = 0;

    // Envoi de trames en continu
    while (1) {
        // Les joysticks sont remis en position 0 par défaut à chaque itération
        uint32_t joystick1A=2048;
        uint32_t joystick1R=2048;
        uint32_t joystick2A=2048;
        uint32_t joystick2R=2048;
        // Buffer pour la trame avec CRC
        uint8_t frame[128];
        // Créer la trame avec CRC
        size_t frame_length = create_frame_with_crc(frame);

        // Envoi de la trame sur le port série
        uart_write_bytes(UART_NUM, (const char *)frame, frame_length);

        // On recupere les commandes locales au robot pour debug
        read_chariot_target_level(&chariot_target_level);

        // Recevoir la réponse de la télécommande
        check_uart_data(&joystick1A, &joystick1R, &joystick2A, &joystick2R, &chariot_target_level,
          &memo_pinces_interieur_bloque_planches, &pinces_interieur_bloque_planches,
          &memo_pinces_exterieur, &pinces_exterieur,
          &memo_pousse_planches, &pousse_planches,
          &memo_ouvre_affiches, &ouvre_affiches,
          &increment_point);

        // Correction des valeurs joystick
        process_joystick_value(&joystick1A);
        process_joystick_value(&joystick1R);

        // Mise à jour de la position courante du chariot
        read_chariot_current_level(&chariot_current_level,
          &chariot_last_level);

        printf("\n");
        printf("Chariot P=N%d C=N%d T=N%d\n",
          chariot_last_level,
          chariot_current_level,
          chariot_target_level);
        //printf("Chariot courant   : N%d\n", chariot_current_level);
        //printf("Chariot target    : N%d\n", chariot_target_level);

        // Piloter le déplacement du robot
        drive_robot(&joystick1A, &joystick1R);

        drive_chariot(&chariot_last_level,
          &chariot_last_level,
          &chariot_target_level);

        drive_actionneurs(&pousse_planches,
          &pinces_interieur_bloque_planches,
          &pinces_exterieur,
          &ouvre_affiches);

        // Attente de 10 milliseconde avant la prochaine trame
        vTaskDelay(10 / portTICK_PERIOD_MS);

    }
}