#include "ms5611.h"
#include "device.h"
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_i2c.h"

struct data_ms5611_s data_ms5611 __attribute__((section(".bss_ccm")));

#if 0
void ms5611_init(void)
{
    GPIO_InitTypeDef  GPIO_InitStructure;
    I2C_InitTypeDef I2C_InitStructure;

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);

    /* config ms5611 pin */
    GPIO_InitStructure.GPIO_Pin = MS5611_PIN_SCL | MS5611_PIN_SDA;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(MS5611_PORT, &GPIO_InitStructure);
    GPIO_PinAFConfig(MS5611_PORT, GPIO_PinSource8, GPIO_AF_I2C1);
    GPIO_PinAFConfig(MS5611_PORT, GPIO_PinSource9, GPIO_AF_I2C1);

    /* config I2C function */
    I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
    I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
    I2C_InitStructure.I2C_OwnAddress1 = 0;
    I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
    I2C_InitStructure.I2C_ClockSpeed = 100 * 1000;
    I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
    I2C_Init(I2Cx, &I2C_InitStructure);
    I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR | I2C_IT_BUF, DISABLE);
    I2C_Cmd(I2Cx, ENABLE);
}

ErrorStatus ms5611_i2c_wait_event(uint32_t i2c_event)
{
    uint32_t timeout = 10000;

    while (I2C_CheckEvent(I2Cx, i2c_event) != SUCCESS) {
        timeout--;
        if (timeout == 0) {
            I2C_GenerateSTOP(I2Cx, ENABLE);
            return ERROR;
        }
    }
    return SUCCESS;
}

ErrorStatus ms5611_i2c_write_data(uint8_t *data, uint16_t len)
{
    ErrorStatus status;
    uint16_t idx = 0;

    I2C_GenerateSTART(I2Cx, ENABLE);
    status = ms5611_i2c_wait_event(I2C_EVENT_MASTER_MODE_SELECT);
    if (status != SUCCESS) {
        return ERROR;
    }
    I2C_Send7bitAddress(I2Cx, MS5611_ADDR, I2C_Direction_Transmitter);
    status = ms5611_i2c_wait_event(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
    if (status != SUCCESS) {
        return ERROR;
    }
    while (idx < len) {
        I2C_SendData(I2Cx, data[idx]);
        if (idx == len - 1) {
            status = ms5611_i2c_wait_event(I2C_EVENT_MASTER_BYTE_TRANSMITTED);
        } else {
            status = ms5611_i2c_wait_event(I2C_EVENT_MASTER_BYTE_TRANSMITTING);
        }
        idx++;
        if (status != SUCCESS) {
            return ERROR;
        }
    }
    I2C_GenerateSTOP(I2Cx, ENABLE);
    return SUCCESS;
}

ErrorStatus ms5611_i2c_read_data(uint8_t *data, uint16_t len)
{
    ErrorStatus status;
    uint16_t idx = 0;

    I2C_GenerateSTART(I2Cx, ENABLE);
    status = ms5611_i2c_wait_event(I2C_EVENT_MASTER_MODE_SELECT);
    if (status != SUCCESS) {
        return ERROR;
    }
    I2C_Send7bitAddress(I2Cx, MS5611_ADDR, I2C_Direction_Receiver);
    status = ms5611_i2c_wait_event(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED);
    if (status != SUCCESS) {
        return ERROR;
    }
    while (idx < len) {
        if (idx == len - 1) {
            I2C_GenerateSTOP(I2Cx, ENABLE);
        }
        status = ms5611_i2c_wait_event(I2C_EVENT_MASTER_BYTE_RECEIVED);
        if (status != SUCCESS) {
            return ERROR;
        }
        data[idx++] = I2C_ReceiveData(I2Cx);
    }
    return SUCCESS;
}

#else

static inline void ms5611_i2c_scl_low(void)
{
    GPIO_ResetBits(MS5611_PORT, MS5611_PIN_SCL);
}

static inline void ms5611_i2c_scl_high(void)
{
    GPIO_SetBits(MS5611_PORT, MS5611_PIN_SCL);
}

static inline void ms5611_i2c_sda_low(void)
{
    GPIO_ResetBits(MS5611_PORT, MS5611_PIN_SDA);
}

static inline void ms5611_i2c_sda_high(void)
{
    GPIO_SetBits(MS5611_PORT, MS5611_PIN_SDA);
}

static inline uint8_t ms5611_i2c_scl_read(void)
{
    return (GPIO_ReadInputDataBit(MS5611_PORT, MS5611_PIN_SCL) == Bit_RESET) ? 0 : 1;
}

static inline uint8_t ms5611_i2c_sda_read(void)
{
    return (GPIO_ReadInputDataBit(MS5611_PORT, MS5611_PIN_SDA) == Bit_RESET) ? 0 : 1;
}

void ms5611_i2c_delay(uint32_t cnt)
{
    while (cnt--) {
        asm volatile (
            "nop\n"
            "nop\n"
            "nop\n"
            "nop\n"
            "nop\n"
            "nop\n"
            "nop\n"
            "nop\n"
            "nop\n"
            : /* output */
            : /* input */
            : /* destruct description */
        );
    }
}

void ms5611_i2c_start(void)
{
    for (int i = 0; i < 9; i++) {
        if (ms5611_i2c_sda_read() == 0) {
            ms5611_i2c_scl_low();
            ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_1);
            ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_2);
            ms5611_i2c_scl_high();
            ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_1);
            ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_2);
        } else {
            break;
        }
    }
    ms5611_i2c_delay(MS5611_DELAY_CNT_START_1);
    ms5611_i2c_sda_low();
    ms5611_i2c_delay(MS5611_DELAY_CNT_START_2);
}

void ms5611_i2c_stop(void)
{
    for (int i = 0; i < 9; i++) {
        ms5611_i2c_scl_low();
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_1);
        ms5611_i2c_sda_high(); /* release SDA line */
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_2);
        if (ms5611_i2c_sda_read() == 0) {
            /* SDA line is pulled low by slave */
            ms5611_i2c_scl_high();
            ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_1);
            ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_2);
        } else {
            break;
        }
    }
    ms5611_i2c_sda_low();
    ms5611_i2c_delay(MS5611_DELAY_CNT_STOP_1);
    ms5611_i2c_scl_high();
    ms5611_i2c_delay(MS5611_DELAY_CNT_STOP_2);
    ms5611_i2c_sda_high();
}

uint8_t ms5611_i2c_write_byte(uint8_t data)
{
    uint8_t nack;

    for (int i = 0; i < 8; i++) {
        ms5611_i2c_scl_low();
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_1);
        if (data & 0x80) {
            ms5611_i2c_sda_high();
        } else {
            ms5611_i2c_sda_low();
        }
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_2);
        ms5611_i2c_scl_high();
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_1);
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_2);
        data <<= 1;
    }
    /* check ack */
    ms5611_i2c_scl_low();
    ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_1);
    ms5611_i2c_sda_high(); /* release SDA line */
    ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_2);
    ms5611_i2c_scl_high();
    ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_1);
    if (ms5611_i2c_sda_read()) {
        nack = 1; /* nack */
    } else {
        nack = 0; /* ack */
    }
    ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_2);
    return nack;
}

uint8_t ms5611_i2c_read_byte(uint8_t nack)
{
    uint8_t data = 0;

    for (int i = 0; i < 8; i++) {
        data <<= 1;
        ms5611_i2c_scl_low();
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_1);
        ms5611_i2c_sda_high(); /* release SDA line */
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_2);
        ms5611_i2c_scl_high();
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_1);
        if (ms5611_i2c_sda_read() == 1) {
            data |= 1;
        }
        ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_2);
    }
    /* ack or nack */
    ms5611_i2c_scl_low();
    ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_1);
    if (nack) {
        ms5611_i2c_sda_high();
    } else {
        ms5611_i2c_sda_low();
    }
    ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_MODIFY_2);
    ms5611_i2c_scl_high();
    ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_1);
    ms5611_i2c_delay(MS5611_DELAY_CNT_DATA_SAMPLE_2);
    return data;
}

ErrorStatus ms5611_i2c_write_data(uint8_t *data, uint16_t len)
{
    uint8_t nack;

    ms5611_i2c_start();
    nack = ms5611_i2c_write_byte(MS5611_ADDR);
    if (nack) {
        ms5611_i2c_stop();
        return ERROR;
    }
    for (uint16_t i = 0; i < len; i++) {
        nack = ms5611_i2c_write_byte(data[i]);
        if (nack) {
            ms5611_i2c_stop();
            return ERROR;
        }
    }
    ms5611_i2c_stop();
    return SUCCESS;
}

ErrorStatus ms5611_i2c_read_data(uint8_t *data, uint16_t len)
{
    uint8_t nack;

    ms5611_i2c_start();
    nack = ms5611_i2c_write_byte(MS5611_ADDR | 1);
    if (nack) {
        ms5611_i2c_stop();
        return ERROR;
    }
    for (uint16_t i = 0; i < len; i++) {
        if (i >= (len - 1)) {
            data[i] = ms5611_i2c_read_byte(1); /* master nack */
        } else {
            data[i] = ms5611_i2c_read_byte(0); /* master ack */
        }
    }
    ms5611_i2c_stop();
    return SUCCESS;
}

void ms5611_init(void)
{
    GPIO_InitTypeDef  GPIO_InitStructure;

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

    /* config ms5611 pin */
    GPIO_InitStructure.GPIO_Pin = MS5611_PIN_SCL | MS5611_PIN_SDA;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(MS5611_PORT, &GPIO_InitStructure);

    data_ms5611.flag = MS5611_FLAG_IDLE;
    ms5611_write_cmd(MS5611_CMD_RESET);
    ms5611_read_prom();
}

#endif

uint8_t ms5611_crc4_check_valid(uint16_t *prom)
{
    uint16_t i;                /* simple counter */
    uint16_t n_rem = 0;       /* crc reminder. QB: 0x0000 at first */
    uint16_t prom7 = prom[7]; /* original value of the crc */
    uint8_t n_bit;

    /* CRC byte is replaced by 0 QB: P[n] is saved as each member with 2bytes */
    prom[7] = 0xFF00 & prom[7];
    /* operation is performed on bytes. QB: Prom[] is divided into 16 parts */
    /* QB: u16(2 bytes) to u8(1 byte & 16 parts) indeed */
    for (i = 0; i < 16; i++) {
        /* choose LSB or MSB QB: MSB: Most Significant Bit */
        if (i % 2 == 1) {/* QB: division operation */
            n_rem ^= (uint16_t)((prom[i >> 1]) & 0x00FF);
        } else {
            n_rem ^= (uint16_t)(prom[i >> 1] >> 8);
        }
        /* QB: CRC processing. Totally, it mod 0b10011 */
        for (n_bit = 8; n_bit > 0; n_bit--) {
            /* Sign: The highest bit is 1 */
            if (n_rem & (0x8000)) {
                /* 0b0011... So the relationship between 0x3000 & hand calculator? */
                n_rem = (n_rem << 1) ^ 0x3000;
            } else {
                n_rem = (n_rem << 1);
            }
        }
    }
    /* QB: final 4-bit reminder is CRC code */
    n_rem = (0x000F & (n_rem >> 12));
    /* restore the crc_read to its original place */
	prom[7] = prom7;
    if (n_rem == (prom7 & 0xF)) {
        return 1;
    } else {
        return 0;
    }
}

void ms5611_read_prom(void)
{
    uint8_t i;
    uint8_t cmd;
    uint8_t retry = 10;
    ErrorStatus status;

    data_ms5611.prom_error = 0;
    while (retry--) {
        for (i = 0; i < 8; i++) {
            cmd = 0xA0 + i * 2;
            status = ms5611_i2c_write_data(&cmd, 1);
            if (status != SUCCESS) {
                break;
            }
            status = ms5611_i2c_read_data((uint8_t *)(data_ms5611.prom + i), 2);
        }
        if (status == SUCCESS) {
            data_ms5611.prom_error = 0;
            break;
        } else {
            data_ms5611.prom_error++;
        }
    }
    for (i = 0; i < sizeof(data_ms5611.prom) / sizeof(data_ms5611.prom[0]); i++) {
        uint16_t temp1 = (data_ms5611.prom[i] >> 8) & 0x00FF;
        uint16_t temp2 = (data_ms5611.prom[i] << 8) & 0xFF00;
        data_ms5611.prom[i] = temp1 | temp2;
    }
    if (data_ms5611.prom_error == 0) {
        if (0 == ms5611_crc4_check_valid(data_ms5611.prom)) {
            data_ms5611.prom_error = ~0;
        }
    }
}

void ms5611_write_cmd(uint8_t cmd)
{
    ms5611_i2c_write_data(&cmd, 1);
}

ErrorStatus ms5611_read_data(uint32_t *data)
{
    ErrorStatus status;
    uint8_t arr[3] = { 0 };

    status = ms5611_i2c_read_data(arr, 3);
    if (status != SUCCESS) {
        return ERROR;
    } else {
        *data = (arr[0] << 16) | (arr[1] << 8) | arr[2];
        return SUCCESS;
    }
}

void ms5611_monitor(void)
{
    int64_t dT = 0, TEMP = 0, T2 = 0, OFF2 = 0, SENS2 = 0;
    int64_t OFF = 0, SENS = 0, P_temp = 0;
    int32_t P = 0;
    int64_t temp1;

    temp1 = (uint64_t)data_ms5611.prom[5] * 256;
    if (data_ms5611.data[1] > temp1) {
        dT = data_ms5611.data[1] - temp1;
    } else {
        dT = temp1 - data_ms5611.data[1];
        dT *= -1;
    }

    TEMP = 2000 + (dT >> 8) * ((int64_t)data_ms5611.prom[6] >> 8) / 128;
    if (TEMP < -4000) {
        TEMP = -4000;
    }
    if (TEMP > 8500) {
        TEMP = 8500;
    }

    OFF = ((int64_t)data_ms5611.prom[2] * 65536) + ((int64_t)data_ms5611.prom[4]* dT) / 128;
    SENS = ((int64_t)data_ms5611.prom[1] * 32768) + ((int64_t)data_ms5611.prom[3] * dT) / 256;

    if (TEMP < 2000) {
        T2 = (dT / 65536) * (dT / 32768);
        OFF2 = (5 * (TEMP - 2000) * (TEMP - 2000)) / 2;
        SENS2 = (5 * (TEMP - 2000) * (TEMP - 2000)) / 4;
        if (TEMP < -1500) {
            OFF2 += 7 * (TEMP + 1500) * (TEMP + 1500);
            SENS2 += 11 * (TEMP + 1500) * (TEMP + 1500) / 2;
        }
    } else {
        T2 = 0;
        OFF2 = 0;
        SENS2 = 0;
    }
    TEMP = TEMP - T2;
    OFF = OFF - OFF2;
    SENS = SENS - SENS2;
    P_temp = ((int64_t)data_ms5611.data[0] * SENS / 2097152 - OFF) / 32768;
    P = (int32_t)P_temp / 10;

    data_ms5611.pressure = P;
}

void ms5611_loop(void)
{
    static uint32_t ms = 0;

    if (system_tick_cnt < (ms + 50)) {
        return;
    }
    ms = system_tick_cnt;
    if (data_ms5611.flag == MS5611_FLAG_IDLE) {
        ms5611_write_cmd(MS5611_CMD_D1_OSR4096);
        data_ms5611.flag = MS5611_FLAG_CMD_D1;
    } else if (data_ms5611.flag == MS5611_FLAG_CMD_D1) {
        ms5611_write_cmd(MS5611_CMD_ADC_READ);
        data_ms5611.flag = MS5611_FLAG_READ_D1;
    } else if (data_ms5611.flag == MS5611_FLAG_READ_D1) {
        ms5611_read_data(&data_ms5611.data[0]);
        data_ms5611.flag = MS5611_FLAG_END_D1;
    } else if (data_ms5611.flag == MS5611_FLAG_END_D1) {
        ms5611_write_cmd(MS5611_CMD_D2_OSR4096);
        data_ms5611.flag = MS5611_FLAG_CMD_D2;
    } else if (data_ms5611.flag == MS5611_FLAG_CMD_D2) {
        ms5611_write_cmd(MS5611_CMD_ADC_READ);
        data_ms5611.flag = MS5611_FLAG_READ_D2;
    } else if (data_ms5611.flag == MS5611_FLAG_READ_D2) {
        ms5611_read_data(&data_ms5611.data[1]);
        data_ms5611.flag = MS5611_FLAG_END_D2;
    } else if (data_ms5611.flag == MS5611_FLAG_END_D2) {
        ms5611_monitor();
        device_data.air_pressure = (uint32_t)data_ms5611.pressure;
        data_ms5611.flag = MS5611_FLAG_IDLE;
    } else {
        data_ms5611.flag = MS5611_FLAG_IDLE;
    }
}
