#include "fsm.h"
#include "gpio.h"

#include <drivers/pwr/adi_pwr.h>
#include <drivers/xint/adi_xint.h>
#include "RTT/SEGGER_RTT.h"

//=========================== prototype =======================================

__STATIC_INLINE int32_t  gpio_initPinMux(void);

//=========================== variables =======================================

//==== global
debug_errors_t debug_errors;

//==== local
static uint8_t  xint_DevMem[ADI_XINT_MEMORY_SIZE];  // Memory required for XINT driver
static uint8_t  gpio_DevMem[ADI_GPIO_MEMORY_SIZE];  // Memory required for GPIO driver

//============================= main ==========================================

int main(void)
{
    uint32_t              clock;
	  ADI_PWR_WAKEUP_STATUS wakeStatus; // TODO this type is probably wrongly defined in adi_pwr.h

//======================== initialisation =====================================

    //==== ADI Drivers and Services
    adi_pwr_Init();                                   // Initialize power service
    adi_xint_Init(xint_DevMem, ADI_XINT_MEMORY_SIZE); // Initialize external interrupt service
    adi_gpio_Init(gpio_DevMem, ADI_GPIO_MEMORY_SIZE); // init adi gpio driver
    gpio_initPinMux();                                // init pin multiplexer 
	
	  DEBUG_MSG(MSG_IDN); // send init MSG
	
	  //==== Process wake up status, in case we are waking up from shut-down
	  adi_pwr_GetWakeUpStatus(&wakeStatus);
	
    // indicate HCLK clock, just to be sure
    adi_pwr_GetClockFrequency(ADI_CLOCK_HCLK, &clock);
    DEBUG_MSG("HCLK = %d Hz, Wake status = 0x%X\n", clock, wakeStatus);
	
    // run fsm
    fsm_enterState(3000, FSM_STATE_START); // jump to start if don't get BOOT event notif in 3seconds		
	  adi_pwr_EnterLowPowerMode(ADI_PWR_MODE_FLEXI, NULL, 0);
}

// _sys_exit() should never be reached as we enter lowpower mode on exit!
// but it must be defined since we do not use MicroLib!
void _sys_exit(int return_code)
{
	ERR_MSG("Program exited!!! Return code = %d\n", return_code);
	label:
	   goto label;
}

//============================= private =======================================

/*
 * Initialize the Port Control MUX Registers
 */
__STATIC_INLINE int32_t gpio_initPinMux(void)
{
    //==== default pin states
    gpio_adc_Off(); 
    gpio_acc_On(); // TODO this should be off
    gpio_ref_Off();
    gpio_ad5700_Off();
    gpio_ledYellow_Off();
    gpio_ledGreen_On();

    // pin mux
    *pREG_GPIO0_CFG = I2C0_SCL_PORTP0_MUX | I2C0_SDA_PORTP0_MUX
                      | UART0_TX_PORTP0_MUX | UART0_RX_PORTP0_MUX
                      | SWD0_SWD0_CLK_PORTP0_MUX | SWD0_SWD0_DATA_PORTP0_MUX;
    *pREG_GPIO1_CFG = SPI1_CLK_PORTP1_MUX | SPI1_MOSI_PORTP1_MUX
                      | SPI1_MISO_PORTP1_MUX | SPI2_CLK_PORTP1_MUX | SPI2_MOSI_PORTP1_MUX
                      | SPI2_MISO_PORTP1_MUX | SPI2_CS_0_PORTP1_MUX | GPIO_P1_1_PORTP1_MUX
                      | UART1_TX_PORTP1_MUX | RTC_RTC_SS2_PORTP1_MUX
                      | SWV_SWV_PORTP1_MUX;
    *pREG_GPIO2_CFG = SPI1_CS_1_PORTP2_MUX | UART1_RX_PORTP2_MUX | ADC0_IN_ADC0_VIN0_PORTP2_MUX
                      | ADC0_IN_ADC0_VIN1_PORTP2_MUX | ADC0_IN_ADC0_VIN2_PORTP2_MUX;

    // enable desired outputs
    adi_gpio_OutputEnable(ADI_GPIO_PORT0, ADI_GPIO_PIN_1 | ADI_GPIO_PIN_2 | ADI_GPIO_PIN_9 | ADI_GPIO_PIN_12, true);
    adi_gpio_OutputEnable(ADI_GPIO_PORT1, ADI_GPIO_PIN_10 | ADI_GPIO_PIN_11 | ADI_GPIO_PIN_12 | ADI_GPIO_PIN_13, true);
    adi_gpio_OutputEnable(ADI_GPIO_PORT2, ADI_GPIO_PIN_8 | ADI_GPIO_PIN_9, true);

    // and inputs
		adi_gpio_InputEnable(ADI_GPIO_PORT0, ADI_GPIO_PIN_13 | ADI_GPIO_PIN_15, true);
    adi_gpio_InputEnable(ADI_GPIO_PORT1, ADI_GPIO_PIN_0 | ADI_GPIO_PIN_1, true);
		adi_gpio_InputEnable(ADI_GPIO_PORT2, ADI_GPIO_PIN_1, true);

    return 0;
}

//=========================== public ==========================================

int16_t DEBUG_MSG(const char* format, ...)
{
#ifdef DEBUG
    va_list arg;
    int16_t done;

	  SEGGER_RTT_WriteString(0, RTT_CTRL_TEXT_WHITE);
    va_start( arg, format);
    done = SEGGER_RTT_vprintf(0, format, &arg); // using SEGGER_RTT_printf.c saves >5kB of flash
	 // done = vprintf(format, arg);              // using stdio.c
    va_end (arg);

    return done;
#else
    return -1;
#endif
}

int16_t ERR_MSG(const char* format, ...)
{
#ifdef DEBUG
    va_list arg;
    int16_t done;

	  SEGGER_RTT_WriteString(0, RTT_CTRL_TEXT_BRIGHT_RED);
    va_start( arg, format);	  
    done = SEGGER_RTT_vprintf(0, format, &arg); // using SEGGER_RTT_printf.c saves >5kB of flash
	 // done = vprintf(format, arg);              // using stdio.c
    va_end (arg);

    return done;
#else
    return -1;
#endif
}

