#include <stdio.h>
#include <stdarg.h>
#include <stddef.h>
#include <stdint.h>
#include <setjmp.h>
#include <stdlib.h>
#include <cmocka.h>
#include "gy521_driver.h"

#define WRITE_BIT   (1<<7)
#define     REG_SIZE    32 

typedef struct reg_addr{
    uint8_t dev_addr;
    uint8_t addr;
    uint8_t value;
}reg_addr;

reg_addr reg_addr_arr[REG_SIZE] = {{0x0, 0x0, 0x0}};
uint8_t idx = 0;

void mock_twi_tx(uint8_t slave_addr, uint8_t *data, uint8_t size)
{
    check_expected(slave_addr);
    check_expected_ptr(data);
    check_expected(size);

}



/* Fake Object for TWI_TX*/
void fake_twi_tx(uint8_t slave_addr, uint8_t *data, uint8_t size)
{
    /*read the device address.*/
    reg_addr_arr[idx].dev_addr = slave_addr;

    /*Save the register it's trying to access.*/
    reg_addr_arr[idx].addr = *data;

    /*Save the data it's sending to that address.*/
    reg_addr_arr[idx].value = *(data + 1);

    /*Post increment the global index*/
    idx++;
    if(idx > REG_SIZE){
        idx = 0;
    }
    
    /*Make use of recursion if there is still more data*/
    if((size - 2) > 0) {
        fake_twi_tx(slave_addr, data + 2, size - 2);
    } 
}

/* Fake Object for TWI_RX*/
void fake_twi_rx(uint8_t slave_addr, uint8_t *data, uint8_t size)
{

    /*Read the device address*/
    reg_addr_arr[idx].dev_addr = slave_addr;

    /*Put down the register address it's trying to read.*/ 
    reg_addr_arr[idx].addr = *data;
    
    /*Fill it with the data from the register*/
    *data = reg_addr_arr[idx].value;

    /*Post increment the global index*/
    idx++;
    if(idx > REG_SIZE){
        idx = 0;
    }
}



/*Setup the fake twi*/
void (*gy521_twi_tx)(uint8_t, uint8_t*, uint8_t) = &fake_twi_tx;
void (*gy521_twi_rx)(uint8_t, uint8_t*, uint8_t) = &fake_twi_rx;

/*
 * ############################
 * Helper functions
 * ############################
 */ 



void print_reg_arr(void)
{
    printf("FAKE REGISTERS::\n");
    for(uint8_t i = 0; i < REG_SIZE; i++){
        print_error("%d:: val: %d, addr: %d, dev_addr: %d\n",
                i, 
                reg_addr_arr[i].value,
                reg_addr_arr[i].addr,
                reg_addr_arr[i].dev_addr);
    }
}

void clear_twi(void)
{
    idx = 0;
    for(int i = 0; i < REG_SIZE; i++){
        reg_addr_arr[i].value = 0;
        reg_addr_arr[i].addr = 0;
        reg_addr_arr[i].dev_addr = 0;
    }
}


static int setup_gy521_instance(void **state)
{
    clear_twi();
    *state = gy521_new();
    assert_non_null(*state);
    return 0;
}

static int teardown_gy521_instance(void **state)
{
    gy521_free(*state);
    clear_twi();
    return 0;
}


/*
 * ############################
 * TESTS
 * ############################
 */ 

/* Tests the donothing function */ 
static void test_gy521_init(void **state) { 
    gy521_module *m = gy521_new();
    assert_false(gy521_init(m, 0x0));
    assert_false(gy521_init(m, 0x67));
    assert_false(gy521_init(m, 0x6A));

    /*Check for it's confirmation of the right twi device*/
    assert_false(gy521_init(m, TWI_GY521_ADDR1));

    /*Fill the address and the value registers with correct response value*/
    /*The zero element get's read on init of the module*/
    reg_addr_arr[0].addr = TWI_GY521_ADDR1;
    reg_addr_arr[0].value = TWI_GY521_ADDR1;
    idx = 0;

    /*Check that it worked*/
    assert_true(gy521_init(m, TWI_GY521_ADDR1));
    assert_true(TWI_GY521_ADDR1 == reg_addr_arr[0].dev_addr); 


    reg_addr_arr[0].addr = TWI_GY521_ADDR2;
    reg_addr_arr[0].value = TWI_GY521_ADDR2;
    idx = 0;

    assert_true(gy521_init(m, TWI_GY521_ADDR2));
    assert_true(TWI_GY521_ADDR2 == reg_addr_arr[0].dev_addr); 

    gy521_free(m);
 
}


static void test_gy521_update_accel(void **sate)
{
    /*check it reads the accel registers*/
    gy521_module *m = gy521_new();
    reg_addr_arr[0].addr = TWI_GY521_ADDR1;
    gy521_init(m, TWI_GY521_ADDR1);
    
    /*Setup the fake accel values.*/
    for(uint8_t i = 0; i < 6; i++) {
        reg_addr_arr[i].value = i * 8;
    }
  
    /*Zero the global index for the TWI*/ 
    idx = 0;  
    gy521_update_accel(m); 

    /*Ensure the correct registers are read*/
    _Bool is_correct = 1;
    assert_true(is_correct);
    for(uint8_t i = 0; i < 6; i++){
        /*The starting address of the registers is 59 and goes to 64*/ 
        if((59 + i) != reg_addr_arr[i].addr) {
            is_correct = 0;
        }
    }
    assert_true(is_correct);     

    /*Check that the values are assembled correctly*/
    struct accel_values accel = gy521_get_accel(m);

    /*Make sure to bitshift by 8 and recomine the two u8 into a single u16*/
    assert_true(accel.x == ((reg_addr_arr[0].value<<8) | reg_addr_arr[1].value));
    assert_true(accel.y == ((reg_addr_arr[2].value<<8) | reg_addr_arr[3].value));
    assert_true(accel.z == ((reg_addr_arr[4].value<<8) | reg_addr_arr[5].value));

    gy521_free(m);
}


static void test_gy521_update_gyro(void **sate)
{
    /*check it reads the gyro registers*/
    gy521_module *m = gy521_new();
    reg_addr_arr[0].addr = TWI_GY521_ADDR1;
    gy521_init(m, TWI_GY521_ADDR1);
    
    /*Setup the fake gyro values.*/
    for(uint8_t i = 1; i < 7; i++) {
        reg_addr_arr[i].value = i * 8;
    }
  
    /*Zero the global index for the TWI*/ 
    idx = 0;  
    gy521_update_gyro(m); 

    /*Ensure the correct registers are read*/
    _Bool is_correct = 1;
    assert_true(is_correct);
    for(uint8_t i = 0; i < 6; i++){
        /*The starting address of the registers is 67 and goes to 72*/ 
        if((67 + i) != reg_addr_arr[i].addr) {
            is_correct = 0;
        }
    }
    assert_true(is_correct);     

    /*Check that the values are assembled correctly*/
    struct gyro_values gyro = gy521_get_gyro(m);

    /*Make sure to bitshift by 8 and recomine the two u8 into a single u16*/
    assert_true(gyro.x == ((reg_addr_arr[0].value<<8) | reg_addr_arr[1].value));
    assert_true(gyro.y == ((reg_addr_arr[2].value<<8) | reg_addr_arr[3].value));
    assert_true(gyro.z == ((reg_addr_arr[4].value<<8) | reg_addr_arr[5].value));

    gy521_free(m);
}


static void test_gy521_self_test_gyro(void **sate)
{
    /*Create instance of struct.*/  
    gy521_module *m  = gy521_new();
    reg_addr_arr[0].addr = TWI_GY521_ADDR1;
    gy521_init(m, TWI_GY521_ADDR1);
    
    /*Zero the global index for the twi*/
    clear_twi(); 
    
    /*Setup the full-scale range to +-250dps*/
    
    /*Load up the gyro regs with passing test values*/
    /*Load up the selftest regs with passing test values.*/
    /*Load up the gyro regs for disabled ST reads*/
    for(uint8_t i = 1; i < 17; i++) {   /*(6regs * 2) + 3ST regs = 15*/
        reg_addr_arr[i].value = 2; 
    }


    self_test_results result = gy521_self_test(m); 
    assert_false(result);   /*The value should be zero, aka zero faults*/

    /*Check to make sure it can fail.*/
    clear_twi();
    for(uint8_t i = 1; i < 5; i++){
        reg_addr_arr[i].value = 2;
    }
    for(int i = 4; i < 9; i++){
        reg_addr_arr[i].value = 255;
    }
    for(int i = 9; i < 17; i++){
        reg_addr_arr[i].value = 2;
    }
        
    result = gy521_self_test(m);


    /*Print out the register values*/
    print_reg_arr();

    assert_true(result); 

    /*Free the struct*/
    gy521_free(m);
}


static void test_gy521_self_test_accel(void **sate)
{
    /*Create instance of struct.*/  
    gy521_module *m  = gy521_new();
    reg_addr_arr[0].addr = TWI_GY521_ADDR1;
    gy521_init(m, TWI_GY521_ADDR1);
    
    /*Zero the global index for the twi*/
    clear_twi(); 
    
    /*Setup the full-scale range to +-250dps*/
    
    /*Load up the gyro regs with passing test values*/
    /*Load up the selftest regs with passing test values.*/
    /*Load up the gyro regs for disabled ST reads*/
    for(uint8_t i = 1; i < 17; i++) {   /*(6regs * 2) + 3ST regs = 15*/
        reg_addr_arr[i].value = 2; 
    }

    self_test_results result = gy521_self_test(m); 
    assert_false(result);   /*The value should be zero, aka zero faults*/

    /*Check to make sure it can fail.*/
    clear_twi();
    for(uint8_t i = 1; i < 5; i++){
        reg_addr_arr[i].value = 2;
    }
    for(int i = 4; i < 9; i++){
        reg_addr_arr[i].value = 255;
    }
    for(int i = 9; i < 17; i++){
        reg_addr_arr[i].value = 2;
    }
        
    result = gy521_self_test(m);


    /*Print out the register values*/
    print_reg_arr();

    assert_true(result); 

    /*Free the struct*/
    gy521_free(m);
}


/*Mock Object/function for TWI RX*/


static void test_gy521_testing(void **state)
{
    assert_false(1);
}

static void test_gyro_update(void **state)
{
    assert_true(1);
}

static void test_enable_self_test_gyro(void **state)
{
    //expect_value(func, param, val);
    
    /*The read register function should be called with */
    expect_value(read_register, reg, 27);
    enable_self_test_gyro(*state);
}


int main(void)
{
    int result = 0;

    /*For testing TWI/I2C related stuff*/
    const struct CMUnitTest twi_group[] = {
        cmocka_unit_test_setup_teardown(test_gy521_testing, setup_gy521_instance, teardown_gy521_instance),
        cmocka_unit_test_setup_teardown(test_gyro_update, setup_gy521_instance, teardown_gy521_instance),
        cmocka_unit_test_setup_teardown(test_enable_self_test_gyro, setup_gy521_instance, teardown_gy521_instance),
    }; 
    const struct CMUnitTest tests[] = {
        cmocka_unit_test(test_gy521_init),
        cmocka_unit_test(test_gy521_update_accel),
        cmocka_unit_test(test_gy521_update_gyro),
        cmocka_unit_test(test_gy521_self_test_gyro),
        cmocka_unit_test(test_gy521_self_test_accel),
    };
    //result = cmocka_run_group_tests(twi_group, setup_gy521_instance, teardown_gy521_instance); 
    result = cmocka_run_group_tests(twi_group, NULL, NULL);
    result = cmocka_run_group_tests(tests, NULL, NULL);

    return result;
}