//magnetometer2BM.c

#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include "magnetometer2BM.h"
#include "mcc_generated_files/pin_manager.h"
#include "mcc_generated_files/i2c2.h"
#include "mcc_generated_files/usb/usb_device_cdc.h"

static uint16_t BM_Data_X = 0;
static uint16_t BM_Data_Y = 0;
static uint16_t BM_Data_Z = 0;
static BM_DATA BM_Measured_Data = {0, 0, 0};
static uint8_t BM_Data_XYZ[9] = {0};
static int16_t BM_Offset[3] = {-1109, -3019, -2205};
static int16_t BM_Delta[3] = {0};
static float BM_Avg = 0;
static float BM_Scale[3] = {1, 1, 1};
int8_t Buff_BM_send [256];
int8_t Buff_BM_Komp_send [256];
float Buff_BM_float_send [128];
int16_t bmraw_x;
int16_t bmraw_y;
int16_t bmraw_z;
int16_t BM_Data_HI_Calib_X;
int16_t BM_Data_HI_Calib_Y;
int16_t BM_Data_HI_Calib_Z;
float BM_Data_Compensated_X;
float BM_Data_Compensated_Y;
float BM_Data_Compensated_Z;

struct {
    _axesBMInt max;
    _axesBMInt min;
} BM_CAL;

void BM_Calib(uint16_t loop) {
    int8_t length;
    // init
    BM_CAL.max.x = -32767;
    BM_CAL.max.y = -32767;
    BM_CAL.max.z = -32767;

    BM_CAL.min.x = 32767;
    BM_CAL.min.y = 32767;
    BM_CAL.min.z = 32767;

    while (--loop) {
        BM_Read_Data(BM_X_out_0);

        // solve extremes
        // x
        if (bmraw_x > BM_CAL.max.x)
            BM_CAL.max.x = bmraw_x;
        if (bmraw_x < BM_CAL.min.x)
            BM_CAL.min.x = bmraw_x;

        // y
        if (bmraw_y > BM_CAL.max.y)
            BM_CAL.max.y = bmraw_y;
        if (bmraw_y < BM_CAL.min.y)
            BM_CAL.min.y = bmraw_y;

        // z
        if (bmraw_z > BM_CAL.max.z)
            BM_CAL.max.z = bmraw_z;
        if (bmraw_z < BM_CAL.min.z)
            BM_CAL.min.z = bmraw_z;

        length = sprintf((char*) Buff_BM_send, "%d\t%d\t%d\r\n", bmraw_x, bmraw_y, bmraw_z);
        USB_printSlowUSB(Buff_BM_send, length);
        __delay_ms(12);
    }

    BM_Offset[0] = (BM_CAL.max.x + BM_CAL.min.x) / 2;
    BM_Offset[1] = (BM_CAL.max.y + BM_CAL.min.y) / 2;
    BM_Offset[2] = (BM_CAL.max.z + BM_CAL.min.z) / 2;


    //write offset BM
    length = sprintf((char*) Buff_BM_send, "\r\n%d\t%d\t%d\r\n\r\n%d\t%d\r\n\r\n%d\t%d\r\n\r\n%d\t%d\r\n", BM_Offset[0], BM_Offset[1], BM_Offset[2], BM_CAL.max.x, BM_CAL.min.x, BM_CAL.max.y, BM_CAL.min.y, BM_CAL.max.z, BM_CAL.min.z);
    USB_printSlowUSB(Buff_BM_send, length);
    __delay_ms(12);
}

void BM_Meas_HI_Compensated(uint16_t loop) {
    int8_t length;
    while (--loop) {
        BM_Read_Data(BM_X_out_0);

        length = sprintf((char*) Buff_BM_send, "%d\t%d\t%d\r\n", BM_Data_HI_Calib_X, BM_Data_HI_Calib_Y, BM_Data_HI_Calib_Z);
        USB_printSlowUSB(Buff_BM_send, length);
        __delay_ms(12);
    }
}

void BM_Meas_Compensated(uint16_t loop) {
    int8_t length;
    while (--loop) {
        BM_Read_Data(BM_X_out_0);

        length = sprintf((char*) Buff_BM_float_send, "%f\t%f\t%f\r\n", BM_Data_Compensated_X, BM_Data_Compensated_Y, BM_Data_Compensated_Z);
        USB_printSlowUSB(Buff_BM_float_send, length);
        __delay_ms(12);
    }
}

void BM_SI_Calib(uint16_t loop) {
    int8_t length;
    // init
    BM_CAL.max.x = -32767;
    BM_CAL.max.y = -32767;
    BM_CAL.max.z = -32767;

    BM_CAL.min.x = 32767;
    BM_CAL.min.y = 32767;
    BM_CAL.min.z = 32767;

    while (--loop) {
        BM_Read_Data(BM_X_out_0);

        // solve extremes
        // x
        if (BM_Data_HI_Calib_X > BM_CAL.max.x)
            BM_CAL.max.x = BM_Data_HI_Calib_X;
        if (BM_Data_HI_Calib_X < BM_CAL.min.x)
            BM_CAL.min.x = BM_Data_HI_Calib_X;

        // y
        if (BM_Data_HI_Calib_Y > BM_CAL.max.y)
            BM_CAL.max.y = BM_Data_HI_Calib_Y;
        if (BM_Data_HI_Calib_Y < BM_CAL.min.y)
            BM_CAL.min.y = BM_Data_HI_Calib_Y;

        // z
        if (BM_Data_HI_Calib_Z > BM_CAL.max.z)
            BM_CAL.max.z = BM_Data_HI_Calib_Z;
        if (BM_Data_HI_Calib_Z < BM_CAL.min.z)
            BM_CAL.min.z = BM_Data_HI_Calib_Z;

        length = sprintf((char*) Buff_BM_send, "%d\t%d\t%d\r\n", BM_Data_HI_Calib_X, BM_Data_HI_Calib_Y, BM_Data_HI_Calib_Z);
        USB_printSlowUSB(Buff_BM_send, length);
        __delay_ms(12);
    }

    BM_Delta[0] = (BM_CAL.max.x - BM_CAL.min.x) / 2;
    BM_Delta[1] = (BM_CAL.max.y - BM_CAL.min.y) / 2;

    BM_Avg = (BM_Delta[0] + BM_Delta[1]) / 2.000;

    BM_Scale[0] = BM_Avg / BM_Delta[0];
    BM_Scale[1] = BM_Avg / BM_Delta[1];

    length = sprintf((char*) Buff_BM_send, "\r\n%f\t%f\t%f\r\n\r\n%d\t%d\t%d\r\n\r\n%d\t%d\t%d\r\n", BM_Scale[0], BM_Scale[1], BM_Scale[2], BM_Delta[0], BM_CAL.max.x, BM_CAL.min.x, BM_Delta[1], BM_CAL.max.y, BM_CAL.min.y);
    USB_printSlowUSB(Buff_BM_send, length);
    __delay_ms(12);
}

void BM_Meas_Azimuth(uint16_t loop) {
    int8_t length;
    while (--loop) {
        BM_Read_Data(BM_X_out_0);

        float headingb = 57.295779 * atan2(-BM_Measured_Data.bm_x, -BM_Measured_Data.bm_y);
        if (headingb < 0)
            headingb *= -1;
        else
            headingb = 360 - headingb;
        
        length = sprintf((char*) Buff_BM_Komp_send, "%f\t%f\t%f\r\n", BM_Measured_Data.bm_x, BM_Measured_Data.bm_y, headingb);
        USB_printSlowUSB(Buff_BM_Komp_send, length);
        __delay_ms(100);
    }
}

float BM_Single_Meas_Azimuth(void) {
    BM_Read_Data(BM_X_out_0);

    float headingb = 57.295779 * atan2(-BM_Measured_Data.bm_x, -BM_Measured_Data.bm_y);
    if (headingb < 0)
        headingb *= -1;
    else
        headingb = 360 - headingb;

    return headingb;
}

void BM_Meas_Set(uint8_t Contr_Reg_Num, uint8_t Contr_Reg_Num_Set) {
    uint8_t addr[2] = {Contr_Reg_Num, Contr_Reg_Num_Set};
    I2C2_Write(&addr[0], 2, BM_Address);
}

void BM_init(void) {
    BM_Meas_Set(BM_Contr_Reg_1, BM_Contr_Reg_1_Set);
    BM_Meas_Set(BM_Contr_Reg_4H, BM_Contr_Reg_4H_Set);
    BM_Meas_Set(BM_Contr_Reg_2, BM_Contr_Reg_2_Set);
    BM_Meas_Set(BM_Contr_Reg_3, BM_Contr_Reg_3_Set);
}

void BM_Separate_XYZ(uint8_t *data) {
    BM_Data_X = ((uint16_t) data[1] << 8) + data[0];
    BM_Data_Y = ((uint16_t) data[3] << 8) + data[2];
    BM_Data_Z = ((uint16_t) data[5] << 8) + data[4];
}

float BM_Read_Data(uint8_t address) {
    I2C2_Write(&address, 1, BM_Address);
    I2C2_Read(&BM_Data_XYZ[0], 6, BM_Address);

    BM_Separate_XYZ(&BM_Data_XYZ[0]);
    //int16_t* BM_Data_Signed = (int16_t*) &BM_Data_X;

    bmraw_x = BM_Data_X;
    bmraw_y = BM_Data_Y;
    bmraw_z = BM_Data_Z;

    BM_Data_HI_Calib_X = bmraw_x - BM_Offset[0];
    BM_Data_HI_Calib_Y = bmraw_y - BM_Offset[1];
    BM_Data_HI_Calib_Z = bmraw_z - BM_Offset[2];

    BM_Data_Compensated_X = BM_Data_HI_Calib_X * BM_Scale[0];
    BM_Data_Compensated_Y = BM_Data_HI_Calib_Y * BM_Scale[0];
    BM_Data_Compensated_Z = BM_Data_HI_Calib_Z * BM_Scale[0];

    BM_Measured_Data.bm_x = -((( BM_Data_Compensated_X) * 0.042)*0.01);
    BM_Measured_Data.bm_y = ((( BM_Data_Compensated_Y) * 0.042)*0.01);
    BM_Measured_Data.bm_z = ((( BM_Data_Compensated_Z) * 0.042)*0.01);
    return (BM_Measured_Data.bm_x);
}

void BM_Get_Float_Data(BM_DATA *data) {
    data->bm_x = BM_Measured_Data.bm_x;
    data->bm_y = BM_Measured_Data.bm_y;
    data->bm_z = BM_Measured_Data.bm_z;
}

