//IMUIIM.c

#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include "IMUIIM.h"
#include "magnetometer1MMC.h"
#include "mcc_generated_files/pin_manager.h"
#include "mcc_generated_files/spi1.h"
#include "mcc_generated_files/usb/usb_device_cdc.h"

uint8_t buff_SPI_write[16] = {};
static uint8_t buff_SPI_read[16] = {};
uint8_t buff_SPI_help[16] = {};
uint16_t buff_SPI_data = 0;
static int16_t IIM_Data_Temp = 0;
static int16_t IIM_Data_Acc_X = 0;
static int16_t IIM_Data_Acc_Y = 0;
static int16_t IIM_Data_Acc_Z = 0;
static int16_t IIM_Data_Gyro_X = 0;
static int16_t IIM_Data_Gyro_Y = 0;
static int16_t IIM_Data_Gyro_Z = 0;
static int16_t IIM_Offset_Acc[3] = {-367, 404, 23};
static int16_t IIM_Offset_Gyro[3] = {40, -52, 010};
static IIM_DATA IIM_Measured_Data = {0, 0, 0, 0, 0, 0, 0};
static IIM_DATA IIM_Data_ms2 = {0, 0, 0, 0, 0, 0, 0};
float angle = 0;
float angle_x = 0;
float angle_y = 0;
float angle_z = 0;
float lastGyroZ = 0;
static float angle_complementary = 0;
float angle_drift_x = 0;
float angle_drift_y = 0;
float angle_drift_z = 0;
float tilt_angle = 0;
float tilt_angle_prev = 0;
float angle_drift_complementary = 0;
float angle_trapez = 0;
float alpha = 0.85;
float time_const_complem_filt = 0.78;
int8_t Buff_IIM_send [256];
float Buff_IIM_float_send [128];
uint8_t drift_x = 0;
uint8_t drift_y = 0;
uint8_t drift_z = 0;
//int8_t error_x = 0;
//int8_t error_y = 0;
//int8_t error_z = 0;
int8_t Buff_iimsend [256];
IIM_DATA idata;

int16_t IIM_Data_Acc_Compensated_X;
int16_t IIM_Data_Acc_Compensated_Y;
int16_t IIM_Data_Acc_Compensated_Z;
int16_t IIM_Data_Gyro_Compensated_X;
int16_t IIM_Data_Gyro_Compensated_Y;
int16_t IIM_Data_Gyro_Compensated_Z;

typedef struct {
    _axesIIMInt acc;
    _axesIIMInt gyro;
    float temperature;
} IIM_val;

IIM_val iimraw, iimmax, iimmin;

uint8_t IIM_Read_Operation(uint8_t Reg_Name) {
    uint8_t Read_Name = Reg_Name + 0x80;
    return (Read_Name);
}

void IIM_Bank_Set(uint8_t IIM_Bank_Num) {
    uint8_t addr[2] = {IIM_Register_Bank_Sel, IIM_Bank_Num};
    SS1OUT_SetLow();
    buff_SPI_data = SPI1_Exchange8bitBuffer(&addr[0], 2, buff_SPI_help);
    SS1OUT_SetHigh();
}

void IIM_Reg_Set(uint16_t Reg_Name, uint8_t Reg_Name_Set) {
    uint8_t bank = (Reg_Name >> 8);
    IIM_Bank_Set(bank);
    uint8_t addr[2] = {(0xFF & Reg_Name), Reg_Name_Set};
    SS1OUT_SetLow();
    buff_SPI_data = SPI1_Exchange8bitBuffer(&addr[0], 2, buff_SPI_help);
    SS1OUT_SetHigh();
}

void IIM_init(void) {
    IIM_Reg_Set(IIM_Gyro_Config_0, 0x49); //Gyro Scale +-500 dps, ODR 50 Hz
    IIM_Reg_Set(IIM_Acc_Config_0, 0x69); //Acc Scale +-2g dps, ODR 50 Hz
    IIM_Reg_Set(IIM_Sensor_Config_0, 0x00); //Acc & Gyro x, y, z on
    IIM_Reg_Set(IIM_Int_Config_1, 0x00); //INT pulse 100 us, se-assertion 100 us
    IIM_Reg_Set(IIM_Int_Source_0, 0x08); //INT1 is Data Ready
    IIM_Reg_Set(IIM_Power_Management_0, 0x0F); //Temp on, Gyro Low Noise, Acc Low Noise
}

void IIM_Calib(uint16_t num) {
    int8_t length;
    uint16_t a = num;
    // init of sums
    int64_t sum_acc_x = 0;
    int64_t sum_acc_y = 0;
    int64_t sum_acc_z = 0;
    int64_t sum_gyro_x = 0;
    int64_t sum_gyro_y = 0;
    int64_t sum_gyro_z = 0;

    while (--a) {
        IIM_Read_Data(IIM_Temp_Data_1, 14);
        
        sum_acc_x += iimraw.acc.x;
        sum_acc_y += iimraw.acc.y;
        sum_acc_z += iimraw.acc.z;
        sum_gyro_x += iimraw.gyro.x;
        sum_gyro_y += iimraw.gyro.y;
        sum_gyro_z += iimraw.gyro.z;

        length = sprintf((char*) Buff_IIM_send, "%d\t%d\t%d\t\t%d\t%d\t%d\r\n", iimraw.acc.x, iimraw.acc.y, iimraw.acc.z, iimraw.gyro.x, iimraw.gyro.y, iimraw.gyro.z);
        USB_printSlowUSB(Buff_IIM_send, length);
        __delay_ms(12);
    }

    IIM_Offset_Acc[0] = sum_acc_x / num;
    IIM_Offset_Acc[1] = sum_acc_y / num;
    IIM_Offset_Acc[2] = (sum_acc_z / num) - 16384;
    IIM_Offset_Gyro[0] = sum_gyro_x / num;
    IIM_Offset_Gyro[1] = sum_gyro_y / num;
    IIM_Offset_Gyro[2] = sum_gyro_z / num;

    //write offset IIM acc
    length = sprintf((char*) Buff_IIM_send, "\r\n%d\t%d\t%d\t\t%d\t%d\t%d\r\n", IIM_Offset_Acc[0], IIM_Offset_Acc[1], IIM_Offset_Acc[2], IIM_Offset_Gyro[0], IIM_Offset_Gyro[1], IIM_Offset_Gyro[2]);
    USB_printSlowUSB(Buff_IIM_send, length);
    __delay_ms(12);
}

void IIM_Meas_Tilt(void) {
    int8_t length;
    IIM_Read_Data(IIM_Temp_Data_1, 14);
    
    angle_x = 0.02 * IIM_Measured_Data.iim_gyro_x;
    angle_y = 0.02 * IIM_Measured_Data.iim_gyro_y;
         
    long double xyplane = sqrt(((IIM_Measured_Data.iim_acc_x * IIM_Measured_Data.iim_acc_x) + (IIM_Measured_Data.iim_acc_y * IIM_Measured_Data.iim_acc_y)));
    double tilt = 57.295779 * atan2(xyplane, IIM_Measured_Data.iim_acc_z);
    
    length = sprintf((char*) Buff_IIM_float_send, "%f\t%f\t%f\t%f\r\n", IIM_Measured_Data.iim_acc_x, IIM_Measured_Data.iim_acc_y, IIM_Measured_Data.iim_acc_z, tilt);
    USB_printSlowUSB(Buff_IIM_float_send, length);
}

float IIM_Single_Meas_Tilt(void) {
    IIM_Read_Data(IIM_Temp_Data_1, 14);
    
    angle_x = 0.02 * IIM_Measured_Data.iim_gyro_x;
    angle_y = 0.02 * IIM_Measured_Data.iim_gyro_y;
 
    long double xyplane = sqrt(((IIM_Measured_Data.iim_acc_x * IIM_Measured_Data.iim_acc_x) + (IIM_Measured_Data.iim_acc_y * IIM_Measured_Data.iim_acc_y)));
    double tilt = 57.295779 * atan2(xyplane, IIM_Measured_Data.iim_acc_z);
    
    return tilt;
}

void IIM_Meas_Angle(void) {
    int8_t length;

    IIM_Read_Data(IIM_Temp_Data_1, 14);
    float magneto = MMC_Single_Meas_Azimuth();

    //WIthout drift correction & with complementary filter
    angle_z = 0.02 * IIM_Measured_Data.iim_gyro_z;
    angle_complementary = (alpha * (angle_complementary + angle_z)) + (1 - alpha) * magneto;

    
    //With drift compensation & without filter
    IIM_Drift_Error(10, 10, 10);
    angle_z= 0.02 * IIM_Measured_Data.iim_gyro_z;
    angle_drift_complementary += angle_z;
    if (angle_drift_complementary < 0)
            angle_drift_complementary += 360;
    if (angle_drift_complementary > 360)
        angle_drift_complementary -= 360;
    
    angle_z = 0.02 * lastGyroZ +  ((0.02 * (IIM_Measured_Data.iim_gyro_z - lastGyroZ)/2));
    angle_trapez += angle_z;
    if (angle_trapez < 0)
        angle_trapez += 360;
    if (angle_trapez > 360)
        angle_trapez -= 360;
            
            lastGyroZ = IIM_Measured_Data.iim_gyro_z;
    
    length = sprintf((char*) Buff_IIM_float_send, "%f\t%f\t%f\r\n",  angle_complementary, angle_drift_complementary, angle_trapez);
    USB_printSlowUSB(Buff_IIM_float_send, length);
}

float IIM_Single_Meas_Filtered_Angle(void) {
    IIM_Read_Data(IIM_Temp_Data_1, 14);
    float magneto = MMC_Single_Meas_Azimuth();

    //WIthout drift correction & with complementary filter
    angle_z = 0.02 * IIM_Measured_Data.iim_gyro_z;
    angle_complementary = (alpha * (angle_complementary + angle_z)) + (1 - alpha) * magneto;

    return angle_complementary;
}

float IIM_Single_Meas_Drift_Angle(void) {
    IIM_Read_Data(IIM_Temp_Data_1, 14);
    
    //With drift compensation
    IIM_Drift_Error(10, 10, 10);
    
    angle_z= 0.02 * IIM_Measured_Data.iim_gyro_z;
    angle_drift_complementary += angle_z;
    if (angle_drift_complementary < 0)
            angle_drift_complementary += 360;
    if (angle_drift_complementary > 360)
        angle_drift_complementary -= 360;
    
    return angle_drift_complementary;
}

void IIM_Drift_Error(int8_t error_x, int8_t error_y, int8_t error_z) {

    if (IIM_Data_Gyro_Compensated_X < error_x && IIM_Data_Gyro_Compensated_X > -error_x) {
        if (!drift_x)
            IIM_Data_Gyro_Compensated_X = 0;

        drift_x = 0;
    } else {
        drift_x = 1;
    }

    if (IIM_Data_Gyro_Compensated_Y < error_y && IIM_Data_Gyro_Compensated_Y > -error_y) {
        if (!drift_y)
            IIM_Data_Gyro_Compensated_Y = 0;

        drift_y = 0;
    } else {
        drift_y = 1;
    }

    if (IIM_Data_Gyro_Compensated_Z < error_z && IIM_Data_Gyro_Compensated_Z > -error_z) {
        if (!drift_z)
            IIM_Data_Gyro_Compensated_Z = 0;

        drift_z = 0;
    } else {
        drift_z = 1;
    }
}

void IIM_Meas_Raw(uint16_t loop) {
    int8_t length;
    while (--loop) {
        IIM_Read_Data(IIM_Temp_Data_1, 14);
        length = sprintf((char*) Buff_IIM_send, "%d\t%d\t%d\t\t%d\t%d\t%d\r\n", iimraw.acc.x, iimraw.acc.y, iimraw.acc.z, iimraw.gyro.x, iimraw.gyro.y, iimraw.gyro.z);
        USB_printSlowUSB(Buff_IIM_send, length);
        __delay_ms(12);
    }
}

void IIM_Meas_Compensated(uint16_t loop) {
    int8_t length;
    while (--loop) {
        IIM_Read_Data(IIM_Temp_Data_1, 14);
        length = sprintf((char*) Buff_IIM_send, "%d\t%d\t%d\t\t%d\t%d\t%d\t%d\t%d\t%d\t\t%d\t%d\t%d\r\n", iimraw.acc.x, iimraw.acc.y, iimraw.acc.z, iimraw.gyro.x, iimraw.gyro.y, iimraw.gyro.z, IIM_Data_Acc_Compensated_X, IIM_Data_Acc_Compensated_Y, IIM_Data_Acc_Compensated_Z, IIM_Data_Gyro_Compensated_X, IIM_Data_Gyro_Compensated_Y, IIM_Data_Gyro_Compensated_Z);
        USB_printSlowUSB(Buff_IIM_send, length);
        __delay_ms(12);
    }
    length = sprintf((char*) Buff_IIM_send, "\r\n%d\t%d\t%d\t\t%d\t%d\t%d\r\n", IIM_Offset_Acc[0], IIM_Offset_Acc[1], IIM_Offset_Acc[2], IIM_Offset_Gyro[0], IIM_Offset_Gyro[1], IIM_Offset_Gyro[2]);
    USB_printSlowUSB(Buff_IIM_send, length);
    __delay_ms(12);
}

void IIM_Separate_Data(uint8_t *data) {
    IIM_Data_Temp = ((uint16_t) data[1] << 8) + ((uint16_t) data[2]);
    IIM_Data_Acc_X = ((uint16_t) data[3] << 8) + ((uint16_t) data[4]);
    IIM_Data_Acc_Y = ((uint16_t) data[5] << 8) + ((uint16_t) data[6]);
    IIM_Data_Acc_Z = ((uint16_t) data[7] << 8) + ((uint16_t) data[8]);
    IIM_Data_Gyro_X = ((uint16_t) data[9] << 8) + ((uint16_t) data[10]);
    IIM_Data_Gyro_Y = ((uint16_t) data[11] << 8) + ((uint16_t) data[12]);
    IIM_Data_Gyro_Z = ((uint16_t) data[13] << 8) + ((uint16_t) data[14]);
}

float IIM_Read_Data(uint16_t Reg_Name, uint8_t Byte_Count) {
    uint8_t bank = (Reg_Name >> 8);
    IIM_Bank_Set(bank);
    uint8_t Reg_Read_Name = IIM_Read_Operation(Reg_Name);
    uint8_t addr[16] = {(0xFF & Reg_Read_Name)};
    SS1OUT_SetLow();
    SPI1_Exchange8bitBuffer(&addr[0], (Byte_Count + 1), buff_SPI_read);
    SS1OUT_SetHigh();

    IIM_Separate_Data(&buff_SPI_read[0]);
   
    iimraw.acc.x = IIM_Data_Acc_X;
    iimraw.acc.y = IIM_Data_Acc_Y;
    iimraw.acc.z = IIM_Data_Acc_Z;
    iimraw.gyro.x = IIM_Data_Gyro_X;
    iimraw.gyro.y = IIM_Data_Gyro_Y;
    iimraw.gyro.z = IIM_Data_Gyro_Z;

    IIM_Data_Acc_Compensated_X = iimraw.acc.x - IIM_Offset_Acc[0];
    IIM_Data_Acc_Compensated_Y = iimraw.acc.y - IIM_Offset_Acc[1];
    IIM_Data_Acc_Compensated_Z = iimraw.acc.z - IIM_Offset_Acc[2];
    IIM_Data_Gyro_Compensated_X = iimraw.gyro.x - IIM_Offset_Gyro[0];
    IIM_Data_Gyro_Compensated_Y = iimraw.gyro.y - IIM_Offset_Gyro[1];
    IIM_Data_Gyro_Compensated_Z = iimraw.gyro.z - IIM_Offset_Gyro[2];

    IIM_Measured_Data.iim_temp = ((((float) *(&IIM_Data_Temp)) / 132.48) + 25.0);
    IIM_Measured_Data.iim_acc_x = -(((float) *(&IIM_Data_Acc_Compensated_X)) / 16384);
    IIM_Measured_Data.iim_acc_y = (((float) *(&IIM_Data_Acc_Compensated_Y)) / 16384);
    IIM_Measured_Data.iim_acc_z = (((float) *(&IIM_Data_Acc_Compensated_Z)) / 16384);
    IIM_Measured_Data.iim_gyro_x = (((float) *(&IIM_Data_Gyro_Compensated_X)) / 65.563);
    IIM_Measured_Data.iim_gyro_y = (((float) *(&IIM_Data_Gyro_Compensated_Y)) / 65.563);
    IIM_Measured_Data.iim_gyro_z = (((float) *(&IIM_Data_Gyro_Compensated_Z)) / 65.563);
    
    IIM_Data_ms2.iim_acc_x = IIM_Measured_Data.iim_acc_x * 9.81;
    IIM_Data_ms2.iim_acc_y = IIM_Measured_Data.iim_acc_y * 9.81;
    IIM_Data_ms2.iim_acc_z = IIM_Measured_Data.iim_acc_z * 9.81;
   
    return (IIM_Measured_Data.iim_temp);
}

void IIM_Get_Float_Data(IIM_DATA *data) {
    data->iim_temp = IIM_Measured_Data.iim_temp;
    data->iim_acc_x = IIM_Measured_Data.iim_acc_x;
    data->iim_acc_y = IIM_Measured_Data.iim_acc_y;
    data->iim_acc_z = IIM_Measured_Data.iim_acc_z;
    data->iim_gyro_x = IIM_Measured_Data.iim_gyro_x;
    data->iim_gyro_y = IIM_Measured_Data.iim_gyro_y;
    data->iim_gyro_z = IIM_Measured_Data.iim_gyro_z;
}