/**
 * *brief	init trig pin as input, install interrupt handler on event 1
 * IO pins on lpc11xx can be configured for hysteresis ~0.4V, we enable this to avoid
 * multiple interrupts when optocoupler input is fired from a potential noisy and slow input
 * */
void trigInputInit(void)
{
    /* init timers */
    /* avoid triggerning after reset, and allow inputs to settle */
    timeLast1 = 0;
    timeLast2 = 0;
    timeEdge1 = 0;
    timeEdge2 = 0;

    /* allow interrupts on GPIO 1 & 2 */
    NVIC_EnableIRQ(EINT1_IRQn);
    NVIC_EnableIRQ(EINT2_IRQn);

    /* hard coded pins set pullups and hysteresis TODO*/
    LPC_IOCON->PIO2_2  = 0x00000030;
    LPC_IOCON->PIO1_10 = 0x000000B0;


    GPIOSetDir( TRIG_IN_PORT_1, TRIG_IN_PIN_1, 0);
    GPIOSetDir( TRIG_IN_PORT_2, TRIG_IN_PIN_2, 0);

    /* attach pin to falling edge Interrupt */
    GPIOSetInterrupt(TRIG_IN_PORT_1, TRIG_IN_PIN_1, 0, 0, 0);
    GPIOSetInterrupt(TRIG_IN_PORT_2, TRIG_IN_PIN_2, 0, 0, 0);
    /*enable interrupts*/
    GPIOIntEnable( TRIG_IN_PORT_1, TRIG_IN_PIN_1);
    GPIOIntEnable( TRIG_IN_PORT_2, TRIG_IN_PIN_2);
}

void rfid_init(void)
{
	/* reset SSP peripheral */
	LPC_SYSCON->PRESETCTRL = 0x01;

	/* Enable SSP clock */
	LPC_SYSCON->SYSAHBCLKCTRL |= (1 << 11);

	// Enable SSP peripheral
	LPC_IOCON->PIO0_8 = 0x01 | (0x01 << 3); /* MISO, Pulldown */
	LPC_IOCON->PIO0_9 = 0x01; /* MOSI */

	LPC_IOCON->SCKLOC = 0x00; /* route to PIO0_10 */
	LPC_IOCON->JTAG_TCK_PIO0_10 = 0x02; /* SCK */

	/* Set SSP clock to 4.5MHz */
	LPC_SYSCON->SSPCLKDIV = 0x01;
	LPC_SSP->CR0 = 0x0707;
	LPC_SSP->CR1 = 0x0002;
	LPC_SSP->CPSR = 0x02;

	/* Initialize chip select line */
	GPIOSetDir(PN532_CS_PORT, PN532_CS_PIN, 1);
	rfid_cs(1);

	/* Initialize RESET line */
	GPIOSetDir(PN532_RESET_PORT, PN532_RESET_PIN, 1);
	rfid_reset(0);

}

int main (void)
{
  uint8_t ledState;

  //Set LED1 pin as an output
  GPIOSetDir( LED1_PORT, LED1_PIN, GPIO_OUTPUT);
  GPIOSetValue( LED1_PORT, LED1_PIN, LED_OFF);

  //Set SW2 pin as an input
  GPIOSetDir( SW2_PORT, SW2_PIN, GPIO_INPUT);

  //enter forever loop
  while (1)
  {
    //delay a specified period of time (the sample period)
    delayMS(75);

    //check if push-button is pressed
    if (GPIOGetValue(SW2_PORT, SW2_PIN) == SW_PRESSED)
    {
      //toggle LED
	    if (ledState == LED_ON)
	      ledState = LED_OFF;
	    else
        ledState = LED_ON;
      GPIOSetValue( LED1_PORT, LED1_PIN, ledState);

      //wait until push-button is released
	    while(GPIOGetValue(SW2_PORT, SW2_PIN) == SW_PRESSED)
		    ;
	  }
  }
  return 0;
}

error_t pn532_bus_HWInit(void)
{
  #ifdef PN532_DEBUGMODE
  PN532_DEBUG("Initialising I2C%s", CFG_PRINTF_NEWLINE);
  #endif
  i2cInit(I2CMASTER);

  // Set reset pin as output and reset device
  GPIOSetDir(CFG_PN532_RSTPD_PORT, CFG_PN532_RSTPD_PIN, 1);
  #ifdef PN532_DEBUGMODE
  PN532_DEBUG("Resetting the PN532%s", CFG_PRINTF_NEWLINE);
  #endif
  LPC_GPIO->CLR[CFG_PN532_RSTPD_PORT] = (1 << CFG_PN532_RSTPD_PIN);
  systickDelay(400);
  LPC_GPIO->SET[CFG_PN532_RSTPD_PORT] = (1 << CFG_PN532_RSTPD_PIN);

  // Wait for the PN532 to finish booting
  systickDelay(100);

  // Ping the I2C device first to see if it exists!
  if (i2cCheckAddress(PN532_I2C_ADDRESS) == false)
  {
    #ifdef PN532_DEBUGMODE
    PN532_DEBUG("Can't find PN532 on the I2C bus%s", CFG_PRINTF_NEWLINE);
    #endif
    return ERROR_I2C_DEVICENOTFOUND;
  }

  // Set IRQ pin to input
  GPIOSetDir(CFG_PN532_I2C_IRQPORT, CFG_PN532_I2C_IRQPIN, 0);

  return ERROR_NONE;
}

void rfid_init(void)
{
	/* reset SSP peripheral */
	LPC_SYSCON->PRESETCTRL = 0x01;

	/* Enable SSP clock */
	LPC_SYSCON->SYSAHBCLKCTRL |= (1 << 11);

	// Enable SSP peripheral
	LPC_IOCON->PIO0_8 = 0x01 | (0x01 << 3); /* MISO, Pulldown */
	LPC_IOCON->PIO0_9 = 0x01; /* MOSI */

	LPC_IOCON->SCKLOC = 0x00; /* route to PIO0_10 */
	LPC_IOCON->JTAG_TCK_PIO0_10 = 0x02; /* SCK */

	/* Set SSP clock to 4.5MHz */
	LPC_SYSCON->SSPCLKDIV = 0x01;
	LPC_SSP->CR0 = 0x0707;
	LPC_SSP->CR1 = 0x0002;
	LPC_SSP->CPSR = 0x02;

	/* Initialize chip select line */
	rfid_cs(1);
	GPIOSetDir(PN532_CS_PORT, PN532_CS_PIN, 1);

	/* Initialize RESET line */
	rfid_reset(0);
	GPIOSetDir(PN532_RESET_PORT, PN532_RESET_PIN, 1);

	/* Create PN532 task */
	xTaskCreate(rfid_task, (const signed char*) "RFID", TASK_RFID_STACK_SIZE,
			NULL, TASK_RFID_PRIORITY, NULL);
}

/*****************************************************************************
** Function name:		GPIOInit
**
** Descriptions:		Initialize GPIO, install the
**						GPIO interrupt handler
**
** parameters:			None
** Returned value:		true or false, return false if the VIC table
**						is full and GPIO interrupt handler can be
**						installed.
** 
*****************************************************************************/
void GPIOInit( void )
{
	//int i;
  /* Enable AHB clock to the GPIO domain. */
  LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6);

	LPC_IOCON->R_PIO0_11  &= ~0x07;
  LPC_IOCON->R_PIO0_11  |= 0x01;

	LPC_IOCON->R_PIO1_0  &= ~0x07;
  LPC_IOCON->R_PIO1_0  |= 0x01;
	
  LPC_IOCON->R_PIO1_1  &= ~0x07;
  LPC_IOCON->R_PIO1_1  |= 0x01;

	LPC_IOCON->R_PIO1_2  &= ~0x07;
  LPC_IOCON->R_PIO1_2  |= 0x01;
	
	LPC_IOCON->PIO0_4 = 0x100;
	LPC_IOCON->PIO0_5 = 0x100;
	
	//NodeId_Adress
	LPC_GPIO[PORT2]->DIR &= ~(0xDC0); //ADDR0-4
	GPIOSetDir(PORT1,10,E_IO_INPUT);		//ADDR5
	GPIOSetDir(PORT1,11,E_IO_INPUT);	//ADDR6
	
	GPIOSetDir(PORT1, 2, E_IO_INPUT);	//EINT

  /* Set up NVIC when I/O pins are configured as external interrupts. */
  //NVIC_EnableIRQ(EINT0_IRQn);
  //NVIC_EnableIRQ(EINT1_IRQn);
  //NVIC_EnableIRQ(EINT2_IRQn);
  //NVIC_EnableIRQ(EINT3_IRQn);
  return;
}

int main(void) {
	//GPIOInit();
	SysTick_Config( SystemCoreClock/1000 );
	SysTick->CTRL &= (0 << 1);
	//initDrive();
	ADCInit( ADC_CLK );
	//initReflex();
	//GPIOSetDir( 3, 2, 1 );
	//GPIOSetValue( 3, 2, 0);
	GPIOSetDir( LED_PORT, LED_BIT, 1 );
	GPIOSetValue( LED_PORT, LED_BIT, LED_OFF );
	GPIOSetDir( 3, 2, 1 );
	// Enter an infinite loop, just incrementing a counter
	volatile static int i = 0 ;
	while(1)
	{
		//uint16_t adcValue = ADCRead( 5 );
		if(reflexRead())//adcValue > 100)
		{
			GPIOSetValue( LED_PORT, LED_BIT, LED_ON );
		}
		else
		{
			GPIOSetValue( LED_PORT, LED_BIT, LED_OFF );
		}
		//i++;
	}
	return 0 ;
}

/*****************************************************************************
**   Main Function  main()
******************************************************************************/
int main (void)
{
  SystemInit();

  GPIOInit();

  /* use port0_1 as input event, interrupt test. */
  GPIOSetDir( PORT0, 1, 0 );
  /* port0_1, single trigger, active high. */
  GPIOSetInterrupt( PORT0, 1, 0, 0, 0 );
  GPIOIntEnable( PORT0, 1 );

  /* use port1_1 as input event, interrupt test. */
  GPIOSetDir( PORT1, 1, 0 );
  /* port0_1, single edge trigger, active high. */
  GPIOSetInterrupt( PORT1, 1, 0, 0, 0 );
  GPIOIntEnable( PORT1, 1 );

  /* use port2_1 as input event, interrupt test. */
  GPIOSetDir( PORT2, 1, 0 );
  /* port0_1, single edge trigger, active high. */
  GPIOSetInterrupt( PORT2, 1, 0, 0, 0 );
  GPIOIntEnable( PORT2, 1 );

  /* use port3_1 as input event, interrupt test. */
  GPIOSetDir( PORT3, 1, 0 );
  /* port0_1, single edge trigger, active high. */
  GPIOSetInterrupt( PORT3, 1, 0, 0, 0 );
  GPIOIntEnable( PORT3, 1 );

  while( 1 );
}

void flashLight(int timeMS, int OnOrOff)
{
	GPIOSetDir( LED_PORT, LED_BIT, OnOrOff );
	//delay for time
	delaySysTick(timeMS/10);
	GPIOSetDir( LED_PORT, LED_BIT, 0 );
}

static void prvSetupHardware( void )
{
extern unsigned long _vStackTop[], _pvHeapStart[];
unsigned long ulInterruptStackSize;

	/* Initialize GPIO (sets up clock) */
	GPIOInit();
	/* Set LED port pin to output */
	GPIOSetDir(LED_PORT, LED_GREEN_BIT, 1);
	GPIOSetDir(LED_PORT, LED_RED_BIT, 1);
	GPIOSetValue(LED_PORT, LED_GREEN_BIT, 0);
	GPIOSetValue(LED_PORT, LED_RED_BIT, 0);

	PumpsInit();
	FlowrateInit();
	PacketInit(230400);

	/* The size of the stack used by main and interrupts is not defined in
	the linker, but just uses whatever RAM is left.  Calculate the amount of
	RAM available for the main/interrupt/system stack, and check it against
	a reasonable number.  If this assert is hit then it is likely you don't
	have enough stack to start the kernel, or to allow interrupts to nest.
	Note - this is separate to the stacks that are used by tasks.  The stacks
	that are used by tasks are automatically checked if
	configCHECK_FOR_STACK_OVERFLOW is not 0 in FreeRTOSConfig.h - but the stack
	used by interrupts is not.  Reducing the conifgTOTAL_HEAP_SIZE setting will
	increase the stack available to main() and interrupts. */
	ulInterruptStackSize = ( ( unsigned long ) _vStackTop ) - ( ( unsigned long ) _pvHeapStart );
	configASSERT( ulInterruptStackSize > 350UL );

	/* Fill the stack used by main() and interrupts to a known value, so its
	use can be manually checked. */
	memcpy( ( void * ) _pvHeapStart, ucExpectedInterruptStackValues, sizeof( ucExpectedInterruptStackValues ) );
}

int main (void)
{
  draw_lcd_t lcd;

  // outputs
  GPIOSetDir(OLED_SSEL_PORT, OLED_SSEL_PIN, GPIO_OUTPUT);
  GPIOSetDir(OLED_DC_PORT, OLED_DC_PIN, GPIO_OUTPUT);
  GPIOSetDir(OLED_RESET_PORT, OLED_RESET_PIN, GPIO_OUTPUT);

  GPIOSetValue(OLED_SSEL_PORT, OLED_SSEL_PIN, 1);
  GPIOSetValue(OLED_DC_PORT, OLED_DC_PIN, 1);
  GPIOSetValue(OLED_RESET_PORT, OLED_RESET_PIN, 1);

  SSP0Init(6000000);

  printf("\nInitializing oled driver...");
  oled_init(&lcd);

  rainbow(&lcd);

  //enter forever loop -
  while (1) {
  }

  return 0;
}

void power_mgr_init() {
   GPIOSetDir( POWER_MGR_PORT, POWER_MGR_PIN_PLAYER, 1);
   GPIOSetDir( POWER_MGR_PORT, POWER_MGR_PIN_AMP, 1);

   // set default value (0 means active due relay module)
   GPIOSetValue( POWER_MGR_PORT, POWER_MGR_PIN_PLAYER, 0);
   GPIOSetValue( POWER_MGR_PORT, POWER_MGR_PIN_AMP, 0);
}

/******************************************************************************
 *
 * Description:
 *    Initialize Temp Sensor driver
 *
 * Params:
 *   [in] getMsTicks - callback function for retrieving number of elapsed ticks
 *                     in milliseconds
 *
 *****************************************************************************/
void temp_init (uint32_t (*getMsTicks)(void))
{
#ifdef TEMP_USE_P0_2
    GPIOSetDir( PORT0, 2, 0 );
#else
    GPIOSetDir( PORT1, 5, 0 );
#endif
    getTicks = getMsTicks;
}

/******************************************************************************
 *
 * Description:
 *    Initialize RGB driver
 *
 *****************************************************************************/
void rgb_init (void)
{
    GPIOSetDir( PORT1, 9, 1 );
    GPIOSetDir( PORT1, 2, 1 );
    GPIOSetDir( PORT1, 10, 1 );

    LPC_IOCON->PIO1_10 = LPC_IOCON->PIO1_10 & ~0x7;
    LPC_IOCON->PIO1_9 = LPC_IOCON->PIO1_9 & ~0x7;
    //DR:
    LPC_IOCON->R_PIO1_2 = (LPC_IOCON->R_PIO1_2 & ~0x7) | 0x1;
}

void touchInit(){
  SSP0_IOConfig(); 
  GPIOSetDir(ADS7843_CS_GPIO,ADS7843_CS_GPIOBIT,1);  //out
  GPIOSetDir(ADS7843_INT_GPIO,ADS7843_INT_GPIOBIT,0); //in put
  GPIOSetInterrupt(ADS7843_INT_GPIO,ADS7843_INT_GPIOBIT, 0, 0, 0); //signal fall edge 
  //GPIOIntEnable(ADS7843_INT_GPIO, ADS7843_INT_GPIOBIT);
  GPIOSetValue(ADS7843_CS_GPIO,ADS7843_CS_GPIOBIT,1); //output hight
  SSP0_Init();   //spi_clk is 1mhz
  touchTimerInit();
  
}

/******************************************************************************
 *
 * Description:
 *    Initialize the ISL29003 Device
 *
 * Params:
 *   [in] baudRate - the baud rate to use
 *   [in] chan - the channel to use. Channel A connected to RS232 port.
 *               Channel B connected to J53.
 *
 *****************************************************************************/
void uart2_init (uint32_t baudRate, uart2_channel_t chan)
{

    GPIOSetDir(PORT0, 9, 1);
    GPIOSetDir(PORT2, 8, 1);

    GPIOSetValue( PORT0, 9, 1 ); // SI-A1
    GPIOSetValue( PORT2, 8, 1 ); // CS#-A0

    channel = chan;
    uart2_setBaudRate(baudRate);
}

/**
 * Initialize encoders on robot
 */
void encoder_init(void) {
  // enable IRQ from gpio port
  // set left&right encoder pins as inputs
  GPIOSetDir(PORT2, 6, GPIO_INPUT);
  GPIOSetDir(PORT2, 11, GPIO_INPUT);

  // initialize pins to generate interrupts (falling edge sensitive).
  LPC_GPIO2->IEV &= ~(1 << 6);  // set falling edge sensitive
  LPC_GPIO2->IE |= (1 << 6);  // enable interrupt from PIO2_6
  LPC_GPIO2->IEV &= ~(1 << 11);  // set falling edge sensitive
  LPC_GPIO2->IE |= (1 << 11);  // enable interrupt from PIO2_11
  NVIC_EnableIRQ(EINT2_IRQn);
}

void reflexDigitalInit()
{
	LPC_IOCON->R_PIO0_11 &= ~0x1F;			// - remove bits 0,1 & 2 and pull resistors
	LPC_IOCON->R_PIO0_11 |= 0x01;
	LPC_IOCON->R_PIO1_0 &= ~0x1F;			// - remove bits 0,1 & 2 and pull resistors
	LPC_IOCON->R_PIO1_0 |= 0x01;
	LPC_IOCON->R_PIO1_1 &= ~0x1F;			// - remove bits 0,1 & 2 and pull resistors
	LPC_IOCON->R_PIO1_1 |= 0x01;
	LPC_IOCON->R_PIO1_2 &= ~0x1F;			// - remove bits 0,1 & 2 and pull resistors
	LPC_IOCON->R_PIO1_2 |= 0x01;
	GPIOSetDir( REFLEX_1_PORT, REFLEX_1_BIT, 0 );
	GPIOSetDir( REFLEX_2_PORT, REFLEX_2_BIT, 0 );
	GPIOSetDir( REFLEX_3_PORT, REFLEX_3_BIT, 0 );
	GPIOSetDir( REFLEX_4_PORT, REFLEX_4_BIT, 0 );
}

void initLed()
{
	unsigned long timeNow;

	/* update time */
	timeNow = millis();

	/* Set LED port p1.11 to output */
	GPIOSetDir(LED_PORT, LED_PIN, 1);
	/* LED off */
	GPIOSetValue(LED_PORT, LED_PIN, 0);

	/* set update times accordingly */
	ledStatus[0].index = 0;
	ledStatus[1].index = 0;
	ledStatus[2].index = 0;
	ledStatus[3].index = 0;

	/* somehow fails during init - TODO*/
	ledStatus[0].repeatPattern = 1;
	ledStatus[1].repeatPattern = 1;
	ledStatus[2].repeatPattern = 0;
	ledStatus[3].repeatPattern = 1;
	ledStatus[4].repeatPattern = 1;

	ledStatus[0].nextLedUpdateTime = timeNow + ledStatus[0].timing[ledStatus[0].index].nextLedUpdateTime;
	ledStatus[1].nextLedUpdateTime = timeNow + ledStatus[1].timing[ledStatus[1].index].nextLedUpdateTime;
	ledStatus[2].nextLedUpdateTime = timeNow + ledStatus[2].timing[ledStatus[2].index].nextLedUpdateTime;
	ledStatus[3].nextLedUpdateTime = timeNow + ledStatus[3].timing[ledStatus[3].index].nextLedUpdateTime;
	ledStatus[4].nextLedUpdateTime = timeNow + ledStatus[4].timing[ledStatus[4].index].nextLedUpdateTime;

}

error_t chb_drvr_init()
{
    // config SPI for at86rf230 access
    chb_spi_init();

    // Set sleep and reset as output
    GPIOSetDir(CFG_CHIBI_SLPTRPORT, CFG_CHIBI_SLPTRPIN, 1);
    GPIOSetDir(CFG_CHIBI_RSTPORT, CFG_CHIBI_RSTPIN, 1);

    // configure IOs
    LPC_GPIO->SET[CFG_CHIBI_SLPTRPORT] = (1 << CFG_CHIBI_SLPTRPIN); // Set sleep high
    LPC_GPIO->SET[CFG_CHIBI_RSTPORT] = (1 << CFG_CHIBI_RSTPIN);     // Set reset high

    // config radio
    return chb_radio_init();
}