
#include <xc.h>

#include "mcc_generated_files/system.h"
#include <libpic30.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include "mcc_generated_files/pin_manager.h"
#include "mcc_generated_files/usb/usb_device_cdc.h"
#include "mcc_generated_files/i2c2.h"
#include "mcc_generated_files/spi1.h"
#include "mcc_generated_files/adc1.h"
#include "magnetometer1MMC.h"
#include "magnetometer2BM.h"
#include "IMUIIM.h"
#include "MemoryCAT24.h"
#include "mcc_generated_files/tmr1.h"

typedef enum COMMAND {
    GeneralError = 0x08,
    MMCcalibration = 'a',
    MMCsoftironing = 'b',
    MMCHIcompensated = 'c',
    MMCcompensated = 'd',
    MMCazimuth = 'e',
    BMcalibration = 'f',
    BMsoftironing = 'g',
    BMHIcompensated = 'h',
    BMcompensated = 'i',
    BMazimuth = 'j',
    IIMcalibration = 'k',
    IIMcompensated = 'l',
    IIMtilt = 'm',
    IIMazimuth = 'n',
    Memorydownload = 'o',
    Memorydelete = 'p'
} COMMAND;

uint8_t sizeOfResponseBuffer = 0;
COMMAND command = GeneralError;
COMMAND responseCmd = GeneralError;
uint8_t responseBuffer[32] = {};
uint8_t buffer[16] = {};

uint8_t buff_ADC[16] = {}; //ADC
float Temp = 0; //ADC
char Temp_write[64] = {0}; //ADC
uint32_t address = 0;

uint8_t buff_memory1[256] = {};
uint8_t buff_memory2[256] = {};
uint8_t measureAzimuthBM = 0;
uint8_t measureAzimuthMMC = 0;
uint8_t measureAzimuthIIM = 0;
uint8_t measureTiltIIM = 0;

uint32_t i = 0;

int main(void) {
    //
    SYSTEM_Initialize();
    __delay_ms(100);
    IIM_init();
    BM_init();
    MMC_init();
    __delay_ms(100);
    TMR1_Start();
    __delay_ms(1000);
    LED_0_SetHigh();
    __delay_ms(1000);
    
    while (1) {

        if (getsUSBUSART(buffer, 4)) {
            command = GeneralError;
            uint8_t dataSetupMSB = 0;
            uint8_t dataSetupLSB = 0;
            uint8_t skip_response = 0;
            responseCmd = GeneralError;
            memset(responseBuffer, 0, sizeof (responseBuffer));
            sizeOfResponseBuffer = 0;

            USB_parseCommandPacket(buffer, (uint8_t*) & command, &dataSetupMSB, &dataSetupLSB);

            switch (command) {
                case GeneralError:
                {
                    responseCmd = GeneralError;
                }
                    break;
                case MMCcalibration:
                {
                    LED_1_SetHigh();
                    MMC_Calib(500);
                    __delay_ms(20); //total time = 6,03 s
                    LED_1_SetLow();
                    skip_response = 1;
                }
                    break;
                case MMCsoftironing:
                {
                    LED_1_SetHigh();
                    MMC_SI_Calib(500);
                    __delay_ms(20); //total time = 6,03 s
                    LED_1_SetLow();
                    skip_response = 1;
                }
                    break;
                case MMCHIcompensated:
                {
                    LED_1_SetHigh();
                    MMC_Meas_HI_Compensated(500);
                    __delay_ms(20);
                    LED_1_SetLow(); //total time = 6,02 s
                    skip_response = 1;
                }
                    break;
                case MMCcompensated:
                {
                    LED_1_SetHigh();
                    MMC_Meas_Compensated(500);
                    __delay_ms(20);
                    LED_1_SetLow();
                    skip_response = 1;
                }
                    break;
                case MMCazimuth:
                {
                    // MMC_Meas_Compas(500);
                    //__delay_ms(20);
                    if (measureAzimuthMMC == 1)
                        measureAzimuthMMC = 0;
                    else
                        measureAzimuthMMC = 1;
                    skip_response = 1;
                }
                    break;
                case BMcalibration:
                {
                    LED_1_SetHigh();
                    BM_Calib(500);
                    __delay_ms(20);
                    LED_1_SetLow(); //total time = 6,03 s
                    skip_response = 1;
                }
                    break;
                case BMsoftironing:
                {
                    LED_1_SetHigh();
                    BM_SI_Calib(500);
                    __delay_ms(20); //total time = 6,03 s
                    LED_1_SetLow();
                    skip_response = 1;
                }
                    break;
                case BMHIcompensated:
                {
                    LED_1_SetHigh();
                    BM_Meas_HI_Compensated(500);
                    __delay_ms(20);
                    LED_1_SetLow(); //total time = 6,02 s
                    skip_response = 1;
                }
                    break;
                case BMcompensated:
                {
                    LED_1_SetHigh();
                    BM_Meas_Compensated(500);
                    __delay_ms(20); //total time = 6,02 s
                    LED_1_SetLow();
                    skip_response = 1;
                }
                    break;
                case BMazimuth:
                {
                    //BM_Meas_Compas(1);
                    //__delay_ms(20);
                    if (measureAzimuthBM == 1)
                        measureAzimuthBM = 0;
                    else
                        measureAzimuthBM = 1;
                    skip_response = 1;
                }
                    break;
                case IIMcalibration:
                {
                    LED_1_SetHigh();
                    IIM_Calib(500);
                    __delay_ms(20); //total time = 6,03 s
                    LED_1_SetLow();
                    skip_response = 1;
                }
                    break;
                case IIMcompensated:
                {
                    LED_1_SetHigh();
                    IIM_Meas_Compensated(500);
                    __delay_ms(20);
                    LED_1_SetLow(); //total time = 6,02 s
                    skip_response = 1;
                }
                    break;
                case IIMtilt:
                {
                    if (measureTiltIIM == 1)
                        measureTiltIIM = 0;
                    else
                        measureTiltIIM = 1;
                    skip_response = 1;
                }
                    break;
                case IIMazimuth:
                {
                    if (measureAzimuthIIM == 1)
                        measureAzimuthIIM = 0;
                    else
                        measureAzimuthIIM = 1;
                    skip_response = 1;
                }
                    break;

                case Memorydownload:
                {
                    for (i = 0; i <= 0x1FF; i++) {
                        CAT_Read_Data(i << 8, 0x80, buff_memory1);
                        USB_printSlowUSB(buff_memory1, 0x80);
                        CAT_Read_Data(((i << 8) | 0x80), 0x80, buff_memory2);
                        USB_printSlowUSB(buff_memory2, 0x80);
                    }
                    __delay_ms(10);
                    skip_response = 1;
                }
                    break;
                case Memorydelete:
                {
                    CAT_Erase();
                    __delay_ms(20);
                    responseCmd = Memorydelete;
                }
                    break;

                default:
                {
                    responseCmd = GeneralError;
                }
                    break;
            }
            if (skip_response == 0) {
                USB_sendCommandPacket((uint8_t) responseCmd, responseBuffer, sizeOfResponseBuffer);
            }

        }
        if ((measureAzimuthBM == 1) || (measureAzimuthMMC == 1)) {
            if (measureAzimuthBM == 1) {
                BM_Meas_Azimuth(2);
            }
            if (measureAzimuthMMC == 1) {
                MMC_Meas_Azimuth(2);
            }
            __delay_ms(20);
        }

        if (measureAzimuthIIM == 1) {
            if (measureAzimuthIIM == 1) {
                IIM_Meas_Angle();
                //IIM_Drift_Error(0, 0, 0);
                __delay_ms(20);
            }
        }
        if (measureTiltIIM == 1) {
            if (measureTiltIIM == 1) {
                IIM_Meas_Tilt();
                __delay_ms(20);
            }
        }

        if (USBIsBusSuspended() == true) {
            static uint8_t state = 0;
            if (wasBtnPressed()) {
                if (++state > 2)
                    state = 0;
                
                LED_0_SetLow();
                LED_1_SetLow();
                LED_2_SetLow();
            }
            float varToSave = 0;
            if (state == 0) {
                LED_0_SetHigh();
                varToSave = IIM_Single_Meas_Filtered_Angle();
            } else if (state == 1) {
                LED_1_SetHigh();
                varToSave = IIM_Single_Meas_Drift_Angle();
            } else if (state == 2) {
                LED_2_SetHigh();
                varToSave = IIM_Single_Meas_Tilt();
            } else {
                state = 0;
            }
            
            CAT_Write_Data(address, (uint8_t*)&varToSave, sizeof(float));
            address += sizeof(float);
            __delay_ms(100);
        }

        uint16_t adcResult = ADC1_ConversionResultGet(VBat_Mon);
        // BatteryVoltage = ((2*adcResult)/4095)*3.3;
        if (adcResult < 1900) { //smaller than 3 V
            MMC_Meas_Set(MMC_Device_Status_0, 0x00);
            IIM_Reg_Set(IIM_Power_Management_0, 0x20);
            BM_Meas_Set(BM_Contr_Reg_1, BM_Contr_Reg_1_Set_sleep);
            BM_Meas_Set(BM_Contr_Reg_2, BM_Contr_Reg_2_Set_sleep);
            Sleep();
        }
    }

    return 1;
}