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


#include "nrf_gpio.h"
#include "nrf.h"
#include "nrf_drv_qdec.h"
#include "nrf_error.h"
#include "app_error.h"
#include "nrf_delay.h" 


#include "bsp.h"
#include "qenc_sim.h"
#include "nordic_common.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#include "nrfx_qdec.h" 

#define GPIO_INPUT_A    4
#define GPIO_INPUT_B    3
#define GPIO_INPUT_LED  0
#define GPIO_OUTPUT_LED 28


#define STATE_0 0
#define STATE_1 1
#define STATE_2 2
#define STATE_3 3

#define Phase_A 31
#define Phase_B 30

// Global Variable declaration
static bool pressed_topleft; 
static bool pressed_topright;
static bool pressed_bottomleft;
static bool pressed_bottomright; 


int8_t EVENT = 0; 
uint8_t STATE = 0; 
int32_t  m_accdblread = 0;
int32_t  m_accread    = 0; 
int32_t  sample       = 0; 
int32_t  acc_external = 0; 

// Function Prototypes
void button_PRESS(void);
void button_DEPRESS(void);
void update_STATE(void);
void init_GPIO(void);

void return_settings_QDEC();
void set_settings_QDEC();




static void qdec_event_handler(nrf_drv_qdec_event_t event)
{

    if (event.type == NRF_QDEC_EVENT_REPORTRDY)
    {
	//printf("Entered Interrupt REPORTRDY \n\n"); 
	
	m_accread             = nrf_qdec_accread_get(); 
	m_accdblread          = nrf_qdec_accdblread_get(); 

	acc_external          = acc_external + m_accread;
	
	//printf("RR    ACC: %i \n", m_accread); 
	//printf("RR ACCdbl: %i \n", m_accdblread); 
	//printf("RR acc_external: %i \n", acc_external); 
	//printf("RR ACC ext: %i \n", acc_external); 
    }
    if (event.type == NRF_QDEC_EVENT_SAMPLERDY)
    {
	printf("Entered Interrupt SAMPLERDY \n\n");
	printf("Entered Interrupt SAMPLERDY \n\n");
	m_accdblread        = event.data.report.accdbl;
        m_accread           = event.data.report.acc;
	sample               = NRF_QDEC->SAMPLE; 
        //m_report_ready_flag = true;

        printf("Sample Ready Event \n"); 
	printf("SS    ACC: %i \n", m_accread); 
	printf("SS ACCdbl: %i \n", m_accdblread); 
	printf("SS SAMPLE: %i \n", sample);         
    }
}

int main(void)
{
    uint32_t err_code;
     init_GPIO();
    nrf_gpio_cfg_output(GPIO_OUTPUT_LED);  
    int32_t temp = 0; 

    nrf_drv_qdec_config_t qdec_cfg = 
    {
        .reportper = NRF_QDEC_REPORTPER_10 ,                     /**< Report period in samples. */
        .sampleper = NRF_QDEC_SAMPLEPER_128us,                  /**< Sampling period in microseconds. */
        .psela = GPIO_INPUT_A,                                  /**< Pin number for A input. */
        .pselb = GPIO_INPUT_B,                                  /**< Pin number for B input. */
        .pselled = NRF_QDEC_LED_NOT_CONNECTED,                  /**< Pin number for LED output. */
        .ledpre = 0,                                           /**< Time (in microseconds) how long LED is switched on before sampling. */
        .ledpol = 0,                                            /**< Active LED polarity. */
        .dbfen = false,                                         /**< State of debouncing filter. */
        .sample_inten = false,                                   /**< Enabling sample ready interrupt. */
        .interrupt_priority = 3,                                /**< QDEC interrupt priority. */
    };
    
    err_code = nrf_drv_qdec_init(&qdec_cfg, qdec_event_handler);
    //err_code = nrf_drv_qdec_init(NULL, qdec_event_handler);
    APP_ERROR_CHECK(err_code);
    

    printf("************************************** \n\n"); 
    printf("************************************** \n\n"); 
    return_settings_QDEC(); 
    printf("************************************** \n\n"); 
    printf("************************************** \n\n"); 
    set_settings_QDEC(); 
    printf("************************************** \n\n"); 
    printf("************************************** \n\n"); 
    return_settings_QDEC(); 
    printf("************************************** \n\n"); 
    printf("************************************** \n\n"); 



    temp = nrf_qdec_reportper_to_value(QDEC_CONFIG_REPORTPER);

    nrf_drv_qdec_enable();

  

    while (true)
    {
	button_PRESS();
	button_DEPRESS(); 
	if( pressed_topleft == 0 & pressed_topright == 0)
	{
	    update_STATE();
	}
        //__WFI();
    }
}

void button_PRESS(void)
{
    if(!nrf_gpio_pin_read(11) & pressed_topleft == 0)   // Top left button pressed?
    {
	nrf_delay_ms(20); 
	if( !nrf_gpio_pin_read(11) )
	{
	    pressed_topleft = 1;               //keep track of when button is pressed 
	    STATE = STATE - 1;                   //increment state to the next state
	    STATE = STATE % 4;                   // address rollover
	}
    }

    if(!nrf_gpio_pin_read(12) & pressed_topright == 0)   // Top left button pressed?
    {
	nrf_delay_ms(20); 
	if( !nrf_gpio_pin_read(12) )
	{
	    pressed_topright = 1;                // keep track of when button is pressed 
	    STATE = STATE + 1;                   // increment state to the next state
	    STATE = STATE % 4;                   // address rollover
	}
    }

    if(!nrf_gpio_pin_read(24) & pressed_bottomleft == 0)
    {
	nrf_delay_ms(20); 
	if( !nrf_gpio_pin_read(24) )
	{
	    pressed_bottomleft = 1;               // keep track of when button is pressed 
	    EVENT = 0x01;                         // Event, check accumulation register

	}
    }
    if(!nrf_gpio_pin_read(25) & pressed_bottomright == 0)
    {
	nrf_delay_ms(20); 
	if( !nrf_gpio_pin_read(25) )
	{
	    pressed_bottomright = 1;               //keep track of when button is pressed 
	    EVENT = 0x02;                          // Event, clear accumulation register
	}
    }



    
}

void button_DEPRESS(void)
{
       if( nrf_gpio_pin_read(11) & pressed_topleft == 1)   // Top left button pressed?
    {
	nrf_delay_ms(20); 
	if( nrf_gpio_pin_read(11) )
	{
	    pressed_topleft = 0;
	}
    }

    if( nrf_gpio_pin_read(12) & pressed_topright == 1)   // Top left button pressed?
    {
	nrf_delay_ms(20); 
	if( nrf_gpio_pin_read(12) )
	{
	    pressed_topright = 0; 
	}
    }

    if( nrf_gpio_pin_read(24) & pressed_bottomleft == 1)   // Top left button pressed?
    {
	nrf_delay_ms(20); 
	if( nrf_gpio_pin_read(12) )
	{
	    pressed_bottomleft = 0; 
	}
    }

    if( nrf_gpio_pin_read(25) & pressed_bottomright == 1)   // Top left button pressed?
    {
	nrf_delay_ms(20); 
	if( nrf_gpio_pin_read(25) )
	{
	    pressed_bottomright = 0; 
	}
    }

}

void update_STATE()
{
    int32_t temp = 0; 
    if(STATE == STATE_0)               //High - High
    {
	nrf_gpio_pin_clear(13);        //    Top Left,  on
	nrf_gpio_pin_clear(14);        //    Top Right, on
	nrf_gpio_pin_clear(15);        // Bottom Left,  on
	nrf_gpio_pin_clear(16);        // Bottom Right, on

	nrf_gpio_pin_set(Phase_A);     // Phase A High (on)
	nrf_gpio_pin_set(Phase_B);     // Phase B High (on) 
    }
    if(STATE == STATE_1)               // Low - High
    {
	nrf_gpio_pin_set(13);          //     Top Left,  off
	nrf_gpio_pin_clear(14);        //    Top Right, on
	nrf_gpio_pin_set(15);          //  Bottom Left,  off
	nrf_gpio_pin_clear(16);        // Bottom Right, on

	nrf_gpio_pin_clear(Phase_A);   // Phase A Low  (off)
	nrf_gpio_pin_set(Phase_B);     // Phase B High (on)
    } 
    if(STATE == STATE_2)               // Low - Low
    {
	nrf_gpio_pin_set(13);          //    Top Left,  off
	nrf_gpio_pin_set(14);          //    Top Right, off
	nrf_gpio_pin_set(15);          // Bottom Left,  off
	nrf_gpio_pin_set(16);          // Bottom Right, off

	nrf_gpio_pin_clear(Phase_A);   // Phase A Low  (off)
	nrf_gpio_pin_clear(Phase_B);   // Phase B Low (off)
    }
    if(STATE == STATE_3)               // High - Low
    {
	nrf_gpio_pin_clear(13);        //    Top Left,  on
	nrf_gpio_pin_set(14);          //    Top Right,  off
	nrf_gpio_pin_clear(15);        // Bottom Left,  on
	nrf_gpio_pin_set(16);          // Bottom Right,  off
 
	nrf_gpio_pin_set(Phase_A);     // Phase A High  (off)
	nrf_gpio_pin_clear(Phase_B);   // Phase B Low (off)
    }
    if( EVENT == 0x01)                 // Check ACC register
    {
	//temp = nrf_qdec_accread_get(); 
	//acc_external = acc_external + temp;
	//temp = nrf_qdec_acc_get(); 
	//temp = nrf_qdec_accread_get(); 
	printf("CHECK EVENT, acc_ext: %i \n", acc_external);
	EVENT = 0;  
    }

    if( EVENT == 0x02)
    {   
        nrf_qdec_task_trigger(NRF_QDEC_TASK_READCLRACC);
	acc_external = 0; 
	printf(" RESET EVENT, after clear, acc_ext: %i \n", acc_external); 
	EVENT = 0; 

    }
   
}


void init_GPIO(void)
{
  nrf_gpio_cfg(13, 1, 0, 0, 3, 0);  // Configure as output, strong drive
  nrf_gpio_cfg(14, 1, 0, 0, 3, 0);  // Configure as output, strong drive
  nrf_gpio_cfg(15, 1, 0, 0, 3, 0);  // Configure as output, strong drive
  nrf_gpio_cfg(16, 1, 0, 0, 3, 0);  // Configure as output, strong drive

  nrf_gpio_cfg(11, 0, 0, 3, 0, 0); // Top left button, Configure as input, pull down
  nrf_gpio_cfg(12, 0, 0, 3, 0, 0); // Top Right button, Configure as input, pull down
  nrf_gpio_cfg(24, 0, 0, 3, 0, 0); // Bottom Left Button, Configure as input, pull down
  nrf_gpio_cfg(25, 0, 0, 3, 0, 0); // Bottom Right Button, Configure as input, pull down

  nrf_gpio_cfg(Phase_A, 1, 0, 0, 3, 0);
  nrf_gpio_cfg(Phase_B, 1, 0, 0, 3, 0); 



  nrf_gpio_pin_clear(13); 
  nrf_gpio_pin_clear(14);
  nrf_gpio_pin_clear(15); 
  nrf_gpio_pin_clear(16);
}


void return_settings_QDEC()
{
    uint32_t temp = 0; 

    temp = nrf_qdec_reportper_reg_get(); 
    printf("Current REPORTPER: %i \n", temp); 

    temp = nrf_qdec_sampleper_reg_get();
    printf("Current SAMPLEPER: %i \n", temp);

    temp = nrf_qdec_dbfen_get(); 
    printf("Current DBFEN: %i \n", temp); 

   
    //temp = nrf_qdec_ledpre_get(); 
    //printf("LED pre-ON in uS: %i \n", temp); 

    //temp = nrf_qdec_ledpol_get(); 
    //printf("LED Active %i \n", temp); 

    temp = nrf_qdec_int_enable_check(NRF_QDEC_INT_SAMPLERDY_MASK | NRF_QDEC_INT_REPORTRDY_MASK | NRF_QDEC_INT_ACCOF_MASK); 
    printf("Enabled Interrupt Mask: 0x%0X \n", temp); 

    printf("MNRF_QDEC_INT_SAMPLERDY_MASK: 0x%8X \n", NRF_QDEC_INT_SAMPLERDY_MASK); 
    printf("NRF_QDEC_INT_REPORTRDY_MASK:  0x%8X \n", NRF_QDEC_INT_REPORTRDY_MASK); 
    printf("NRF_QDEC_INT_ACCOF_MASK:      0x%8X \n", NRF_QDEC_INT_ACCOF_MASK); 
}


void set_settings_QDEC()
{
    uint32_t temp = 0; 
    nrf_qdec_reportper_set(NRF_QDEC_REPORTPER_1);
    nrf_qdec_sampleper_set(NRF_QDEC_SAMPLEPER_128us); 
    nrf_qdec_dbfen_disable();
    //nrf_qdec_ledpre_set(50); 
    //nrf_qdec_ledpol_set(1);

    nrf_qdec_int_disable(NRF_QDEC_INT_SAMPLERDY_MASK);
    nrf_qdec_int_disable(NRF_QDEC_INT_REPORTRDY_MASK); 
    nrf_qdec_int_disable(NRF_QDEC_INT_ACCOF_MASK); 
    nrf_qdec_int_enable (NRF_QDEC_INT_REPORTRDY_MASK);  
    //nrf_qdec_int_enable(NRF_QDEC_INT_REPORTRDY_MASK); // only enable report ready
    //nrf_qdec_int_enable(NRF_QDEC_INT_ACCOF_MASK);
    temp = nrf_qdec_int_enable_check(NRF_QDEC_INT_SAMPLERDY_MASK | NRF_QDEC_INT_REPORTRDY_MASK | NRF_QDEC_INT_ACCOF_MASK);
    printf("Enable Int Mask: 0x%X \n", temp); 



    
    //nrf_qdec_int_disable(NRF_QDEC_INT_SAMPLERDY_MASK);    //can disable SAMPLERDY mask
    //nrf_qdec_int_disable(NRF_QDEC_INT_SAMPLERDY_MASK);    //can disable SAMPLERDY mask
    //nrf_qdec_enable();
    //nrf_qdec_disable(); 
    //nrf_qdec_int_enable(NRF_QDEC_INT_SAMPLERDY_MASK); 
    //nrf_qdec_int_enable(NRF_QDEC_INT_REPORTRDY_MASK);    // can't seem to enable REPORTRDY mask
    //nrf_qdec_int_enable_check(temp);
    //printf("Quick interrupt %i \n", temp); 
    //nrf_qdec_int_enable(NRF_QDEC_INT_ACCOF_MASK); 
    //nrf_qdec_int_enable_check(temp);
    //printf("Quick interrupt %i \n", temp); 
}
