/* Author: Musa Mahmood - JIIM, Inc. 
 * Date: 11/11/2023
 * License: This is proprietary code and may not be used without express written permission. 
 */
 
#include "ads1298.h"
#include "app_util_platform.h"
#include "nrf_delay.h"
#include "app_error.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"
#include "nrf_gpio.h"
#include "nrf_drv_gpiote.h"

#include "base/usb_logging.h"

#include <string.h>

String create_string(char* data) {
    s32 count = strlen(data);
    String s = {count, data};
    return s;
} // #MOVE.

uint8_t ads1298_default_regs[] = {
    ADS1298_REGDEFAULT_CONFIG1,
    ADS1298_REGDEFAULT_CONFIG2,
    ADS1298_REGDEFAULT_CONFIG3,
    ADS1298_REGDEFAULT_LOFF,
    ADS1298_REGDEFAULT_CH1SET,
    ADS1298_REGDEFAULT_CH2SET,
    ADS1298_REGDEFAULT_CH3SET,
    ADS1298_REGDEFAULT_CH4SET,
    ADS1298_REGDEFAULT_CH5SET,
    ADS1298_REGDEFAULT_CH6SET,
    ADS1298_REGDEFAULT_CH7SET,
    ADS1298_REGDEFAULT_CH8SET,
    ADS1298_REGDEFAULT_RLD_SENSP,
    ADS1298_REGDEFAULT_RLD_SENSN,
    ADS1298_REGDEFAULT_LOFF_SENSP,
    ADS1298_REGDEFAULT_LOFF_SENSN,
    ADS1298_REGDEFAULT_LOFF_FLIP,
    ADS1298_REGDEFAULT_LOFF_STATP,
    ADS1298_REGDEFAULT_LOFF_STATN,
    ADS1298_REGDEFAULT_GPIO,
    ADS1298_REGDEFAULT_PACE,
    ADS1298_REGDEFAULT_RESP,
    ADS1298_REGDEFAULT_CONFIG4,
    ADS1298_REGDEFAULT_WCT1,
    ADS1298_REGDEFAULT_WCT2
};

// SPI flags:
static volatile bool spi_xfer_done;
static uint8_t rx_buffer[27+2];

// SPI Instance:
static const nrf_drv_spi_t spi0 = NRF_DRV_SPI_INSTANCE(0);

// SPI Event Handler:
static void spi_event_handler(nrf_drv_spi_evt_t const *p_event, void *p_context)
{
    spi_xfer_done = true;
#if ADS1298_LOG_DEBUG
    JIIM_LOG(" > SPI transfer completed.");
#endif
}

void ads1298_initialize(ads1298_info_t *p_info) {
    NRF_LOG_INFO("Initializing ADS1298...");
    // 1. Power up the ADS1298:
    ads1298_power_up();
    #if ADS1298_LOG_DEBUG
        NRF_LOG_FLUSH();
    #endif
    nrf_drv_spi_config_t spi_config;
    memset(&spi_config, 0, sizeof(spi_config));
    // 2. Initialize SPI
    spi_config.bit_order    = NRF_DRV_SPI_BIT_ORDER_MSB_FIRST;
    spi_config.ss_pin       = ADS1298_CS_PIN;
    spi_config.miso_pin     = ADS1298_MISO_PIN;
    spi_config.mosi_pin     = ADS1298_MOSI_PIN;
    spi_config.sck_pin      = ADS1298_SCK_PIN;
    //spi_config.frequency    = NRF_DRV_SPI_FREQ_500K;
    spi_config.frequency    = NRF_DRV_SPI_FREQ_1M;
    //spi_config.frequency    = NRF_DRV_SPI_FREQ_2M;
    spi_config.irq_priority = APP_IRQ_PRIORITY_HIGHEST;
    spi_config.mode         = NRF_DRV_SPI_MODE_1;
    spi_config.orc          = 0x55;

    APP_ERROR_CHECK(nrf_drv_spi_init(&spi0, &spi_config, spi_event_handler, NULL));
    // 3. Stop read data continuous mode (rdatac)
    ads1298_stop_rdatac();
    #if ADS1298_LOG_DEBUG
        NRF_LOG_FLUSH();
    #endif
    // 4. Init registers:
    // Copy default registers into p_info->registers (first 25 bytes)
    memset(p_info->registers, 0, sizeof(p_info->registers));
    memcpy(p_info->registers, ads1298_default_regs, ADS1298_REGISTER_COUNT);
    ads1298_init_default_registers();
    #if ADS1298_LOG_DEBUG
        NRF_LOG_FLUSH();
    #endif
    // 5. Soft start: 
    ads1298_soft_start_conversion();
    // 6. Check ID: (sets lower 3 bits of ads1298_settings[25])
    ads1298_check_id(p_info);
    // 7. Start read data continuous mode (rdatac):
    ads1298_start_rdatac();
    // 8. Place on standby mode:
    ads1298_standby();

    #if ADS1298_LOG_DEBUG
        NRF_LOG_FLUSH();
    #endif
}

void ads1298_uninitialize(void) {
    JIIM_LOG("Uninitializing ADS1298...");
    nrf_drv_spi_uninit(&spi0);
}

bool ads1298_check_id(ads1298_info_t *p_info) {
    bool device_found = false;
    #if ADS1298_LOG_DEBUG
        JIIM_LOG("Checking ADS129xR? ID:");
    #endif
    uint8_t tx_buf[6] = {ADS1298_OPC_RREG|ADS1298_REGADDR_ID, 0x01, 0, 0, 0, 0};
    uint8_t rx_buf[6] = {0,0,0,0,0,0};
    spi_xfer_done = false;
    APP_ERROR_CHECK(nrf_drv_spi_transfer(&spi0, tx_buf, 2, rx_buf, 6));
    while(!spi_xfer_done) {
        __WFE();
    }
    #if ADS1298_LOG_DEBUG
        JIIM_LOG("rx_buf[3] = 0x%X", rx_buf[3]); 
        JIIM_LOG("rx_buf dump:");
        JIIM_LOG("rx_buf[0:5] = 0x%X%X%X%X%X", rx_buf[0], rx_buf[1], rx_buf[2], rx_buf[3], rx_buf[4], rx_buf[5]); 
        
        // NRF_LOG_HEXDUMP_INFO(rx_buf, 6);
    #endif
    // Check lower 3 bits 0x[...]..[xxx]
    uint8_t nch_check = rx_buf[3] & 0x07; // 0x.....111
    p_info->nChs = 0;
    if (nch_check == ADS129x_4CH_BITMASK) {
        p_info->nChs = 4; // ADS1294
    } else if (nch_check == ADS129x_6CH_BITMASK) {
        p_info->nChs = 6; // ADS1296
    } else if (nch_check == ADS129x_8CH_BITMASK) {
        p_info->nChs = 8; // ADS1298
    } else {
        JIIM_LOG("[ERROR] Expected ADS129xX not detected! E0");
        return false;
    }
    // Check middle bits 0x[...]10[...] < always should be 10
    if ((rx_buf[3] & 0x10) != 0x10 || p_info->nChs == 0) {
        JIIM_LOG("[ERROR] Expected ADS129xX not detected! E1");
        return false;
    }
    // Check first three bits: 0x[xxx]..[...]
    uint8_t r_ver_chk = rx_buf[3] & 0xE0;
    if (r_ver_chk == ADS129x_DEVICE_FAMILY_BITMASK) {
        p_info->name_len = sprintf(p_info->name, "ADS129%d", p_info->nChs);
        JIIM_LOG("%s detected! (name_len: %d)", p_info->name, p_info->name_len);
        device_found = true;
    } else if (r_ver_chk == ADS129xR_DEVICE_FAMILY_BITMASK) {
        p_info->name_len = sprintf(p_info->name, "ADS129%dR", p_info->nChs);
        JIIM_LOG("%s detected! (name_len: %d)", p_info->name, p_info->name_len);
        device_found = true;
    } else {
        JIIM_LOG("[ERROR] Expected ADS129xX not detected! E2");
        return false;
    }
    
    return device_found;
}

// chNumber = 1:8
__STATIC_INLINE bool ads1298_check_channel(ads1298_info_t *p_info, uint8_t chNumber) {
    if (chNumber > p_info->nChs) return false;
    if (chNumber > 8) {
        JIIM_LOG("[ads1298_check_channel] INVALID CHANNEL NUMBER: %d!", chNumber);
        return false;
    }
    return !((p_info->registers[3 + chNumber] & 0x80) == 0x80);
}

void ads1298_update_active_chs(ads1298_info_t *p_info) {
    p_info->active_chs = 0;
    if (ads1298_check_channel(p_info, 1)) p_info->active_chs |= 0x80;
    if (ads1298_check_channel(p_info, 2)) p_info->active_chs |= 0x40;
    if (ads1298_check_channel(p_info, 3)) p_info->active_chs |= 0x20;
    if (ads1298_check_channel(p_info, 4)) p_info->active_chs |= 0x10;
    if (p_info->nChs == 4) return;
    if (ads1298_check_channel(p_info, 5)) p_info->active_chs |= 0x08;
    if (ads1298_check_channel(p_info, 6)) p_info->active_chs |= 0x04;
    if (p_info->nChs == 6) return;
    if (ads1298_check_channel(p_info, 7)) p_info->active_chs |= 0x02;
    if (ads1298_check_channel(p_info, 8)) p_info->active_chs |= 0x01;
}

void ads1298_update_registers(ads1298_info_t *p_info) {
    JIIM_LOG("Updating ADS1298 registers...");
    ads1298_update_active_chs(p_info);
    uint8_t i = 0; // Register index
    uint8_t tx_data[ADS1298_REGISTER_COUNT + 2]; // plus 2 for opcodes
    uint8_t rx_data[ADS1298_REGISTER_COUNT + 2];
    memset(tx_data, 0, ADS1298_REGISTER_COUNT + 2);
    memset(rx_data, 0, ADS1298_REGISTER_COUNT + 2);
    // Write Register Opcode | Register Address:
    tx_data[0] = ADS1298_OPC_WREG | ADS1298_REGADDR_CONFIG1;
    tx_data[1] = ADS1298_REGISTER_COUNT;
    // Copy default register values:
    memcpy(&tx_data[2], p_info->registers, ADS1298_REGISTER_COUNT);
    // Transaction size = 2 + number of registers to write
    spi_xfer_done = false;
    APP_ERROR_CHECK(nrf_drv_spi_transfer(&spi0, tx_data, ADS1298_REGISTER_COUNT + 2, rx_data, ADS1298_REGISTER_COUNT + 2));
    while (!spi_xfer_done)
    {
        __WFE();
    }
#if ADS1298_LOG_DEBUG
    JIIM_LOG("[ADS1298] Registers updated!");
#endif
}

void ads1298_init_default_registers(void) {
    JIIM_LOG("Initializing ADS1298 registers...");
    uint8_t i = 0; // Register index
    uint8_t tx_data[ADS1298_REGISTER_COUNT + 2]; // plus 2 for opcodes
    uint8_t rx_data[ADS1298_REGISTER_COUNT + 2];
    memset(tx_data, 0, ADS1298_REGISTER_COUNT + 2);
    memset(rx_data, 0, ADS1298_REGISTER_COUNT + 2);
    // Write Register Opcode | Register Address:
    tx_data[0] = ADS1298_OPC_WREG | ADS1298_REGADDR_CONFIG1;
    tx_data[1] = ADS1298_REGISTER_COUNT;
    // Copy default register values:
    memcpy(&tx_data[2], ads1298_default_regs, ADS1298_REGISTER_COUNT);
    // Transaction size = 2 + number of registers to write
    spi_xfer_done = false;
    APP_ERROR_CHECK(nrf_drv_spi_transfer(&spi0, tx_data, ADS1298_REGISTER_COUNT + 2, rx_data, ADS1298_REGISTER_COUNT + 2));
    while (!spi_xfer_done)
    {
        __WFE();
    }
#if ADS1298_LOG_DEBUG
    JIIM_LOG("[ADS1298] Registers initialized.");
#endif
}

// Power controls:
void ads1298_power_down(void) {
    nrf_gpio_pin_clear(ADS1298_PWDN_PIN);
    nrf_delay_ms(5);
}

void ads1298_power_up(void) {
    nrf_gpio_pin_set(ADS1298_PWDN_PIN);
    nrf_delay_ms(80);
}

// Standby/Wakeup controls:
void ads1298_wakeup(void) {
    JIIM_LOG("Waking up ADS1298...");
    uint8_t cmd = ADS1298_OPC_WAKEUP;
    uint8_t rx_buf;
    spi_xfer_done = false;
    APP_ERROR_CHECK(nrf_drv_spi_transfer(&spi0, &cmd, 1, &rx_buf, 1));
    while (!spi_xfer_done)
    {
        __WFE();
    }
}

void ads1298_standby(void) {
    JIIM_LOG("Placing ADS1298 in standby mode...");
    uint8_t cmd = ADS1298_OPC_STANDBY;
    uint8_t rx_buf;
    spi_xfer_done = false;
    APP_ERROR_CHECK(nrf_drv_spi_transfer(&spi0, &cmd, 1, &rx_buf, 1));
    while (!spi_xfer_done)
    {
        __WFE();
    }
}

// Start/stop conversions
void ads1298_soft_start_conversion(void) {
    JIIM_LOG("[ADS1298] Starting conversion...");
    uint8_t cmd = ADS1298_OPC_START;
    uint8_t rx_buf;
    spi_xfer_done = false;
    APP_ERROR_CHECK(nrf_drv_spi_transfer(&spi0, &cmd, 1, &rx_buf, 1));
    while (!spi_xfer_done)
    {
        __WFE();
    }
}

void ads1298_start_rdatac(void) {
    JIIM_LOG("[ADS1298] Starting Read Data Continuous Mode...");
    uint8_t cmd = ADS1298_OPC_RDATAC;
    uint8_t rx_buf;
    spi_xfer_done = false;
    APP_ERROR_CHECK(nrf_drv_spi_transfer(&spi0, &cmd, 1, &rx_buf, 1));
    while (!spi_xfer_done)
    {
        __WFE();
    }
}

void ads1298_stop_rdatac(void) {
    JIIM_LOG("[ADS1298] Stopping Read Data Continuous Mode...");
    uint8_t cmd = ADS1298_OPC_SDATAC;
    uint8_t rx_buf;
    spi_xfer_done = false;
    APP_ERROR_CHECK(nrf_drv_spi_transfer(&spi0, &cmd, 1, &rx_buf, 1));
    while (!spi_xfer_done)
    {
        __WFE();
    }
}

// __STATIC_INLINE uint8_t get_channel_count_from_settings(uint8_t settings) {
//   uint8_t lower_bits = settings & 0x07; 
//   if (lower_bits == ADS129x_4CH_BITMASK) {
//     return 4;
//   } else if (lower_bits == ADS129x_6CH_BITMASK) {
//     return 6;
//   } else if (lower_bits == ADS129x_8CH_BITMASK) {
//     return 8;
//   }
// }

// TODO: Add option for 2 bytes (low resolution)
/*
__STATIC_INLINE void copy_relevant_data(ble_exg_t *p_exg, uint8_t active_chs) {
    if ((active_chs & 0x80) == 0x80) memcpy(&p_exg->exg_ch1_buffer[p_exg->data_buffer_count], &rx_buffer[3], 3);
    if ((active_chs & 0x40) == 0x40) memcpy(&p_exg->exg_ch2_buffer[p_exg->data_buffer_count], &rx_buffer[6], 3);
    if ((active_chs & 0x20) == 0x20) memcpy(&p_exg->exg_ch3_buffer[p_exg->data_buffer_count], &rx_buffer[9], 3);
    if ((active_chs & 0x10) == 0x10) memcpy(&p_exg->exg_ch4_buffer[p_exg->data_buffer_count], &rx_buffer[12], 3);
    if ((active_chs & 0x08) == 0x08) memcpy(&p_exg->exg_ch5_buffer[p_exg->data_buffer_count], &rx_buffer[15], 3);
    if ((active_chs & 0x04) == 0x04) memcpy(&p_exg->exg_ch6_buffer[p_exg->data_buffer_count], &rx_buffer[18], 3);
    if ((active_chs & 0x02) == 0x02) memcpy(&p_exg->exg_ch7_buffer[p_exg->data_buffer_count], &rx_buffer[21], 3);
    if ((active_chs & 0x01) == 0x01) memcpy(&p_exg->exg_ch8_buffer[p_exg->data_buffer_count], &rx_buffer[24], 3);
    p_exg->data_buffer_count += 3;
}

// Buffer size depends on version (27 bytes for ADS1298, 21 for ADS1296, and 15 for ADS1294)
void ads1298_get_data(ble_exg_t *p_exg, uint8_t active_chs) {
    spi_xfer_done = false;
    memset(rx_buffer, 0, 27);
    nrf_drv_spi_transfer(&spi0, rx_buffer, 27, rx_buffer, 27);
    while (!spi_xfer_done) { __WFE(); }
    copy_relevant_data(p_exg, active_chs);
}

void ads1296_get_data(ble_exg_t *p_exg, uint8_t active_chs) {
    spi_xfer_done = false;
    memset(rx_buffer, 0, 21);
    nrf_drv_spi_transfer(&spi0, rx_buffer, 21, rx_buffer, 21);
    while (!spi_xfer_done) { __WFE(); }
    copy_relevant_data(p_exg, active_chs);
}

void ads1294_get_data(ble_exg_t *p_exg, uint8_t active_chs) {
    spi_xfer_done = false;
    memset(rx_buffer, 0, 15);
    nrf_drv_spi_transfer(&spi0, rx_buffer, 15, rx_buffer, 15);
    while (!spi_xfer_done) { __WFE(); }
    // Check mode p_exg->ads1298_settings[26] b0
    copy_relevant_data(p_exg, active_chs);
}
*/