//magnetometer1MMC.c

#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include "magnetometer1MMC.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 MMC_Data_X = 0;
static uint16_t MMC_Data_Y = 0;
static uint16_t MMC_Data_Z = 0;
static MMC_DATA MMC_Measured_Data = {0, 0, 0};
static uint8_t MMC_Data_XYZ[9] = {0};
static int16_t MMC_Offset[3] = {-670, -19, -314};
static int16_t MMC_Delta[3] = {0};
static float MMC_Avg = 0;
static float MMC_Scale[3] = {1, 1, 1};
int8_t Buff_send [256];
int8_t Buff_Komp_send [256];
float Buff_float_send [128];
int16_t mmcraw_x;
int16_t mmcraw_y;
int16_t mmcraw_z;
int16_t MMC_Data_HI_Calib_X;
int16_t MMC_Data_HI_Calib_Y;
int16_t MMC_Data_HI_Calib_Z;
float MMC_Data_Compensated_X;
float MMC_Data_Compensated_Y;
float MMC_Data_Compensated_Z;

struct {
    _axesInt max;
    _axesInt min;
} MMC_CAL;

void MMC_Set(void) {
    MMC_Meas_Set(MMC_Contr_Reg_0, 0x08);
}

void MMC_Reset(void) {
    MMC_Meas_Set(MMC_Contr_Reg_0, 0x10);
}

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

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

    while (--loop) {
        MMC_Read_Data(MMC_X_out_0);

        // solve extremes
        // x
        if (mmcraw_x > MMC_CAL.max.x)
            MMC_CAL.max.x = mmcraw_x;
        if (mmcraw_x < MMC_CAL.min.x)
            MMC_CAL.min.x = mmcraw_x;

        // y
        if (mmcraw_y > MMC_CAL.max.y)
            MMC_CAL.max.y = mmcraw_y;
        if (mmcraw_y < MMC_CAL.min.y)
            MMC_CAL.min.y = mmcraw_y;

        // z
        if (mmcraw_z > MMC_CAL.max.z)
            MMC_CAL.max.z = mmcraw_z;
        if (mmcraw_z < MMC_CAL.min.z)
            MMC_CAL.min.z = mmcraw_z;

        length = sprintf((char*) Buff_send, "%d\t%d\t%d\r\n", mmcraw_x, mmcraw_y, mmcraw_z);
        USB_printSlowUSB(Buff_send, length);
        __delay_ms(12);
    }

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


    //write offset MMC x
    length = sprintf((char*) Buff_send, "\r\n%d\t%d\t%d\r\n\r\n%d\t%d\r\n\r\n%d\t%d\r\n", MMC_Offset[0], MMC_Offset[1], MMC_Offset[2], MMC_CAL.max.x, MMC_CAL.min.x, MMC_CAL.max.y, MMC_CAL.min.y);
    USB_printSlowUSB(Buff_send, length);
    __delay_ms(12);
}

void MMC_Meas_HI_Compensated(uint16_t loop) {
    int8_t length;
    while (--loop) {
        MMC_Read_Data(MMC_X_out_0);

        length = sprintf((char*) Buff_send, "%d\t%d\t%d\r\n", MMC_Data_HI_Calib_X, MMC_Data_HI_Calib_Y, MMC_Data_HI_Calib_Z);
        USB_printSlowUSB(Buff_send, length);
        __delay_ms(12);
    }
}

void MMC_Meas_Compensated(uint16_t loop) {
    int8_t length;
    while (--loop) {
        MMC_Read_Data(MMC_X_out_0);

        length = sprintf((char*) Buff_float_send, "%f\t%f\t%f\r\n", MMC_Data_Compensated_X, MMC_Data_Compensated_Y, MMC_Data_Compensated_Z);
        USB_printSlowUSB(Buff_float_send, length);
        __delay_ms(12);
    }
}

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

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

    while (--loop) {
        MMC_Read_Data(MMC_X_out_0);

        // solve extremes
        // x
        if (MMC_Data_HI_Calib_X > MMC_CAL.max.x)
            MMC_CAL.max.x = MMC_Data_HI_Calib_X;
        if (MMC_Data_HI_Calib_X < MMC_CAL.min.x)
            MMC_CAL.min.x = MMC_Data_HI_Calib_X;

        // y
        if (MMC_Data_HI_Calib_Y > MMC_CAL.max.y)
            MMC_CAL.max.y = MMC_Data_HI_Calib_Y;
        if (MMC_Data_HI_Calib_Y < MMC_CAL.min.y)
            MMC_CAL.min.y = MMC_Data_HI_Calib_Y;

        // z
        if (MMC_Data_HI_Calib_Z > MMC_CAL.max.z)
            MMC_CAL.max.z = MMC_Data_HI_Calib_Z;
        if (MMC_Data_HI_Calib_Z < MMC_CAL.min.z)
            MMC_CAL.min.z = MMC_Data_HI_Calib_Z;

        length = sprintf((char*) Buff_send, "%d\t%d\t%d\r\n", MMC_Data_HI_Calib_X, MMC_Data_HI_Calib_Y, MMC_Data_HI_Calib_Z);
        USB_printSlowUSB(Buff_send, length);
        __delay_ms(12);
    }

    MMC_Delta[0] = (MMC_CAL.max.x - MMC_CAL.min.x) / 2;
    MMC_Delta[1] = (MMC_CAL.max.y - MMC_CAL.min.y) / 2;
    MMC_Avg = (MMC_Delta[0] + MMC_Delta[1]) / 2.000;

    MMC_Scale[0] = MMC_Avg / MMC_Delta[0];
    MMC_Scale[1] = MMC_Avg / MMC_Delta[1];

    length = sprintf((char*) Buff_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", MMC_Scale[0], MMC_Scale[1], MMC_Scale[2], MMC_Delta[0], MMC_CAL.max.x, MMC_CAL.min.x, MMC_Delta[1], MMC_CAL.max.y, MMC_CAL.min.y);
    USB_printSlowUSB(Buff_send, length);
    __delay_ms(12);
}

void MMC_Meas_Azimuth(uint16_t loop) {
    int8_t length;
    while (--loop) {
        MMC_Read_Data(MMC_X_out_0);
 
        float headingm = 57.295779 * atan2(-MMC_Measured_Data.mmc_x, -MMC_Measured_Data.mmc_y);
        if(headingm < 0)
            headingm *= -1;
        else
            headingm = 360 - headingm;
        
        length = sprintf((char*) Buff_Komp_send, "%f\t%f\t%f\r\n", MMC_Measured_Data.mmc_x, MMC_Measured_Data.mmc_y, headingm);
        USB_printSlowUSB(Buff_Komp_send, length);
        __delay_ms(100);
    }
}

float MMC_Single_Meas_Azimuth(void) {
    MMC_Read_Data(MMC_X_out_0);

    float headingm = 57.295779 * atan2(-MMC_Measured_Data.mmc_x, -MMC_Measured_Data.mmc_y);
    if (headingm < 0)
        headingm *= -1;
    else
        headingm = 360 - headingm;

    return headingm;
}

void MMC_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, MMC_Address);
}

void MMC_Separate_XYZ(uint8_t *data) {
    MMC_Data_X = ((uint16_t) data[0] << 8) + data[1];
    MMC_Data_Y = ((uint16_t) data[2] << 8) + data[3];
    MMC_Data_Z = ((uint16_t) data[4] << 8) + data[5];
}

float MMC_Read_Data(uint8_t address) {
    I2C2_Write(&address, 1, MMC_Address);
    I2C2_Read(&MMC_Data_XYZ[0], 6, MMC_Address);

    MMC_Separate_XYZ(&MMC_Data_XYZ[0]);
    mmcraw_x = MMC_Data_X - 32768;
    mmcraw_y = MMC_Data_Y - 32768;
    mmcraw_z = MMC_Data_Z - 32768;

    MMC_Data_HI_Calib_X = mmcraw_x - MMC_Offset[0];
    MMC_Data_HI_Calib_Y = mmcraw_y - MMC_Offset[1];
    MMC_Data_HI_Calib_Z = mmcraw_z - MMC_Offset[2];

    MMC_Data_Compensated_X = MMC_Data_HI_Calib_X * MMC_Scale[0];
    MMC_Data_Compensated_Y = MMC_Data_HI_Calib_Y * MMC_Scale[0];
    MMC_Data_Compensated_Z = MMC_Data_HI_Calib_Z * MMC_Scale[0];

    MMC_Measured_Data.mmc_x = -((MMC_Data_Compensated_X) / 1024);
    MMC_Measured_Data.mmc_y = ((MMC_Data_Compensated_Y) / 1024);
    MMC_Measured_Data.mmc_z = ((MMC_Data_Compensated_Z) / 1024);

    return (MMC_Measured_Data.mmc_x);
}

void MMC_Get_Float_Data(MMC_DATA *data) {
    data->mmc_x = MMC_Measured_Data.mmc_x;
    data->mmc_y = MMC_Measured_Data.mmc_y;
    data->mmc_z = MMC_Measured_Data.mmc_z;
}

void MMC_init(void) {
    MMC_Meas_Set(MMC_ODR, 100);
    MMC_Meas_Set(MMC_Contr_Reg_0, MMC_Contr_Reg_0_Set);
    MMC_Meas_Set(MMC_Contr_Reg_2, 0x1C);
}

