#include "mcc_generated_files/mcc.h"
#include "defines.h"
#include "crc8.h"

#define LTERM '.'

typedef struct {
    uint8_t calibrated;
    uint16_t id;
    uint8_t t_delay, t_settle;
    uint8_t low_batt; //misura in decimi di volt
    uint8_t n_read;
    uint16_t s1_min, s2_min; //offset dello zero per mappare
    uint16_t s1_max, s2_max; //slope del sensore
    uint16_t s1_bar, s2_bar; // pressione massima nominale del sensore
    uint8_t crc;
} settings_t;

typedef struct {
    uint16_t s1_raw, s2_raw;
    uint16_t s1_map, s2_map;
} meas_t;

typedef union {
    struct {
        unsigned connected :1;
        unsigned bonded :1;
        unsigned stream_open :1;
        unsigned err :1;
        unsigned reboot :1;
        unsigned received_status :1;
        unsigned received_command: 1;
    };
    uint8_t stat;
} bt_status_t;

//////////// VARIABILI GLOBALI //////////////////
extern uint8_t eusartRxBuffer[EUSART_RX_BUFFER_SIZE];
extern uint8_t eusartRxHead;
extern volatile uint8_t eusartTxBufferRemaining;
extern volatile uint8_t eusartRxCount;

settings_t settings, defaults;
bt_status_t bt_status;
char comm[BUF_COMM];
meas_t last_meas;
bool isRunning = true;
bool command_timeout = false;

long map(long x, long in_min, long in_max, long out_min, long out_max){
    return(x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void EEput(uint8_t *data, uint8_t sidx, uint8_t len){
    while (len--) {
        DATAEE_WriteByte(sidx++, *(data++));
    }
}

void EEget(uint8_t *data, uint8_t sidx, uint8_t len){
    while (len--) {
        *(data++) = DATAEE_ReadByte(sidx++);
    }
}

void My_RXhandler(void){
    uint8_t c = '\0';
    static bool cmdstart = false, statstart = false;
    /// salva il carattere nel buffer  ////
    c=RCREG;
#ifdef ECHO
    putch(c);
#endif
    if (c!='#' && c!='(' && c!=')') {//escludi i terminatori di comando e stato
        eusartRxBuffer[eusartRxHead++] = c;
        if(sizeof(eusartRxBuffer) <= eusartRxHead) {
            eusartRxHead = 0;
        }
        eusartRxCount++;
    }
    if (!cmdstart && c=='#'){
        cmdstart=true;
        return;
    } else if (cmdstart && c==LTERM){
        cmdstart = false;
        bt_status.received_command = true;
        bt_status.received_status = false;
        return;
    }
    if (c=='(' && !statstart) {
        statstart=true;
        return;
    } else if (c==')' && statstart) {
        statstart=false;
        bt_status.received_status = true;
        bt_status.received_command = false;
        return;
    }
}

void BTstat1_handler(void){
    volatile bool v = BT_Stat1_GetValue();
    bt_status.connected =~ v;// stato basso indica BT connesso
    if (v) Led_SetLow();
    else Led_SetHigh();
#ifndef DEBUG // abilita timeout solo quando in run
    TMR2_StartTimer();
#endif
}

void BTstat2_handler(void){
    volatile bool v = BT_Stat2_GetValue();
    if (v) Led2_SetHigh();
    else Led2_SetLow();
    bt_status.stream_open = v;
}

void TMR1_handler(void){
    Led_Toggle();
}

void TMR2_recv_timeout_handler(void){
    command_timeout=true;
}

//////////// PROTOTIPI FUNZIONI /////////////////////////////
meas_t readSensors(void);

void main(void) {
    // initialize the device
    SYSTEM_Initialize();
    // set interrupt handlers
    EUSART_SetRxInterruptHandler(My_RXhandler);
    TMR1_SetInterruptHandler(TMR1_handler);
    TMR2_SetInterruptHandler(TMR2_recv_timeout_handler);
    IOCAF4_SetInterruptHandler(BTstat1_handler);
    IOCAF5_SetInterruptHandler(BTstat2_handler);
    // set initial output states
    O_Vbrd_SetHigh();
    BT_Rst_SetLow();
    // Enable the Global Interrupts
    INTERRUPT_GlobalInterruptEnable();
    // Enable the Peripheral Interrupts
    INTERRUPT_PeripheralInterruptEnable();
    // set wake up timer
    TMR1_StopTimer();
    TMR2_StopTimer();
    // wait for power stabilize
    DELAY_milliseconds(T_SETTLE);
    
    if (DATAEE_ReadByte(0) == 0xff) { // inizializza la eeprom
#ifdef DEBUG
        printf("Init EEPROM\n");
#endif
        // tutto questo si puo' togliere se si scrive la eeprom dal programmatore
        defaults.calibrated = 0;
        defaults.id = 0;
        defaults.low_batt = L_BATT;
        defaults.n_read = N_READ;
        defaults.s1_bar = defaults.s2_bar = S_BAR;
        defaults.s1_min = defaults.s2_min = S_MIN;
        defaults.s1_max = defaults.s2_max = S_MAX;
        defaults.t_delay = T_DELAY;
        defaults.t_settle = T_SETTLE;
        defaults.crc = CRC8((uint8_t*) & defaults, sizeof(defaults) - 1);
        EEput((uint8_t*) &defaults, 0, sizeof(settings_t)); // configurazione default
        EEput((uint8_t*) &defaults, sizeof(settings_t), sizeof(settings_t)); // configurazione settings
    } else {
#ifdef DEBUG
        printf("Read EEPROM\n");
#endif
        EEget((uint8_t*) & defaults, 0, sizeof(settings_t));
        EEget((uint8_t*) & settings, sizeof(settings_t), sizeof(settings_t));
        if (CRC8((uint8_t*) & settings, sizeof(settings_t) - 1) != settings.crc) {
            printf("EEPROM CRC Error");
        }
    }
    ///////////////////
    //// MAIN LOOP ////
    ///////////////////
    BT_Rst_SetHigh();
    while (isRunning) {
        char cmd;
        while (!bt_status.received_command && !bt_status.received_status && !command_timeout); // attendi un evento
        TMR2_StopTimer();
        TMR2_WriteTimer(0);
        if (bt_status.received_command){
            cmd=getch(); // prendo un solo carattere, che dovrebbe essere il comando
            switch (cmd) {
                case 'H': { //invia header hello
                    printf("#V:%d,ID:%d\n", VERSION, settings.id);
                    break;
                }
                case 'S': { //setta parametri configurazione
                    settings_t *s = &settings;
                    int rv = 0;
//                    rv = sscanf(comm, "#S:%hu;%u;%hu;%hu;%hu;%hu;%u;%u;%u;%u;%u;%u",
//                            &s->calibrated, &s->id, &s->t_delay, &s->t_settle,
//                            &s->low_batt, &s->n_read,
//                            &s->s1_min, &s->s1_max, &s->s1_bar,
//                            &s->s2_min, &s->s2_max, &s->s2_bar);
                    if (rv == N_PARAMS) { //scrivo i dati nella eeprom
                        settings.crc = CRC8((uint8_t*) s, sizeof(settings_t) - 1); //calcola il crc escludendolo dal calcolo
                        EEput((uint8_t*) &settings, sizeof(settings_t), sizeof(settings_t));
                        printf("Parametri Salvati\n");
                    } else {
                        printf("Errore Parametri:%d\n", rv);
                    }
                    break;
                }
                case 'P': { //stampa i parametri correnti
                    settings_t *s = &settings;
                    printf("#P:%hu;%u;%hu;%hu;%hu;%hu;%u;%u;%u;%u;%u;%u:%02x\n",
                            s->calibrated, s->id, s->t_delay, s->t_settle,
                            s->low_batt, s->n_read,
                            s->s1_min, s->s1_max, s->s1_bar,
                            s->s2_min, s->s2_max, s->s2_bar,
                            s->crc);
                    break;
                }
                case 'G': { //preleva le misure
                    last_meas = readSensors();
                    printf("#G:%hu,%hu,%hu,%hu\n", last_meas.s1_raw, last_meas.s1_map, last_meas.s2_raw, last_meas.s2_map);
                    break;
                }
                case 'B': {
                    break;
                }
                case 'R': { //resetta a default
                    EEput((uint8_t*)&defaults,0,sizeof(settings_t));
                    settings = defaults;
                    printf("Reset Parametri\n");
                    break;
                }
                case 'I': {
                    DATAEE_WriteByte(0,0xff);
                    printf("EEPROM Clear, Reboot\n");
                    RESET();
                    break;
                }
                case 'E': { //abilita timer misura
                    break;
                }
                case 'D': { //disabilita timer misura e lascia in sleep
                    break;
                } 
                default:{
                   //printf("Comando Sconosciuto\n");
                   break;
                }
           }
           
        } else if (bt_status.received_status) {
#ifdef DEBUG
            printf("Stato...\n");
#endif
        } else if (command_timeout){
#ifdef DEBUG
            printf("Timeout...\n");
#endif
            command_timeout=false;
        }
        bt_status.received_command=false;
        bt_status.received_status=false;
        while (eusartTxBufferRemaining != EUSART_TX_BUFFER_SIZE); // flush output buffer
        while (eusartRxCount > 0 && (cmd = getch()) != '\r'); // flush input buffer
        DELAY_milliseconds(10);
        if (!bt_status.stream_open) SLEEP();
    }
}

meas_t readSensors(void)
{
    uint16_t m1[N_READ_MAX], m2[N_READ_MAX];
    meas_t mean;
    O_Vbrd_SetLow();
    DELAY_milliseconds(settings.t_settle);
    for (uint8_t k = 0; k < settings.n_read; k++) {
        m1[k] = ADC_GetConversion(A_S1);
        m2[k] = ADC_GetConversion(A_S2);
#ifdef DEBUG
        printf("%d\t%d  %d\n", k, m1[k], m2[k]);
#endif
        DELAY_milliseconds(settings.t_delay);
    }
    O_Vbrd_SetHigh(); // spegni subito che consumano!  
    mean.s1_raw = 0;
    mean.s2_raw = 0;
    for (uint8_t k = 0; k < settings.n_read; k++) {
        mean.s1_raw += m1[k];
        mean.s2_raw += m2[k];
    }
    mean.s1_raw = (uint16_t) (mean.s1_raw / settings.n_read);
    mean.s2_raw = (uint16_t) (mean.s2_raw / settings.n_read);
    mean.s1_map = (uint16_t) map(mean.s1_raw, settings.s1_min, settings.s1_max, 0, settings.s1_bar);
    mean.s2_map = (uint16_t) map(mean.s2_raw, settings.s2_min, settings.s2_max, 0, settings.s2_bar);
    return mean;
}
// Disable the Global Interrupts
//INTERRUPT_GlobalInterruptDisable();

// Disable the Peripheral Interrupts
//INTERRUPT_PeripheralInterruptDisable();