#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdio.h>

#include "nrf.h"
#include "nrf_drv_usbd.h"
#include "nrf_drv_clock.h"
#include "nrf_gpio.h"
#include "nrf_delay.h"
#include "nrf_drv_power.h"

#include "app_error.h"
#include "app_util.h"
#include "app_usbd_core.h"
#include "app_usbd.h"
#include "app_usbd_string_desc.h"
#include "app_usbd_cdc_acm.h"
#include "app_usbd_serial_num.h"

#include "app_timer.h"

#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#include "custom_board.h"

static enum recording_mode_t recording_mode = RECORDING_MODE_DISABLED;

#if ADS1298
  #include "ads1298.h"
  #include "nrf_drv_gpiote.h"
  
  static bool drdy_flag = false;
  ads1298_info_t m_info;
#endif

// #TEMP!
char g_Peripheral_Info[] = "ADS1298|";
char g_WhoAmI[] = "multimodal_cv_dev0_ecg";
uint8_t g_HWFW_Ver[] = {1,0,0,1}; // HW Maj, HW Min, FW Maj, FW Min

static volatile bool run_throughput_test = false;
static char usb_tx_buffer[NRF_DRV_USBD_EPSIZE];

#define READ_SIZE 64
static char usb_read_buffer[READ_SIZE];


#ifndef USBD_POWER_DETECTION
#define USBD_POWER_DETECTION true
#endif

static void cdc_acm_user_ev_handler(app_usbd_class_inst_t const * p_inst,
                                    app_usbd_cdc_acm_user_event_t event);

#define CDC_ACM_COMM_INTERFACE  0
#define CDC_ACM_COMM_EPIN       NRF_DRV_USBD_EPIN2

#define CDC_ACM_DATA_INTERFACE  1
#define CDC_ACM_DATA_EPIN       NRF_DRV_USBD_EPIN1
#define CDC_ACM_DATA_EPOUT      NRF_DRV_USBD_EPOUT1


APP_USBD_CDC_ACM_GLOBAL_DEF(m_app_cdc_acm,
                            cdc_acm_user_ev_handler,
                            CDC_ACM_COMM_INTERFACE,
                            CDC_ACM_DATA_INTERFACE,
                            CDC_ACM_COMM_EPIN,
                            CDC_ACM_DATA_EPIN,
                            CDC_ACM_DATA_EPOUT,
                            APP_USBD_CDC_COMM_PROTOCOL_AT_V250);


void usb_transmit_message_ex(uint8_t message_prefix, uint8_t message_part_two, uint8_t* message, uint8_t message_length) {
    usb_tx_buffer[0] = message_prefix;
    usb_tx_buffer[1] = message_part_two;
    usb_tx_buffer[2] = message_length + 3;
    memcpy(&usb_tx_buffer[3], message, message_length);
    ret_code_t ret = app_usbd_cdc_acm_write(&m_app_cdc_acm, usb_tx_buffer, message_length + 3);
    
    NRF_LOG_INFO("[ex]Writing message of length %d, ret: %d", message_length + 3, ret);
}

void usb_transmit_message(uint8_t message_prefix, uint8_t* message, uint8_t message_length) {
    usb_tx_buffer[0] = message_prefix;
    usb_tx_buffer[1] = message_length + 2;
    memcpy(&usb_tx_buffer[2], message, message_length);
    ret_code_t ret = app_usbd_cdc_acm_write(&m_app_cdc_acm, usb_tx_buffer, message_length + 2);
    
    NRF_LOG_INFO("[0]Writing message of length %d, ret: %d", message_length + 2, ret);
}

// length is always `READ_SIZE`
void write_ic_settings(uint8_t* new_packet) {
    switch (SECOND_NIBBLE(new_packet[0])) {
        case TN_IC_ADS1298: {
            // #TODO: &new_packet[1]
            #if ADS1298
                NRF_LOG_INFO("Writing new ADS1298 registers:");
                NRF_LOG_HEXDUMP_INFO(&new_packet[1], ADS1298_REGISTER_COUNT);
                memcpy(m_info.registers, &new_packet[1], ADS1298_REGISTER_COUNT);
                ads1298_update_registers(&m_info);
                ads1298_set_data_buffer_length(&m_info);
            #endif
        } break;
        default:
            break;
    }
}

void read_ic_settings(uint8_t* new_packet) {
    switch (SECOND_NIBBLE(new_packet[0])) {
        case TN_IC_ADS1298: {
            // #NOTE: you will not be able to read registers while in RDATAC mode.
            ads1298_stop_rdatac();
            ads1298_check_id(&m_info);
            usb_transmit_message_ex(ID_REGISTER_READBACK_PREFIX, REGISTER_READBACK_ADS1298, 
                                    m_info.id_buffer, sizeof(m_info.id_buffer));
            usb_transmit_message_ex(REGISTER_READBACK_PREFIX, REGISTER_READBACK_ADS1298, 
                                    m_info.registers, ADS1298_REGISTER_COUNT);
        } break;
        default:
            break;
    }
}

// length is always `READ_SIZE`
static void process_new_packet(uint8_t* new_packet) {
    switch (FIRST_NIBBLE(new_packet[0])) {
        case CN_WRITE_IC_REGS:  {
            write_ic_settings(new_packet);
        } break;
        case CN_READ_IC_REGS:   {
            read_ic_settings(new_packet);
        } break;
        case CN_STREAM_CONTROL: {
            if (SECOND_NIBBLE(new_packet[0]) == TN_STREAM_START) {
                recording_mode = RECORDING_MODE_ALL;
                #if ADS1298
                if (m_info.state == 0) {
                    m_info.state = 1;
                    // ads1298_update_registers(&m_info);
                    // ads1298_set_data_buffer_length(&m_info);
                    ads1298_wakeup();
                    NRF_LOG_INFO("[ADS129x] Active channels: 0x%X", m_info.active_chs);
                }
                #endif
            }
            if (SECOND_NIBBLE(new_packet[0]) == TN_STREAM_STOP) {
                recording_mode = RECORDING_MODE_DISABLED;
                #if ADS1298
                if (m_info.state == 1) { 
                    m_info.state = 0;
                      ads1298_standby();
                }
                #endif
            }
            if (SECOND_NIBBLE(new_packet[0]) == TN_STREAM_REQUEST_INFO) {
                usb_transmit_message(CTRL_STAT_INFO_PERIPHERALS, g_Peripheral_Info, strlen(g_Peripheral_Info));
                usb_transmit_message(CTRL_STAT_INFO_WHO_AM_I, g_WhoAmI, strlen(g_WhoAmI));
                usb_transmit_message(CTRL_STAT_INFO_HW_FW_VERSION, g_HWFW_Ver, sizeof(g_HWFW_Ver));
            }
            if (SECOND_NIBBLE(new_packet[0]) == TN_STREAM_REQUEST_BATTERY_LEVEL) {
                // #NOTE: Will not use. This is not a battery-powered device. (I mean, it can 
                // be, but it's also wired, so who cares.) 
                // Just return some constant.
            }
            
        } break;
        case CN_MISC_CONTROLS: {
            if (SECOND_NIBBLE(new_packet[0]) == TN_MISC_THROUGHPUT_TEST) { // 0xEF
                NRF_LOG_INFO("Starting throughput test!");
                run_throughput_test = true;
            }
            if (SECOND_NIBBLE(new_packet[0]) == TN_MISC_RTT_REQUEST) {
                // [↓] #TODO [Later]
            }
        } break;
        default:
            break;
    }
}

static void cdc_acm_user_ev_handler(app_usbd_class_inst_t const * p_inst,
                                    app_usbd_cdc_acm_user_event_t event) {
    app_usbd_cdc_acm_t const * p_cdc_acm = app_usbd_cdc_acm_class_get(p_inst);

    switch (event) {
        case APP_USBD_CDC_ACM_USER_EVT_PORT_OPEN: {

            /*Setup first transfer*/
            ret_code_t ret = app_usbd_cdc_acm_read(&m_app_cdc_acm, usb_read_buffer, READ_SIZE);
            UNUSED_VARIABLE(ret);
            break;
        }
        case APP_USBD_CDC_ACM_USER_EVT_PORT_CLOSE:
            break;
        case APP_USBD_CDC_ACM_USER_EVT_TX_DONE:
            break;
        case APP_USBD_CDC_ACM_USER_EVT_RX_DONE: {
            ret_code_t ret;
            NRF_LOG_INFO("Bytes waiting: %d", app_usbd_cdc_acm_bytes_stored(p_cdc_acm));
            do {
                /*Get amount of data transfered*/
                size_t size = app_usbd_cdc_acm_rx_size(p_cdc_acm);

                NRF_LOG_INFO("RX[size:%lu] [0]: 0x%X", size, usb_read_buffer[0]);
                
                process_new_packet(usb_read_buffer);
                
                // if (usb_read_buffer[0] == 0xBE) {
                    
                // }
                
                // if (usb_read_buffer[0] == 0xEF) {
                //     run_throughput_test = true;
                // }
                // if (usb_read_buffer[0] == 0xE1) {
                //     run_throughput_test = false;
                // }
                // NRF_LOG_INFO("Received packet; first byte: 0x%X", new_packet[0]);

                /* Fetch data until internal buffer is empty */
                ret = app_usbd_cdc_acm_read(&m_app_cdc_acm, usb_read_buffer, READ_SIZE);
            } while (ret == NRF_SUCCESS);

            break;
        }
        default:
            break;
    }
}

static void usbd_user_ev_handler(app_usbd_event_type_t event) {
    switch (event) {
        case APP_USBD_EVT_DRV_SUSPEND:
            break;
        case APP_USBD_EVT_DRV_RESUME:
            break;
        case APP_USBD_EVT_STARTED:
            break;
        case APP_USBD_EVT_STOPPED:
            app_usbd_disable();
            break;
        case APP_USBD_EVT_POWER_DETECTED:
            NRF_LOG_INFO("USB power detected");

            if (!nrf_drv_usbd_is_enabled()) {
                app_usbd_enable();
            }
            break;
        case APP_USBD_EVT_POWER_REMOVED:
            NRF_LOG_INFO("USB power removed");
            app_usbd_stop();
            break;
        case APP_USBD_EVT_POWER_READY:
            NRF_LOG_INFO("USB ready");
            app_usbd_start();
            break;
        default:
            break;
    }
}

void idle_state_handle(void) {
    
}

#if ADS1298
void drdy_pin_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action) {
    UNUSED_PARAMETER(pin);
    UNUSED_PARAMETER(action);
    drdy_flag = true;
}

void ads1298_interrupt_setup(void) {
    nrf_gpio_cfg_output(ADS1298_PWDN_PIN);
    nrf_gpio_pin_clear(ADS1298_PWDN_PIN);
    nrf_gpio_cfg_input(ADS1298_DRDY_PIN, NRF_GPIO_PIN_PULLUP);
    // Initialize GPIOTE:
    ret_code_t err_code = NRF_SUCCESS;
    if (!nrf_drv_gpiote_is_init()) {
        err_code = nrf_drv_gpiote_init();
    }
    NRF_LOG_INFO("GPIOTE error code: %d", err_code);
    APP_ERROR_CHECK(err_code);
    nrfx_gpiote_in_config_t in_config = NRFX_GPIOTE_CONFIG_IN_SENSE_HITOLO(true);
    in_config.is_watcher = true;
    in_config.pull = NRF_GPIO_PIN_NOPULL;
    err_code = nrf_drv_gpiote_in_init(ADS1298_DRDY_PIN, &in_config, drdy_pin_handler);
    APP_ERROR_CHECK(err_code);
    nrf_drv_gpiote_in_event_enable(ADS1298_DRDY_PIN, true);
    ads1298_power_down();
}
#endif

int main(void) {
    ret_code_t ret;
    static const app_usbd_config_t usbd_config = {
        .ev_state_proc = usbd_user_ev_handler
    };

    ret = NRF_LOG_INIT(NULL);
    APP_ERROR_CHECK(ret);

    NRF_LOG_DEFAULT_BACKENDS_INIT(); // Init RTT Backend

    ret = nrf_drv_clock_init();
    APP_ERROR_CHECK(ret);
    
    nrf_drv_clock_lfclk_request(NULL);

    // Wait for LFCLK to init. 
    while(!nrf_drv_clock_lfclk_is_running()) { }

    ret = app_timer_init();
    APP_ERROR_CHECK(ret);

    app_usbd_serial_num_generate();

    ret = app_usbd_init(&usbd_config);
    APP_ERROR_CHECK(ret);
    NRF_LOG_INFO("USBD CDC ACM example started.");

    app_usbd_class_inst_t const * class_cdc_acm = app_usbd_cdc_acm_class_inst_get(&m_app_cdc_acm);
    ret = app_usbd_class_append(class_cdc_acm);
    APP_ERROR_CHECK(ret);

    if (USBD_POWER_DETECTION) {
        ret = app_usbd_power_events_enable();
        APP_ERROR_CHECK(ret);
    } else {
        NRF_LOG_INFO("No USB power detection enabled\r\nStarting USB now");

        app_usbd_enable();
        app_usbd_start();
    }
    
    // Init peripherals:
#if ADS1298
    ads1298_interrupt_setup();
    ads1298_initialize(&m_info);
#endif
    
    memset(usb_tx_buffer, 0x41, NRF_DRV_USBD_EPSIZE); // #TEMP
    
    while (true) {
        while (app_usbd_event_queue_process()) { /* Nothing to do */ }
        
        /*
        // I think this check is totally irrelevant?
        if (recording_mode == RECORDING_MODE_ALL) {
            // #TODO: Count samples 
            #if ADS1298
              switch (m_info.nChs) {
                case 4:
                    ads1294_get_data(&m_info);
                    break;
                case 6:
                    ads1296_get_data(&m_info);
                    break;
                case 8:
                    ads1298_get_data(&m_info);
                    break;
                default:
                    break;
              }
            #endif
            // #TODO: if buffer full transfer via `app_usbd_cdc_acm_write`
        }*/
        
        if (run_throughput_test) {
            /*ret = */app_usbd_cdc_acm_write(&m_app_cdc_acm, usb_tx_buffer, NRF_DRV_USBD_EPSIZE);
        }
        
        UNUSED_RETURN_VALUE(NRF_LOG_PROCESS());
        /* Sleep CPU only if there was no interrupt since last loop processing */
        __WFE();
    }
}

/** @} */
// size_t size = sprintf(m_tx_buffer, "Hello USB CDC FA demo: %u\r\n", frame_counter);