#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 BUTTONN0_PIN     23
#define BUTTONN1_PIN     22
#define BUTTONN2_PIN     21
#define BUTTONN3_PIN     19
#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
#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
#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


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(BUTTONN0_PIN, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(BUTTONN1_PIN, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(BUTTONN2_PIN, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_set_direction(BUTTONN3_PIN, GPIO_MODE_INPUT);  // Définir le bouton comme entrée
    gpio_pullup_en(BUTTONN0_PIN);                       // Activer la résistance pull-up
    gpio_pullup_en(BUTTONN1_PIN);                       // Activer la résistance pull-up
    gpio_pullup_en(BUTTONN2_PIN);                       // Activer la résistance pull-up
    gpio_pullup_en(BUTTONN3_PIN);                       // Activer la résistance pull-up   
}

// 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 buttonN0_state = gpio_get_level(BUTTONN0_PIN);
    int buttonN1_state = gpio_get_level(BUTTONN1_PIN);
    int buttonN2_state = gpio_get_level(BUTTONN2_PIN);
    int buttonN3_state = gpio_get_level(BUTTONN3_PIN);

    // Création de la trame à envoyer
    char message[128];
    int message_length = snprintf(message, sizeof(message),
                          "\nR%s%s%s%s",
                          buttonN0_state == 0 ? "1" : "0",
                          buttonN1_state == 0 ? "1" : "0",
                          buttonN2_state == 0 ? "1" : "0",
                          buttonN3_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) {
    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 (data[2] == '1') {
              ESP_LOGI("UART", "Pinces exterieures");
          }
          if (data[3] == '1') {
              ESP_LOGI("UART", "Pousse planche");
          }
          if (data[4] == '1') {
              ESP_LOGI("UART", "N0");
          }
          if (data[5] == '1') {
              ESP_LOGI("UART", "N1");
          }
          if (data[6] == '1') {
              ESP_LOGI("UART", "N2");
          }
          if (data[7] == '1') {
              ESP_LOGI("UART", "N3");
          }
          if (data[8] == '1') {
              ESP_LOGI("UART", "Tempo PAMI");
          }
          if (data[9] == '1') {
              ESP_LOGI("UART", "Incréments points");
          }
          if (data[10] == '1') {
              ESP_LOGI("UART", "Ouverture affiche");
          }
          *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 = 2048; // 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, 2048]
        *raw_value = (*raw_value - (CENTER + DEADZONE)) * MAX_OUTPUT / (MAX_RAW - (CENTER + DEADZONE));
    } else {
        // En dessous de la zone morte : mappe vers [0, -2048]
        *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 app_main(void)
{
    init_uart();
    init_gpio();
    init_pwm();

    uint32_t joystick1A=2048;
    uint32_t joystick1R=2048;
    uint32_t joystick2A=2048;
    uint32_t joystick2R=2048;
    

    joystick1A=2048;
    process_joystick_value(&joystick1A);
    printf("\nAvance : %lu", joystick1A);
    joystick1A=2148;
    process_joystick_value(&joystick1A);
    printf("\nAvance : %lu", joystick1A);
    joystick1A=2248;
    process_joystick_value(&joystick1A);
    printf("\nAvance : %lu", joystick1A);
    joystick1A=3000;
    process_joystick_value(&joystick1A);
    printf("\nAvance : %lu", joystick1A);
    joystick1A=4090;
    process_joystick_value(&joystick1A);
    printf("\nAvance : %lu", joystick1A);

    /*

    // Envoi de trames en continu
    while (1) {
        // 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);

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

        // Recevoir la réponse de la télécommande
        check_uart_data(&joystick1A, &joystick1R, &joystick2A, &joystick2R);

        joystick1A=2048;
        joystick1R=2048;

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


        // Afficher les valeurs dans la console Wokwi
        printf("\nAvance : %lu, Tourne: %lu", joystick1A, joystick1R);


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

        

    }
    */
}