#include "ads1256.h"
#include "ads1256_constants.h"
#include "stdint.h"

static unsigned char MX_ch_code_p[5] =  {0, 0x01,0x54,0x23, 0x76};
static unsigned char MX_ch_code_n[5] =  {0, 0x10,0x45,0x32, 0x67};

static uint8_t PGA;
static uint8_t PGA_val = ADC_PGA_SET_INIT;

static uint8_t ADC_sps_var = ADC_SPS_SET_INIT;

static uint8_t meas_ptr = 0;
static uint8_t meas_case = MEAS_CASE_INIT;

static uint8_t meas_status = 0;
static int64_t meas[MEAS_BUF_SIZE] = {0};
static uint8_t ADC_state = 0;

void ADC_SET_CS(uint8_t state) {
    if(state) {
        GPIO_SET_BIT(ADC_CS_GPIO, ADC_CS_GPIO_PIN);
    } else {
        GPIO_RESET_BIT(ADC_CS_GPIO, ADC_CS_GPIO_PIN);
    }
}

void ADC_SET_SYNC(uint8_t state) {
    if(state) {
        GPIO_SET_BIT(ADC_SYNC_GPIO, ADC_SYNC_GPIO_PIN);
    } else {
        GPIO_RESET_BIT(ADC_SYNC_GPIO, ADC_SYNC_GPIO_PIN);
    }
}

void ADC_RESET_STATE(uint8_t state) {
    if(state) {
        GPIO_SET_BIT(ADC_RST_GPIO, ADC_RST_GPIO_PIN);
    } else {
        GPIO_RESET_BIT(ADC_RST_GPIO, ADC_RST_GPIO_PIN);
    }
}

uint8_t read_reg(uint8_t reg_name) {
    uint8_t reg_value = 0;
    ADC_SET_CS(0);

    SPI_WRITE(0x10 + reg_name);
    SPI_WRITE(0x00);

    reg_value = SPI_READ(&reg_value);
    ADC_SET_CS(1);

    return reg_value;
}

void write_reg(uint8_t reg_name, uint8_t cmd_byte) {
    ADC_SET_CS(0);
    SPI_WRITE(0x50 + reg_name);
    SPI_WRITE(0x00);
    SPI_WRITE(cmd_byte);

    for (int i = 0; i < 5000; i++) {}
    ADC_SET_CS(1);
}

void ADC_change_SPS(uint8_t SPS) {
    ADC_sps_var = SPS <= ADC_MAX_SPS ? SPS : ADC_MAX_SPS;
    ADC_init(1);
}

void ADC_set_SPS(uint8_t SPS) {
    write_reg(DRATE, SPS);
    while(GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN)) {}
}

void ADC_init(uint8_t buf_state) {
    ADC_RESET_STATE(1);
    ADC_SET_CS(1);
    ADC_SET_SYNC(1);

    for (uint32_t i = 0; i < 999999; i++) {}
    ADC_set_SPS(ADC_sps_var);
    
    buf_state = buf_state << 1;

    write_reg(STATUS, buf_state);
    ADC_set_SPS(ADC_sps_var);
    ADC_set_PGA(PGA_val);
    ADC_SelfCal();
    for(int i = 0; i < 5000; i++) {}
    ADC_state = MEAS_CASE_SET_CH;
    ADC_sync(1);
}

uint8_t ADC_get_SPS(void) {
    return ADC_sps_var;
}

void ADC_change_PGA(uint8_t PGA) {
    if (PGA <= ADC_MAX_RANGE) {
        PGA_val = PGA;
    }
    ADC_init(1);
}

uint8_t ADC_get_PGA(void) {
    return PGA_val;
}

void ADC_set_PGA(uint8_t PGA_val_new) {
    write_reg(ADCON, PGA_val_new);
    PGA = PGA_val_new;
    while(GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN)) {}
}

void ADC_SelfCal() {
    ADC_SET_CS(0);
    SPI_WRITE(0xf0);
    SPI_WRITE(0xf0);
    ADC_SET_CS(1);


    while(GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN)) {}
}

void ADC_sync(uint8_t state) {
    ADC_SET_SYNC(state);
}

void ADC_sync_trig(void) {
    ADC_sync(0);
    iter_delay(1000);
    ADC_sync(1);
    iter_delay(1000);
}

void ADC_setCh_p(uint8_t adcx, uint8_t chx) {
    ADC_SET_CS(0);
    iter_delay(1000);
    SPI_WRITE(0x50 + MUX);
    SPI_WRITE(0x00);
    SPI_WRITE(MX_ch_code_p[chx]);
    iter_delay(1000);
    ADC_SET_CS(1);
}

void ADC_setCh_n(uint8_t adcx, uint8_t chx) {
    ADC_SET_CS(0);
    iter_delay(1000);
    SPI_WRITE(0x50 + MUX);
    SPI_WRITE(0x00);
    SPI_WRITE(MX_ch_code_n[chx]);
    iter_delay(1000);
    ADC_SET_CS(1);
}

int64_t ADC_read(uint8_t ADC_number) {
    int64_t ADC_rbyte_sum = 0;
    ADC_SET_CS(0);
    for (int i = 0; i < 7000; i++) {}
    uint8_t read_cmd = 0x01;
    SPI_READ(&read_cmd);
    uint8_t read_cmd2 = 0x00;

    for (int i =0; i < 10000; i++) {}

    int64_t ADC_rbyte1 = SPI_READ(&read_cmd2);
    int64_t ADC_rbyte2 = SPI_READ(&read_cmd2);
    int64_t ADC_rbyte3 = SPI_READ(&read_cmd2);

    for (int i =0; i < 20000; i++) {}
    ADC_SET_CS(1);

    ADC_rbyte_sum = (ADC_rbyte1 << 24) | (ADC_rbyte2 << 16) | (ADC_rbyte3 << 8);
    ADC_rbyte_sum /= 256;
    ADC_rbyte_sum += 8388608;

    return ADC_rbyte_sum;
}

int64_t ADC_read_aver(uint8_t adcx, uint8_t chx) {
    float Vin = 0;
    int64_t cur_ADC_val = ADC_read(adcx);
    Vin = (float)((cur_ADC_val * ADC_VREF)/(8388607.0 * PGA));
    return (int64_t)((Vin + ADC_VIN_OFFSET)* uV);
}

void iter_delay(long delay) {
    for(uint32_t i = 0; i < delay; i++) { }
}

void ADC_set_pos_ch(uint8_t ch) {
    ADC_setCh_p(2, ch);
    ADC_sync_trig();
}

void ADC_set_neg_ch(uint8_t ch) {
    ADC_setCh_n(2, ch);
    ADC_sync_trig();
}

void ADC_revpol_read(int64_t *ADC_arrptr) {
    switch(meas_case) {
        case MEAS_CASE_INIT:
            meas_status = 0;
            meas_ptr = 0;
            meas_case = CH_1p;
        break;

        case CH_1p:
            switch(ADC_state) {
                case MEAS_CASE_SET_CH:
                    ADC_set_pos_ch(1);
                    ADC_state = MEAS_CASE_GET_CH_DATA;
                break;
                case MEAS_CASE_GET_CH_DATA:
                    if (GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN) == 0) {
                        meas[meas_ptr] = ADC_read_aver(2, meas_ptr);
                        meas_ptr++;
                        meas_case = CH_1n;
                        ADC_state = MEAS_CASE_SET_CH;
                    }
                break;
                default:
                    ADC_state = MEAS_CASE_SET_CH;
                break;
            }
        break;
        case CH_1n:
            switch(ADC_state) {
                case MEAS_CASE_SET_CH:
                    ADC_set_neg_ch(1);
                    ADC_state = MEAS_CASE_GET_CH_DATA;
                break;
                case MEAS_CASE_GET_CH_DATA:
                    if (GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN) == 0) {
                        meas[meas_ptr] = ADC_read_aver(2, meas_ptr);
                        meas_ptr++;
                        if(meas_ptr > CH1_INDEX_RANGE)
                            meas_case = CH_2p;								// next meas case
                        else
                            meas_case = CH_1p;								// next meas case	
                        ADC_state = MEAS_CASE_SET_CH;
                    }
                break;
                default:
                    ADC_state = MEAS_CASE_SET_CH;
                break;
            }
        break;
        case CH_2p:
            switch(ADC_state) {
                case MEAS_CASE_SET_CH:
                    ADC_set_pos_ch(2);
                    ADC_state = MEAS_CASE_GET_CH_DATA;
                break;
                case MEAS_CASE_GET_CH_DATA:
                    if (GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN) == 0) {
                        meas[meas_ptr] = ADC_read_aver(2, meas_ptr);
                        meas_ptr++;
                        meas_case = CH_2n;
                        ADC_state = MEAS_CASE_SET_CH;
                    }
                break;
                default:
                    ADC_state = MEAS_CASE_SET_CH;
                break;
            }
        break;
        case CH_2n:
            switch(ADC_state) {
                case MEAS_CASE_SET_CH:
                    ADC_set_neg_ch(2);
                    ADC_state = MEAS_CASE_GET_CH_DATA;
                break;
                case MEAS_CASE_GET_CH_DATA:
                    if (GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN) == 0) {
                        meas[meas_ptr] = ADC_read_aver(2, meas_ptr);
                        meas_ptr++;
                        if (meas_ptr > CH2_INDEX_RANGE)
                            meas_case = CH_3p;
                        else 
                            meas_case = CH_2p;
                        ADC_state = MEAS_CASE_SET_CH;
                    }
                break;
                default:
                    ADC_state = MEAS_CASE_SET_CH;
                break;
            }
        break;
        case CH_3p:
            switch(ADC_state) {
                case MEAS_CASE_SET_CH:
                    ADC_set_pos_ch(3);
                    ADC_state = MEAS_CASE_GET_CH_DATA;
                break;
                case MEAS_CASE_GET_CH_DATA:
                    if (GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN) == 0) {
                        meas[meas_ptr] = ADC_read_aver(2, meas_ptr);
                        meas_ptr++;
                        meas_case = CH_3n;
                        ADC_state = MEAS_CASE_SET_CH;
                    }
                break;
                default:
                    ADC_state = MEAS_CASE_SET_CH;
                break;
            }
        break;
        case CH_3n:
            switch(ADC_state) {
                case MEAS_CASE_SET_CH:
                    ADC_set_neg_ch(3);
                    ADC_state = MEAS_CASE_GET_CH_DATA;
                break;
                case MEAS_CASE_GET_CH_DATA:
                    if (GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN) == 0) {
                        meas[meas_ptr] = ADC_read_aver(2, meas_ptr);
                        meas_ptr++;
                        if (meas_ptr > CH3_INDEX_RANGE)
                            meas_case = CH_4p;
                        else 
                            meas_case = CH_3p;
                        ADC_state = MEAS_CASE_SET_CH;
                    }
                break;
                default:
                    ADC_state = MEAS_CASE_SET_CH;
                break;
            }
        break;
        case CH_4p:
            switch(ADC_state) {
                case MEAS_CASE_SET_CH:
                    ADC_set_pos_ch(4);
                    ADC_state = MEAS_CASE_GET_CH_DATA;
                break;
                case MEAS_CASE_GET_CH_DATA:
                    if (GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN) == 0) {
                        meas[meas_ptr] = ADC_read_aver(2, meas_ptr);
                        meas_ptr++;
                        meas_case = CH_4n;
                        ADC_state = MEAS_CASE_SET_CH;
                    }
                break;
                default:
                    ADC_state = MEAS_CASE_SET_CH;
                break;
            }
        break;
        case CH_4n:
            switch(ADC_state) {
                case MEAS_CASE_SET_CH:
                    ADC_set_neg_ch(4);
                    ADC_state = MEAS_CASE_GET_CH_DATA;
                break;
                case MEAS_CASE_GET_CH_DATA:
                    if (GPIO_GET_INPUT_STATE(ADC_RDY_GPIO, ADC_RDY_GPIO_PIN) == 0) {
                        meas[meas_ptr] = ADC_read_aver(2, meas_ptr);
                        meas_ptr++;
                        if (meas_ptr > CH4_INDEX_RANGE) {
                            for (int i = 0; i < MEAS_BUF_SIZE; i++) {
                                ADC_arrptr[i] = meas[i];
                            }
                            meas_case = MEAS_CASE_INIT;
                            meas_status = 1;
                        } else {
                            meas_case = CH_4p;
                        }
                        ADC_state = MEAS_CASE_SET_CH;
                    }
                break;
                default:
                    ADC_state = MEAS_CASE_SET_CH;
                break;
            }
        break;
    }
}

uint8_t ADC_meas_ready(void) { return meas_status; }
uint8_t ADC_get_currMeasCh(void) { return (uint8_t)meas_ptr; }