/*
 * Author: Jake Goodwin
 * Date: 2023
 * Description: The implimentation file for the gy521 module, assumes TWI/I2c
 */
#include "gy521_driver.h"

/*
 * ############################
 * MACROS 
 * ############################
 */ 

/*DEBUG macro*/
#define DEBUG
#define debug_print(fmt, ...) \
            do { if (DEBUG) fprintf(stderr, fmt, __VA_ARGS__); } while (0)


/*Stuff for cmocka*/
#if UNIT_TESTING
extern void* _test_malloc(const size_t size, const char* file, const int line);
extern void* _test_calloc(const size_t number_of_elements, const size_t size,
                          const char* file, const int line);
extern void _test_free(void* const ptr, const char* file, const int line);

#define malloc(size) _test_malloc(size, __FILE__, __LINE__)
#define calloc(num, size) _test_calloc(num, size, __FILE__, __LINE__)
#define free(ptr) _test_free(ptr, __FILE__, __LINE__)

/*Mock assertions: redefines assert() calls*/
extern void mock_assert(const int result, const char* const expression, 
                        const char * const file, const int line);
#undef assert
#define assert(expression) \
    mock_assert((int)(expression), #expression, __FILE__, __LINE__);

#endif // UNIT_TESTING


/*
 * ############################
 * Helper/Private Functions
 * ############################
 */ 
    
uint8_t read_register(gy521_module *m, enum gy521_map reg)
{
    /*Load the data var with athe address of the register to read*/
    uint8_t data = reg;

    /*Indicate we want to read a value from the slave.*/
    gy521_twi_rx(m->slave_address, &data, 1);

    /*The value should now be in the data var*/
    return data;
}

void write_register(gy521_module *m, enum gy521_map reg, uint8_t data)
{
    uint8_t in_data = reg;
    gy521_twi_tx(m->slave_address, &in_data, 2);
}


/*
 * ############################
 * Function Definitions
 * ############################
 */

gy521_module* gy521_new(void)
{
    gy521_module *m = (gy521_module *)malloc(sizeof(gy521_module));
    m->slave_address = 0x0;
    return m;
}


void gy521_free(gy521_module *m)
{
    free(m);
}


_Bool gy521_init(gy521_module *m, uint8_t slave_address) {
    /*Check to make sure it's a valid address for this module*/
    if((slave_address != TWI_GY521_ADDR1) && (slave_address != TWI_GY521_ADDR2)) { 
        m->slave_address = 0x0;
        return 0;
    }

    /*Set the slave address of the module*/
    m->slave_address = slave_address;

    /*Make sure TWI is working and read the who_am_i register*/
    uint8_t data = read_register(m, who_am_i);
    
    if(data != m->slave_address) {
        return 0;
    }

    return 1;
}


void gy521_update_accel(gy521_module *m)
{
    /*We shift the High byte over by 8 for a u16*/
    m->accel.x = read_register(m, accel_xouth) <<8;
    m->accel.x |= read_register(m, accel_xoutl);
    
    m->accel.y = read_register(m, accel_youth) <<8;
    m->accel.y |= read_register(m, accel_youtl);
    
    m->accel.z = read_register(m, accel_zouth) <<8;
    m->accel.z |= read_register(m, accel_zoutl);
}

void gy521_update_gyro(gy521_module *m)
{
    /*We shift the High byte over by 8 for a u16*/
    m->gyro.x = read_register(m, gyro_xouth) <<8;
    m->gyro.x |= read_register(m, gyro_xoutl);
    
    m->gyro.y = read_register(m, gyro_youth) <<8;
    m->gyro.y |= read_register(m, gyro_youtl);

    m->gyro.z = read_register(m, gyro_zouth) <<8;
    m->gyro.z |= read_register(m, gyro_zoutl);
}


struct accel_values gy521_get_accel(struct gy521_module* m)
{
    struct accel_values s;
    s.x = m->accel.x;
    s.y = m->accel.y;
    s.z = m->accel.z;
    return s;
}


struct gyro_values gy521_get_gyro(struct gy521_module* m)
{
    struct gyro_values s;
    s.x = m->gyro.x;
    s.y = m->gyro.y;
    s.z = m->gyro.z;
    return s;
}


void enable_self_test_gyro(struct gy521_module* m)
{
    /*Set the values in the GYRO_CONFIG register*/
    /*According to the datasheet setting it for 250 is needed*/
    uint8_t reg = XG_ST + YG_ST + ZG_ST + FS_SEL_250;
    write_register(m, gyro_config, reg);
}

void disable_self_test_gyro(struct gy521_module* m)
{
    write_register(m, gyro_config, 0x00);
}

struct ft_vals self_test_ft_calculation(struct gy521_module* m)
{

    /*We make an array with 0 --> x, 1--> y, 2-->z*/
    float factory_trim_vals[3];
    uint8_t gyro_test[3] = {0};
    
    
    for(uint8_t i = 0; i < 3; i++) {
        /*Reads the registers for self testing the gyro*/
        gyro_test[i] = read_register(m, self_test_x + i) & 0x0F;

        /*Skip doing calculations if the test value is zero*/
        if(gyro_test[i] == 0){
            factory_trim_vals[i] = 0;
            continue;
        }
        else if(gyro_test[i] == 1) {
            factory_trim_vals[i] = 1;
        }
        else {
            factory_trim_vals[i] = 1.046;
            for(uint8_t j = 0; j < (gyro_test[i] - 1); j++){
                factory_trim_vals[i] *= 1.046;    
            }
        }
        factory_trim_vals[i] *= (25 * 131);
    } 

    struct ft_vals ft;
    ft.x = factory_trim_vals[0];
    ft.y = factory_trim_vals[1];
    ft.z = factory_trim_vals[2];

    return ft;
}

/*Self Test register functions*/
self_test_results gy521_self_test(struct gy521_module* m)
{

    /*0. Enable self test*/
    enable_self_test_gyro(m); 

    /*1. read the reg FT values*/ 
    /*2. calulate the FT using the formula from the datasheet*/
    struct ft_vals factory_trim = self_test_ft_calculation(m);
  
    /*3. Read the STR data from the gyro*/
    gy521_update_gyro(m);
    struct gyro_values STR = m->gyro;

    /*4. Disable the self test mode.*/
    disable_self_test_gyro(m);
    
    /*5. Now get the normal gyro data*/  
    gy521_update_gyro(m);

    /*6. Finish the STR calculation*/
    STR.x -= m->gyro.x;
    STR.y -= m->gyro.y;
    STR.z -= m->gyro.z;

    /*7. Now we calculate the change from the FT of the STR*/
    float testing_value = ( ((float)STR.x) - factory_trim.x) / factory_trim.x;
    if(testing_value > GYRO_MAX_ST || testing_value < GYRO_MIN_ST) {
        return gyro_failed;
    }

    testing_value = ( ((float)STR.y) - factory_trim.y) / factory_trim.y;
    if(testing_value > GYRO_MAX_ST || testing_value < GYRO_MIN_ST) {
        return gyro_failed;
    }

    testing_value = ( ((float)STR.z) - factory_trim.z) / factory_trim.z;
    if(testing_value > GYRO_MAX_ST || testing_value < GYRO_MIN_ST) {
        return gyro_failed;
    }

    /*Return that we passed the self test if it's within parameters*/
    return passed;
}