#ifndef F_CPU
#define F_CPU 16000000UL
#endif
#ifndef BAUD
#define BAUD 250000
#endif
//MegaCore:avr:2560 
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <util/setbaud.h>
#include <string.h>
#include <stdio.h>
#include <stdint.h>

#define STX 0x02
#define ETX 0x03
#define ACK 0x06
#define NACK 0x15
#define BUFF_LEN 32
#define MAX_DATA_LENGTH 8
#define UART_UBRRH(uart)  ((uart == UART0) ? UBRR0H : (uart == UART1) ? UBRR1H : (uart == UART2) ? UBRR2H : UBRR3H)
#define UART_UBRRL(uart)  ((uart == UART0) ? UBRR0L : (uart == UART1) ? UBRR1L : (uart == UART2) ? UBRR2L : UBRR3L)
#define UART_UCSRB(uart)  ((uart == UART0) ? UCSR0B : (uart == UART1) ? UCSR1B : (uart == UART2) ? UCSR2B : UCSR3B)
#define UART_UCSRC(uart)  ((uart == UART0) ? UCSR0C : (uart == UART1) ? UCSR1C : (uart == UART2) ? UCSR2C : UCSR3C)
#define UART_UDR(uart)    ((uart == UART0) ? UDR0 : (uart == UART1) ? UDR1 : (uart == UART2) ? UDR2 : UDR3)
#define UART_UCSR_A(uart) ((uart == UART0) ? UCSR0A : (uart == UART1) ? UCSR1A : (uart == UART2) ? UCSR2A : UCSR3A)
#define UART_UDRE(uart)   ((uart == UART0) ? UDRE0 : (uart == UART1) ? UDRE1 : (uart == UART2) ? UDRE2 : UDRE3)
#define UART_RXC(uart)    ((uart == UART0) ? RXC0 : (uart == UART1) ? RXC1 : (uart == UART2) ? RXC2 : RXC3)

char input_buffer[BUFF_LEN];
volatile uint8_t read_spot = 0;
volatile uint8_t process_spot = 0;
volatile unsigned char inbyte;

typedef enum {
    UART0,
    UART1,
    UART2,
    UART3
} UART_Select;

typedef struct {
    uint8_t stx;
    uint8_t type_commande;
    uint8_t numero_abonne;
    uint8_t numero_message;
    uint8_t data[MAX_DATA_LENGTH];
    uint8_t data_length;
    uint8_t etx;
    char  brut_msg[BUFF_LEN];
} Message;

void uart_putchar(UART_Select uart, char c);
void uart_putstr(UART_Select uart, const char *data);
void uart_send_ack(UART_Select uart);
void uart_send_nack(UART_Select uart);
void uart_init(UART_Select uart);
void send_command(UART_Select uart, const char *value);
int parse_message(uint8_t *buffer, int length, Message *msg);
void print_message(const Message *msg);
void configurePins();
void configurePinAsInput(volatile uint8_t *ddr, uint8_t pin);
void configurePinAsOutput(volatile uint8_t *ddr, uint8_t pin);
void SetPinByName(const char* name, bool state);
bool ReadPinByName(const char* name);

volatile int message_ready = 0;
Message msg;
UART_Select debug_uart = UART0; // Choisir l'UART de debug
UART_Select selected_uart = UART1; // Choisir l'UART par défaut


struct PinConfig {
    const char* name;       // Alias de la pin
    volatile uint8_t *ddr;  // Pointeur vers le registre DDR du port
    uint8_t pin;            // Numéro de la pin
    bool isOutput;          // true si la pin est une sortie, false si c'est une entrée
};

// Tableau de configuration des pins avec alias
PinConfig pinConfigs[] = {
    {"A0", &DDRA, PA0, false},  // TOTO1 sur PA0, configurée en entrée
    {"LED_BUILTIN", &DDRB, PB7, true},  // LED_BUILTIN sur PB5, configurée en sortie
    {"TOTO2", &DDRA, PA2, false},  // TOTO2 sur PA2, configurée en entrée
    {"TRUC1", &DDRA, PA3, true},  // TRUC1 sur PA3, configurée en sortie
    {"TRUC2", &DDRL, PL2, true}   // TRUC2 sur PL2, configurée en sortie
    // Ajouter d'autres configurations ici si nécessaire
};

void configurePins() {
    for (uint8_t i = 0; i < sizeof(pinConfigs) / sizeof(pinConfigs[0]); i++) {
        if (pinConfigs[i].isOutput) {
            configurePinAsOutput(pinConfigs[i].ddr, pinConfigs[i].pin); // Configurer comme sortie
        } else {
            configurePinAsInput(pinConfigs[i].ddr, pinConfigs[i].pin);  // Configurer comme entrée
        }
    }
}

void configurePinAsInput(volatile uint8_t *ddr, uint8_t pin) {
    *ddr &= ~(1 << pin);  // Configure la pin comme entrée
}

void configurePinAsOutput(volatile uint8_t *ddr, uint8_t pin) {
    *ddr |= (1 << pin);  // Configure la pin comme sortie
}

int main(void) {
    configurePins();  // Configurer les pins selon le tableau
    uart_init(debug_uart,250000,8,'N');
    uart_init(selected_uart,9600,7,'E');
    
    // Initialiser UART2 et UART3 si nécessaire
    uart_init(UART2,9600,7,'E');
    uart_init(UART3,9600,7,'E');
    
    if (ReadPinByName("A0")) { // Babord
        SetPinByName("LED_BUILTIN", true);  // Set TRUC2 HIGH
        send_command(selected_uart, "0F0");
    } else { // Tribord
        SetPinByName("LED_BUILTIN", false); // Set TRUC2 LOW
        send_command(selected_uart, "0E0");
    }

    while (1) {
        if (message_ready) {
            // Si un message est prêt, le traiter
            message_ready = 0;

            int length = (read_spot >= process_spot) ?
                         (read_spot - process_spot) :
                         (BUFF_LEN - process_spot + read_spot);

            int etx_index = -1;
            for (int i = 0; i < length; i++) {
                int index = (process_spot + i) % BUFF_LEN;
                if (input_buffer[index] == ETX) {
                    etx_index = index;
                    break;
                }
            }

            if (etx_index != -1) {
                length = (etx_index >= process_spot) ?
                         (etx_index - process_spot + 1) :
                         (BUFF_LEN - process_spot + etx_index + 1);

                if (parse_message((uint8_t *)&input_buffer[process_spot], length, &msg) > 0) {
                    // Traitement du message en fonction du numéro
                    get_command(selected_uart,msg.brut_msg);
                    switch (msg.numero_message) {
                        case 0x30:
                            uart_putstr(debug_uart, "Message R0\n");
                            uart_send_ack(selected_uart,&msg);
                            break;
                        case 0x31:
                            uart_putstr(debug_uart, "Message R1\n");
                            uart_send_ack(selected_uart,&msg);
                            break;
                        case 0x32:
                            uart_putstr(debug_uart, "Message R2\n");
                            uart_send_ack(selected_uart,&msg);
                            break;
                        case 0x34:
                            uart_putstr(debug_uart, "Message R4\n");
                            uart_send_ack(selected_uart,&msg);
                            break;
                        case 0x35:
                            uart_putstr(debug_uart, "Message R5\n");
                            uart_send_ack(selected_uart,&msg);
                            break;
                        default:
                            uart_putstr(debug_uart, "Unknown Message");
                            uart_send_nack(selected_uart);
                            break;
                    }

                    process_spot = 0; 
                    read_spot = 0;
                } else {
                    uart_send_nack(selected_uart);
                    process_spot = 0; // Avancer le process_spot après le message traité
                    read_spot = 0;
                }
            }
        }
    }

    return 0;
}

void send_command(UART_Select uart, const char *value) {
    // Log the command and the UART it is sent to using serial_printf
    uart_putstr(uart, value);
    serial_printf(debug_uart, "send_command from %s: %s\n", uart_to_string(uart), value);
}

void get_command(UART_Select uart, const char *value) {
    // Log the command and the UART it is sent to using serial_printf
    serial_printf(debug_uart, "Get_command from %s: %s\n", uart_to_string(uart), value);
}

void serial_printf(UART_Select uart, const char *format, ...) {
    char buffer[64];  // Buffer pour la chaîne formatée
    va_list args;
    
    // Initialiser la liste des arguments
    va_start(args, format);
    
    // Formater les arguments dans la chaîne
    vsnprintf(buffer, sizeof(buffer), format, args);
    
    // Nettoyer la liste des arguments
    va_end(args);
    
    // Envoyer la chaîne formatée via l'UART
    uart_putstr(uart, buffer);
}

int parse_message(uint8_t *buffer, int length, Message *msg) {
    int etx_index = -1, stx_index = -1;

    for (int i = 0; i < length; i++) {
        if (buffer[i] == ETX) {
            etx_index = i;
        } else if (buffer[i] == STX) {
            stx_index = i;
        }
    }

    if (etx_index == -1 || stx_index == -1 || stx_index >= etx_index) {
        uart_putstr(debug_uart, "Error: Invalid message format\n");
        return -1;
    }

    int message_length = etx_index - stx_index + 1;
    if (message_length < 6) {
        uart_putstr(debug_uart, "Error: Message too short\n");
        return -2;
    }

    msg->stx = buffer[stx_index];
    msg->type_commande = buffer[stx_index + 1];
    msg->numero_abonne = buffer[stx_index + 2];
    msg->numero_message = buffer[stx_index + 3];

    msg->data_length = message_length - 5;
    if (msg->data_length > MAX_DATA_LENGTH) {
        msg->data_length = MAX_DATA_LENGTH;
    }
    memcpy(msg->data, &buffer[stx_index + 4], msg->data_length);
    msg->etx = buffer[etx_index];
    memcpy(msg->brut_msg, &buffer[stx_index+1 ], etx_index-1);
    uart_putstr(debug_uart, "Message parsed successfully\n");
    
    return etx_index + 1;
}

void print_message(const Message *msg) {
    serial_printf(debug_uart, "print_message\n");
    serial_printf(debug_uart, "STX: 0x%02X\n", msg->stx);
    serial_printf(debug_uart, "Type commande: 0x%02X\n", msg->type_commande);
    serial_printf(debug_uart, "N° abonné: 0x%02X\n", msg->numero_abonne);
    serial_printf(debug_uart, "N° message: 0x%02X\n", msg->numero_message);
    serial_printf(debug_uart, "Data:");
    for (int i = 0; i < msg->data_length; ++i) {
        serial_printf(debug_uart, " 0x%02X", msg->data[i]);
    }
    serial_printf(debug_uart, "\n");
    serial_printf(debug_uart, "ETX: 0x%02X\n", msg->etx);
}

void uart_send_ack(UART_Select uart,const Message *msg) {
    print_message(msg);
    char msg_string[64];
    snprintf(msg_string, sizeof(msg_string), "%c%c%c", msg->type_commande, msg->numero_abonne, msg->numero_message);
    send_command(selected_uart, msg_string);
    uart_putchar(uart, ACK);
}

void uart_send_nack(UART_Select uart) {
    uart_putchar(uart, NACK);
}

const char* uart_to_string(UART_Select uart) {
    switch (uart) {
        case UART0: return "UART0";
        case UART1: return "UART1";
        case UART2: return "UART2";
        case UART3: return "UART3";
        default: return "Unknown UART";
    }
}

void uart_init(UART_Select uart, unsigned long baudrate, uint8_t data_bits, char parity) {
    cli(); // Désactiver les interruptions pendant la configuration

    // Calculer UBRR (UART Baud Rate Register) en fonction de la formule : UBRR = (F_CPU / (16 * Baudrate)) - 1
    unsigned int ubrr_value = (F_CPU / (16 * baudrate)) - 1;

    UART_UBRRH(uart) = (unsigned char)(ubrr_value >> 8); // Partie haute du registre UBRR
    UART_UBRRL(uart) = (unsigned char)ubrr_value;        // Partie basse du registre UBRR

  #if USE_2X
    UART_UCSR_A(uart) |= _BV(U2X0); // Activer le mode double vitesse (si nécessaire)
  #else
    UART_UCSR_A(uart) &= ~_BV(U2X0); // Désactiver le mode double vitesse
  #endif

    // Activer la réception, l'émission et l'interruption de réception
    UART_UCSRB(uart) = _BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0);

    // Configurer UCSRC pour les bits de données et la parité
    unsigned char ucsrc_value = 0;

    // Configuration du nombre de bits de données
    switch (data_bits) {
        case 5:
            // Aucun bit UCSZ00 ni UCSZ01
            break;
        case 6:
            ucsrc_value |= _BV(UCSZ00);
            break;
        case 7:
            ucsrc_value |= _BV(UCSZ01);
            break;
        case 8:
            ucsrc_value |= _BV(UCSZ00) | _BV(UCSZ01);
            break;
        case 9:
            ucsrc_value |= _BV(UCSZ00) | _BV(UCSZ01);
            UART_UCSRB(uart) |= _BV(UCSZ02); // UCSZ02 se configure dans UCSRB pour 9 bits
            break;
        default:
            // Si une valeur incorrecte est passée, on choisit par défaut 8 bits
            ucsrc_value |= _BV(UCSZ00) | _BV(UCSZ01);
            break;
    }

    // Configuration de la parité
    switch (parity) {
        case 'N': // Pas de parité
            // Aucun bit UPM00 ni UPM01
            break;
        case 'E': // Parité paire
            ucsrc_value |= _BV(UPM01);
            break;
        case 'O': // Parité impaire
            ucsrc_value |= _BV(UPM01) | _BV(UPM00);
            break;
        default:
            // Si une valeur incorrecte est passée, on choisit par défaut pas de parité
            break;
    }

    // Configurer UCSRC avec les options sélectionnées
    UART_UCSRC(uart) = ucsrc_value;

    // Afficher un message de debug pour indiquer l'initialisation de l'UART
    uart_putstr(debug_uart, uart_to_string(uart));
    uart_putstr(debug_uart, " Initialised\n");

    sei(); // Réactiver les interruptions après la configuration
}



void uart_putchar(UART_Select uart, char c) {
    loop_until_bit_is_set(UART_UCSR_A(uart), UART_UDRE(uart));
    UART_UDR(uart) = c;
}

void USART_Flush(UART_Select uart) {
    unsigned char dummy;
    while (UART_UCSR_A(uart) & (1 << UART_RXC(uart))) {
        dummy = UART_UDR(uart);
    }
}


void uart_putstr(UART_Select uart, const char *data) {
    while (*data) {
        uart_putchar(uart, *data++);
    }
}

ISR(USART0_RX_vect) {
    inbyte = UDR0;
    uint8_t next_read_spot = (read_spot + 1) % BUFF_LEN;
    if (next_read_spot != process_spot) {
        input_buffer[read_spot] = inbyte;

        if (inbyte == ETX) {
            message_ready = 1;
        }

        read_spot = next_read_spot;
    } else {
        uart_putstr(debug_uart, "Buffer Overflow\n");
        read_spot = 0;
        process_spot = 0;
    }
  
}

ISR(USART1_RX_vect) {
    inbyte = UDR1;
    uint8_t next_read_spot = (read_spot + 1) % BUFF_LEN;
    if (next_read_spot != process_spot) {
        input_buffer[read_spot] = inbyte;

        if (inbyte == ETX) {
            message_ready = 1;
        }

        read_spot = next_read_spot;
    } else {
        uart_putstr(debug_uart, "Buffer Overflow\n");
        read_spot = 0;
        process_spot = 0;
    }
    
}

ISR(USART2_RX_vect) {
    inbyte = UDR2;
    uint8_t next_read_spot = (read_spot + 1) % BUFF_LEN;
    if (next_read_spot != process_spot) {
        input_buffer[read_spot] = inbyte;

        if (inbyte == ETX) {
            message_ready = 1;
        }

        read_spot = next_read_spot;
    } else {
        uart_putstr(debug_uart, "Buffer Overflow\n");
        read_spot = 0;
        process_spot = 0;
    }
}

ISR(USART3_RX_vect) {
    inbyte = UDR3;
    uint8_t next_read_spot = (read_spot + 1) % BUFF_LEN;
    if (next_read_spot != process_spot) {
        input_buffer[read_spot] = inbyte;

        if (inbyte == ETX) {
            message_ready = 1;
        }

        read_spot = next_read_spot;
    } else {
        uart_putstr(debug_uart, "Buffer Overflow\n");
        read_spot = 0;
        process_spot = 0;
    }
}

PinConfig* getPinConfigByName(const char* name) {
    for (uint8_t i = 0; i < sizeof(pinConfigs) / sizeof(pinConfigs[0]); i++) {
        if (strcmp(pinConfigs[i].name, name) == 0) {
            return &pinConfigs[i];
        }
    }
    return NULL; // Retourne NULL si la pin n'est pas trouvée
}

void SetPinByName(const char* name, bool state) {
    PinConfig* config = getPinConfigByName(name);
    if (config != NULL && config->isOutput) {
        if (state) {
            *(config->ddr + 1) |= (1 << config->pin); // PORTx est à l'adresse DDRx + 1
        } else {
            *(config->ddr + 1) &= ~(1 << config->pin);
        }
    } else {
        // Gérer l'erreur (par exemple, afficher un message d'erreur ou ignorer)
    }
}

bool ReadPinByName(const char* name) {
    PinConfig* config = getPinConfigByName(name);
    if (config != NULL && !config->isOutput) {
        return (*(config->ddr - 1) & (1 << config->pin)) != 0; // PINx est à l'adresse DDRx - 1
    } else {
        // Gérer l'erreur (par exemple, retourner false ou indiquer une erreur)
        return false;
    }
}

//0F244006600
//0F0