/*******************************************************************************
 * IO Driver for IdeasX
 * Author: curiousmuch
 * Description: HAL layer for IdeasX v0.3.X encoders
 * This lib has a couple functions
 * 1) Provide SW interface for encoder hardware for the following functions:
 *      reset, shutdown, enable / disable assistive switches
 * 2) Provide the ability to attach custom ISR to assistive switch inputs
      and INTR pin on LSM6DS3.
 * Todo:
*******************************************************************************/

#include "hal/io.h"
#include "hal/lsm6ds3.h"
#include "log/esp_log.h"

static const char* TAG = "IO Driver";

// debounce timers
static IO_config_t io_config;

void IO_GPIO_Handler(void);
static void switchA_debounce(void);
static void switchB_debounce(void);

void ICACHE_FLASH_ATTR IO_InitIO(void)
{
    ETS_GPIO_INTR_DISABLE();    // disable interrupts

    gpio_init();                // magic function in SDK

    io_config.debounce_delay = DEBOUNCEDELAY;
    io_config.switch_a_cb = NULL;
    io_config.switch_b_cb = NULL;
    io_config.imu_cb = NULL;
    io_config.debounce_timer = false;

    // setup as GPIO pins
    PIN_FUNC_SELECT(LDO_SHUTDOWN_MUX, LDO_SHUTDOWN_FUNC);
    PIN_FUNC_SELECT(SWITCH_A_MUX, SWITCH_A_FUNC);
    PIN_FUNC_SELECT(SWITCH_B_MUX, SWITCH_B_FUNC);
    PIN_FUNC_SELECT(STATUS_LED_MUX, STATUS_LED_FUNC);

    // setup GPIO states
    gpio_output_set(BIT(LDO_SHUTDOWN), BIT(STATUS_LED), (BIT(LDO_SHUTDOWN)|BIT(STATUS_LED)), (BIT(SWITCH_A)|BIT(SWITCH_B)));

    // attach callback function for GPIO interrupts
    ETS_GPIO_INTR_ATTACH(IO_GPIO_Handler, NULL);

    // setup debounce SW timers
    os_timer_disarm(&io_config.switch_a_timer);
    os_timer_disarm(&io_config.switch_b_timer);
    os_timer_setfn(&io_config.switch_a_timer, (os_timer_func_t *)switchA_debounce, NULL);
    os_timer_setfn(&io_config.switch_b_timer, (os_timer_func_t *)switchB_debounce, NULL);

}

void ICACHE_FLASH_ATTR IO_SetSwitchCallback(switch_function_t switch_a_cb, switch_function_t switch_b_cb)
{
    io_config.switch_a_cb = switch_a_cb;
    io_config.switch_b_cb = switch_b_cb;
}

void ICACHE_FLASH_ATTR IO_SetIMUCallback(imu_function_t imu_cb)
{
    io_config.imu_cb = imu_cb;
}

 void ICACHE_FLASH_ATTR IO_EnableIOInterrupts(void)
 {
     ETS_GPIO_INTR_DISABLE();

     // clear existing interrupt
     uint32_t gpio_status = GPIO_REG_READ(GPIO_STATUS_ADDRESS);
     GPIO_REG_WRITE(GPIO_STATUS_W1TC_ADDRESS, gpio_status);

     gpio_pin_intr_state_set(GPIO_ID_PIN(SWITCH_A), GPIO_PIN_INTR_ANYEDGE);
     gpio_pin_intr_state_set(GPIO_ID_PIN(SWITCH_B), GPIO_PIN_INTR_ANYEDGE);
     ETS_GPIO_INTR_ENABLE();
     ESP_LOGV(TAG, "enabled interrupts");
 }

 void ICACHE_FLASH_ATTR IO_DisableIOInterrupts(void)
 {
     ETS_GPIO_INTR_DISABLE();
     gpio_pin_intr_state_set(GPIO_ID_PIN(SWITCH_A), GPIO_PIN_INTR_DISABLE);
     gpio_pin_intr_state_set(GPIO_ID_PIN(SWITCH_B), GPIO_PIN_INTR_DISABLE);
     ESP_LOGV(TAG, "disabled interrupts");
     ETS_GPIO_INTR_DISABLE();
 }

 void ICACHE_FLASH_ATTR IO_ActivateDebounceTimer(void)
 {
     io_config.debounce_timer = true; 
 }

void ICACHE_FLASH_ATTR IO_DisableDebounceTimer(void)
{
    io_config.debounce_timer = false; 
}

uint32_t ICACHE_FLASH_ATTR IO_GetStates(void)
{
    return GPIO_REG_READ(GPIO_IN_ADDRESS);
}

void ICACHE_FLASH_ATTR IO_Shutdown(void)
{
    GPIO_OUTPUT_SET(LDO_SHUTDOWN, 0);
}

void ICACHE_FLASH_ATTR IO_GPIO_Handler(void)
{
	uint8_t i;
	uint32_t gpio_status = GPIO_REG_READ(GPIO_STATUS_ADDRESS);						// read interupt status of pins
	uint32_t gpio_states = GPIO_REG_READ(GPIO_IN_ADDRESS);							// read pin status

	ETS_GPIO_INTR_DISABLE();														// disable GPIO interrupts

	for (i=0;i<GPIO_PIN_COUNT;i++)
	{
		if (BIT(i) & gpio_status)
		{
			gpio_pin_intr_state_set(GPIO_ID_PIN(i), GPIO_PIN_INTR_DISABLE);			// disable interrupt for pin
			GPIO_REG_WRITE(GPIO_STATUS_W1TC_ADDRESS, gpio_status & BIT(i));			// clear interrupt status
            ESP_LOGV(TAG, "Pin Interrupt %d", i);
			switch(i)
			{
				case SWITCH_A:
                {
                    //ESP_LOGD(TAG, "GPIO_STATUS: %x", gpio_states);
                    LSM6DS3_Interrupt_et imu_interrupt;
                    imu_interrupt = LSM6DS3_Get_Interrupts();
                    if (imu_interrupt != LSM6DS3_NO_INT)
                    {
                        if (io_config.imu_cb != NULL)
                            io_config.imu_cb((IMU_Interrupt_et) imu_interrupt);
                        //gpio_pin_intr_state_set(GPIO_ID_PIN(i), GPIO_PIN_INTR_ANYEDGE);     // re-enable interrupts

                    }
                    else
                    {
                        os_delay_us(10000);
                        if (io_config.switch_a_cb != NULL)
                            io_config.switch_a_cb(GPIO_REG_READ(GPIO_IN_ADDRESS));
                        if (io_config.debounce_timer)
                            os_timer_arm(&io_config.switch_a_timer, io_config.debounce_delay, 0);
                    }
        		    break;
				}
				case SWITCH_B:
                {
                    //ESP_LOGD(TAG, "GPIO_STATUS: %x", gpio_states);    
                    os_delay_us(10000);                    
                    if (io_config.switch_b_cb != NULL)
                        io_config.switch_b_cb(GPIO_REG_READ(GPIO_IN_ADDRESS));
                    if (io_config.debounce_timer)
                        os_timer_arm(&io_config.switch_b_timer, io_config.debounce_delay, 0);
                    break;
				}
				default:
                {
                    ESP_LOGW(TAG, "unexpected interrupt on pin %d", i);
				}
			}
		}
	}

    ETS_GPIO_INTR_ENABLE();
}

static void ICACHE_FLASH_ATTR switchA_debounce(void)
{
    ESP_LOGV(TAG, "switchA");
	ETS_GPIO_INTR_DISABLE();

	gpio_pin_intr_state_set(GPIO_ID_PIN(SWITCH_A), GPIO_PIN_INTR_ANYEDGE);
	os_timer_disarm(&io_config.switch_a_timer);

	ETS_GPIO_INTR_ENABLE();
}


static void ICACHE_FLASH_ATTR switchB_debounce(void)
{
    ESP_LOGV(TAG, "switchB");
	ETS_GPIO_INTR_DISABLE();

	gpio_pin_intr_state_set(GPIO_ID_PIN(SWITCH_B), GPIO_PIN_INTR_ANYEDGE);
	os_timer_disarm(&io_config.switch_b_timer);

	ETS_GPIO_INTR_ENABLE();
}