#include "max30102.h"  

static const char *TAG = "max30102";  

#define MAX30102_I2C_SCL                2 // GPIO number used for I2C master clock
#define MAX30102_I2C_SDA                3 // GPIO number used for I2C master data
#define MAX30102_I2C_NUM                0  // I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip
#define MAX30102_I2C_FREQ_HZ            50000 // I2C master clock frequency
#define MAX30102_I2C_TX_BUF_DISABLE     0 // I2C master doesn't need buffer
#define MAX30102_I2C_RX_BUF_DISABLE     0
#define MAX30102_I2C_TIMEOUT_MS         1000

#define MAX30102_GPIO_INT 7 // GPIO number used for MAX30102 int

// | B7 | B6 | B5 | B4 | B3 | B2 | B1 | B0 | WRITE ADDRESS | READ ADDRESS |
// | 1  | 0  | 1  | 0  | 1  | 1  | 1  | R/W| 0xAE          | 0xAF         |  
#define MAX30102_ADDR 0x57 //  I2C device MAX30102's 7-bit address
#define MAX30102_PART_ID_REG_ADDR 0xff  


static xQueueHandle gpio_evt_queue = NULL;


/**
 * @brief init the i2c port for MAX30102
 */
static esp_err_t max30102_i2c_init()
{
    int i2c_master_port = MAX30102_I2C_NUM;

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = MAX30102_I2C_SDA,
        .scl_io_num = MAX30102_I2C_SCL,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = MAX30102_I2C_FREQ_HZ,
    };

    i2c_param_config(i2c_master_port, &conf);

    return i2c_driver_install(i2c_master_port, conf.mode, MAX30102_I2C_RX_BUF_DISABLE, MAX30102_I2C_TX_BUF_DISABLE, 0);

}




/**
 * @brief Read a sequence of bytes from a MAX30102 registers
 */
static esp_err_t max30102_register_read(uint8_t reg_addr, uint8_t *data, size_t len)
{
    return i2c_master_write_read_device(MAX30102_I2C_NUM, MAX30102_ADDR, &reg_addr, 1, data, len, MAX30102_I2C_TIMEOUT_MS / portTICK_RATE_MS);
}

/**
 * @brief Write a byte to a MAX30102 register
 */
static esp_err_t max30102_register_write_byte(uint8_t reg_addr, uint8_t data)
{
    int ret;
    uint8_t write_buf[2] = {reg_addr, data};

    ret = i2c_master_write_to_device(MAX30102_I2C_NUM, MAX30102_ADDR, write_buf, sizeof(write_buf), MAX30102_I2C_TIMEOUT_MS / portTICK_RATE_MS);

    return ret;
}

void gpio_intr_task()
{
    uint8_t byte[6];
    int data[2];
    uint8_t io_num;
    for(;;) {
        if(xQueueReceive(gpio_evt_queue, &io_num, portMAX_DELAY)) {
            ESP_ERROR_CHECK(max30102_register_read(0x07, &byte, 6));
            data[0] = ((byte[0]<<16 | byte[1]<<8 | byte[2]) & 0x03ffff);
            data[1] = ((byte[3]<<16 | byte[4]<<8 | byte[5]) & 0x03ffff);
            printf("Red: %d, IR: %d\n", data[1], data[0]);

        } 
    }
}

static void IRAM_ATTR gpio_isr_handler(void* arg)
{
    uint32_t gpio_num = (uint32_t) arg;
    xQueueSendFromISR(gpio_evt_queue, &gpio_num, NULL);
}

/**
 * @brief init the gpio intr for MAX30102
 */
static esp_err_t max30102_gpio_intr_init()
{
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_NEGEDGE;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = (1ULL<<MAX30102_GPIO_INT);
    io_conf.pull_down_en = 0;
    io_conf.pull_up_en = 1;
    ESP_ERROR_CHECK(gpio_config(&io_conf));
    
    //create a queue to handle gpio event from isr 
    gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));
    //start gpio task
    xTaskCreate(gpio_intr_task, "gpio_intr_task", 2048, NULL, 10, NULL);
    //install gpio isr service
    gpio_install_isr_service(0);
    //hook isr handler for specific gpio pin
    gpio_isr_handler_add(MAX30102_GPIO_INT, gpio_isr_handler, (void*) MAX30102_GPIO_INT);
    return ESP_OK;
 
}

void get_temp()
{
    uint8_t byte[2];
    float temp;
    ESP_ERROR_CHECK(max30102_register_write_byte(0x21, 0x01));
    vTaskDelay(100 / portTICK_RATE_MS);   
    ESP_ERROR_CHECK(max30102_register_read(0x1f, &byte[0], 1));
    ESP_ERROR_CHECK(max30102_register_read(0x20, &byte[1], 1));
    temp = (int8_t)(byte[0]) + byte[1] * 0.0625;
    printf("Temp: %f\n", temp);

    // FIFO
    ESP_ERROR_CHECK(max30102_register_write_byte(0x04, 0x00)); // clear FIFO Write Pointer
    ESP_ERROR_CHECK(max30102_register_write_byte(0x05, 0x00)); // clear FIFO Overflow Counter
    ESP_ERROR_CHECK(max30102_register_write_byte(0x06, 0x00)); // clear FIFO Read Pointer 

    // clear PPG_RDY ! Cannot receive the first interrupt without clearing !  
    uint8_t data;
    ESP_ERROR_CHECK(max30102_register_read(0x00, &data, 1));
    ESP_ERROR_CHECK(max30102_register_read(0x01, &data, 1));
}

void max30102_init()
{
    ESP_ERROR_CHECK(max30102_i2c_init());
    ESP_LOGI(TAG, "MAX30102 I2C initialized successfully");
    max30102_gpio_intr_init();
    ESP_LOGI(TAG, "MAX30102 GPIO INTR initialized successfully");

    // reset
    ESP_ERROR_CHECK(max30102_register_write_byte(0x09, 0x40)); 
    vTaskDelay(100 / portTICK_RATE_MS);   

    // Interrupt Enable
    ESP_ERROR_CHECK(max30102_register_write_byte(0x02, 0xc0)); // enable interrupts: A_FULL: FIFO Almost Full Flag and PPG_RDY: New FIFO Data Ready
    ESP_ERROR_CHECK(max30102_register_write_byte(0x03, 0x02)); // enable interrupt: DIE_TEMP_RDY: Internal Temperature Ready Flag 

    // FIFO
    ESP_ERROR_CHECK(max30102_register_write_byte(0x04, 0x00)); // clear FIFO Write Pointer
    ESP_ERROR_CHECK(max30102_register_write_byte(0x05, 0x00)); // clear FIFO Overflow Counter
    ESP_ERROR_CHECK(max30102_register_write_byte(0x06, 0x00)); // clear FIFO Read Pointer 

    // FIFO Configuration
    ESP_ERROR_CHECK(max30102_register_write_byte(0x08, 0x0f)); // SMP_AVE = 0b000: 1 averaging, FIFO_ROLLOVER_EN = 0, FIFO_A_FULL = 0xf 

    // Mode Configuration 
    ESP_ERROR_CHECK(max30102_register_write_byte(0x09, 0x03)); // MODE = 0b011: SpO2 mode

    // SpO2 Configuration 
    ESP_ERROR_CHECK(max30102_register_write_byte(0x0a, 0x47)); // SPO2_ADC_RGE = 0b10: 8192, SPO2_SR = 0b001: 100 SAMPLES PER SECOND, 
                                                               // LED_PW = 0b11: PULSE WIDTH 411, ADC RESOLUTION 18

    // LED Pulse Amplitude
    ESP_ERROR_CHECK(max30102_register_write_byte(0x0c, 0x50)); // LED1_PA(red) = 0x24, LED CURRENT 16mA 
    ESP_ERROR_CHECK(max30102_register_write_byte(0x0d, 0x50)); // LED2_PA(IR) = 0x24, LED CURRENT 16mA 
    ESP_ERROR_CHECK(max30102_register_write_byte(0x10, 0x50)); // PILOT_PA = 0x24, LED CURRENT 16mA

    // clear PPG_RDY ! Cannot receive the first interrupt without clearing !  
    uint8_t data;
    ESP_ERROR_CHECK(max30102_register_read(0x00, &data, 1));
    ESP_LOGI(TAG, "Interrupt Status 1: 0x%x", data);
    ESP_ERROR_CHECK(max30102_register_read(0x01, &data, 1));
    ESP_LOGI(TAG, "Interrupt Status 2: 0x%x", data);
}