static void
prvQueueReceiveTask (void *pvParameters)
{
  unsigned long ulReceivedValue;

  (void) pvParameters;

  for (;;)
    {
      /* Wait until something arrives in the queue - this task will block
         indefinitely provided INCLUDE_vTaskSuspend is set to 1 in
         FreeRTOSConfig.h. */
      xQueueReceive (xQueue, &ulReceivedValue, portMAX_DELAY);

      /*  To get here something must have been received from the queue, but
         is it the expected value?  If it is, toggle the LED. */
      if (ulReceivedValue == 100UL)
        GPIOSetValue (LED_PORT, LED_BIT, 1);
      else
        GPIOSetValue (LED_PORT, LED_BIT, 0);
      xprintf("received\r\n");
      xprintf("time: %u \r\n", xTaskGetUptime());
      log_error("something happened");
      log_debug("do NOT output this line");
    }
}

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 ;
}

int
main(void)
{
  //Set LED1 pin (PIO0_2) as an output
  GPIOSetDir(PIO_GPIO_LED1, PIO_PIN_LED1, GPIO_OUTPUT);

  while (1)
  {
    // Turn LED1 on
    GPIOSetValue(PIO_GPIO_LED1, PIO_PIN_LED1, LED_ON);

    delayMS(50);

    // Turn LED1 off
    GPIOSetValue(PIO_GPIO_LED1, PIO_PIN_LED1, LED_OFF);

    delayMS(150);

    // Turn LED1 on
    GPIOSetValue(PIO_GPIO_LED1, PIO_PIN_LED1, LED_ON);

    delayMS(50);

    // Turn LED1 off
    GPIOSetValue(PIO_GPIO_LED1, PIO_PIN_LED1, LED_OFF);

    delayMS(750);
  }

  return 0;
}

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;
}

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)
{
  volatile int i;
  /* Basic chip initialization is taken care of in SystemInit() called
   * from the startup code. SystemInit() and chip settings are defined
   * in the CMSIS system_<part family>.c file.
   */

  /* NVIC is installed inside UARTInit file. */
  UARTInit (115200, 0);

  /* Initialize GPIO (sets up clock) */
  GPIOInit ();

  /* Set LED port pin to output */
  GPIOSetDir (LED_PORT, LED_BIT, 1);

  while (1)
    {				/* Loop forever */
      if (UARTCount != 0)
	{
	  LPC_UART->IER = IER_THRE | IER_RLS;	/* Disable RBR */
	  UARTSend ((uint8_t *) UARTBuffer, UARTCount);
	  UARTCount = 0;
	  LPC_UART->IER = IER_THRE | IER_RLS | IER_RBR;	/* Re-enable RBR */

	  debug_printf ("Hello World!\n");

	  GPIOSetValue (LED_PORT, LED_BIT, LED_ON);
	  for (i = 0; i < 10000; i++);
	  GPIOSetValue (LED_PORT, LED_BIT, LED_OFF);
	}
    }
}

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);
}

int main (void) {
  uint32_t interval;

 SystemCoreClockUpdate();

  /* Config CLKOUT, mostly used for debugging. */
  CLKOUT_Setup( CLKOUTCLK_SRC_MAIN_CLK );
  LPC_IOCON->PIO0_1 &= ~0x07;	
  LPC_IOCON->PIO0_1 |= 0x01;		/* CLK OUT */

  /* Enable AHB clock to the GPIO domain. */
  LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6);

  /* TEST_TIMER_NUM is either 0 or 1 for 16-bit timer 0 or 1. */
  interval = SystemCoreClock/1000 - 1;
  if ( interval > 0xFFFF )
  {
	interval = 0xFFFF;
  }
  init_timer16(TEST_TIMER_NUM, interval);
  enable_timer16(TEST_TIMER_NUM);

  /* Set port 2_0 to output */
   GPIOSetDir( 2, 0, 1 );

  while (1)                                /* Loop forever */
  {
#if TEST_TIMER_NUM
	/* I/O configuration and LED setting pending. */
	if ( (timer16_1_counter > 0) && (timer16_1_counter <= 200) )
	{
	  GPIOSetValue( 2, 0, 0 );
	}
	if ( (timer16_1_counter > 200) && (timer16_1_counter <= 400) )
	{
	  GPIOSetValue( 2, 0, 1 );
	}
	else if ( timer16_1_counter > 400 )
	{
	  timer16_1_counter = 0;
	}
#else
	/* I/O configuration and LED setting pending. */
	if ( (timer16_0_counter > 0) && (timer16_0_counter <= 200) )
	{
	  GPIOSetValue( 2, 0, 0 );
	}
	if ( (timer16_0_counter > 200) && (timer16_0_counter <= 400) )
	{
	  GPIOSetValue( 2, 0, 1 );
	}
	else if ( timer16_0_counter > 400 )
	{
	  timer16_0_counter = 0;
	}
#endif
  }
}

void power_mgr_set_player(uint8_t enabled) {
	// negated due relay module
	if (enabled) {
		GPIOSetValue( POWER_MGR_PORT, POWER_MGR_PIN_PLAYER, 0);
	}
	else {
		GPIOSetValue( POWER_MGR_PORT, POWER_MGR_PIN_PLAYER, 1);
	}
}

void
nRFCMD_Shutdown (void)
{
  /* set pins to lowest power */
  GPIOSetDir (RF_IRQ_CPU_PORT, RF_IRQ_CPU_PIN, 1);
  GPIOSetValue (RF_IRQ_CPU_PORT, RF_IRQ_CPU_PIN, 1);
  GPIOSetValue (CPU_CE_RF_PORT, CPU_CE_RF_PIN, 0);
  GPIOSetValue (CPU_SWITCH_RF_PORT, CPU_SWITCH_RF_PIN, 0);
}

void power_mgr_set_amp(uint8_t enabled) {
	// negated due relay module
	if (enabled) {
		GPIOSetValue( POWER_MGR_PORT, POWER_MGR_PIN_AMP, 0);
	}
	else {
		GPIOSetValue( POWER_MGR_PORT, POWER_MGR_PIN_AMP, 1);
	}
}

static
void rfid_task(void *pvParameters)
{
	int i;
	static unsigned char data[80];

	/* touch unused Parameter */
	(void) pvParameters;

	/* release reset line after 400ms */
	vTaskDelay( 400 / portTICK_RATE_MS);
	rfid_reset(1);
	/* wait for PN532 to boot */
	vTaskDelay( 100 / portTICK_RATE_MS);

	/* read firmware revision */
	debug_printf("\nreading firmware version...\n");
	data[0] = PN532_CMD_GetFirmwareVersion;
	rfid_execute(&data, 1, sizeof(data));

	/* enable debug output */
	debug_printf("\nenabling debug output...\n");
	WriteRegister(0x6328, 0xFC);
	// select test bus signal
	WriteRegister(0x6321, 6);
	// select test bus type
	WriteRegister(0x6322, 0x07);

	while (1)
	{
		/* wait 100ms */
		vTaskDelay( 100 / portTICK_RATE_MS);

		/* detect cards in field */
		GPIOSetValue(LED_PORT, LED_BIT, LED_ON);
		debug_printf("\nchecking for cards...\n");
		data[0] = PN532_CMD_InListPassiveTarget;
		data[1] = 0x01; /* MaxTg - maximum cards    */
		data[2] = 0x00; /* BrTy - 106 kbps type A   */
		if (((i = rfid_execute(&data, 3, sizeof(data))) >= 11) && (data[1]
				== 0x01) && (data[2] == 0x01))
		{
			debug_printf("card id: ");
			rfid_hexdump(&data[7], data[6]);
		}
		else
			debug_printf("unknown response of %i bytes\n", i);
		GPIOSetValue(LED_PORT, LED_BIT, LED_OFF);

		/* turning field off */
		debug_printf("\nturning field off again...\n");
		data[0] = PN532_CMD_RFConfiguration;
		data[1] = 0x01; /* CfgItem = 0x01           */
		data[2] = 0x00; /* RF Field = off           */
		rfid_execute(&data, 3, sizeof(data));
	}
}

/*****************************************************************************
**   Main Function  main()
******************************************************************************/
int main (void)
{
  /*** The main Function is an endless loop ****/

  // Configure WDT to run from internal WD oscillator at about 8 kHz
  WDTInit();

  init_timer16( 0, TIME_INTERVALmS * 10 );
  enable_timer16( 0 );

  /* Set LED pin to output and make sure it is off */
  GPIOSetDir( LED_PORT, LED_BIT, 1 );
  GPIOSetValue( LED_PORT, LED_BIT, LED_OFF );

  // Check reset status for watchdog reset- if found, blink quickly
  if ((LPC_SYSCON->SYSRSTSTAT & 0x4) == 0x4)
  {
    LPC_SYSCON->SYSRSTSTAT |= 0x4;
    while( 1 ) 
    {
  	  /* I/O configuration and LED setting pending. */
  	  if ( (timer16_0_counter > 0) && (timer16_0_counter <= FAST_LED_TOGGLE_TICKS/2) )
  	  {
  	    GPIOSetValue( LED_PORT, LED_BIT, LED_OFF );
  	  }
  	  if ( (timer16_0_counter > FAST_LED_TOGGLE_TICKS/2) 
			&& (timer16_0_counter <= FAST_LED_TOGGLE_TICKS) )
  	  {
  	    GPIOSetValue( LED_PORT, LED_BIT, LED_ON );
  	  }
  	  else if ( timer16_0_counter > FAST_LED_TOGGLE_TICKS )
  	{
  	    timer16_0_counter = 0;
  	  }
  	}
  }
  else
  { // No watchdog reset- lets blink slowly
  while( 1 )
  {
	  /* I/O configuration and LED setting pending. */
	  if ( (timer16_0_counter > 0) && (timer16_0_counter <= LED_TOGGLE_TICKS/2) )
	  {
	    GPIOSetValue( LED_PORT, LED_BIT, LED_OFF );
	  }
	  if ( (timer16_0_counter > LED_TOGGLE_TICKS/2) && (timer16_0_counter <= LED_TOGGLE_TICKS) )
	  {
	    GPIOSetValue( LED_PORT, LED_BIT, LED_ON );
	  }
	  else if ( timer16_0_counter > LED_TOGGLE_TICKS )
	  {
	    timer16_0_counter = 0;
	  }
    }
  }
}

unsigned short touchXsample(){
  unsigned char rcvbuf[2];
  unsigned char sendval = ADS7843_CTRL_X_SAMPLE;
  GPIOSetValue(ADS7843_CS_GPIO,ADS7843_CS_GPIOBIT, 0);
  SSP0_Send(&sendval, 1); 
  SSP0_Receive(rcvbuf, sizeof rcvbuf);
  GPIOSetValue(ADS7843_CS_GPIO,ADS7843_CS_GPIOBIT, 1);
  unsigned short val = (((unsigned short)rcvbuf[0])&0x7f)<<5;
  return val | (rcvbuf[1])>>3;
}

void init_mfrc500(void)
{
	int i;
	LPC_IOCON->PIO0_4 &= ~0x07;		/* SSP SSEL is a GPIO pin */
	/* port0, bit 15 is set to GPIO output and high */
	GPIOSetDir( PORT0, 4, 1 );
	GPIOSetValue( PORT0, 4, 1 );
	for(i=0; i<500000; i++);
	GPIOSetValue( PORT0, 4, 0 );
}

int main(void) {
	/* Basic chip initialization is taken care of in SystemInit() called
	 * from the startup code. SystemInit() and chip settings are defined
	 * in the CMSIS system_<part family>.c file.
	 */

	/* Initialize 32-bit timer 0. TIME_INTERVAL is defined as 10mS */
	/* You may also want to use the Cortex SysTick timer to do this */
	init_timer32(0, TIME_INTERVAL);
	/* Enable timer 0. nOur interrupt handler will begin incrementing
	 * the TimeTick global each time timer 0 matches and resets.
	 */
	enable_timer32(0);

	/* Initialize GPIO (sets up clock) */
	GPIOInit();


	LPC_IOCON->R_PIO0_11 |= 1;

	/* Set LED port pin to output */
	GPIOSetDir(LED_PORT, LED_BIT, 1);




	synth_init();


	int i;
	for (i = 0; i < 6; i++) {
		synth_channels[i].freq = 100 * i;
		synth_channels[i].amp = 1 << (16 - i);
		synth_channels[i].func = SYNTH_SAW;
	}

	while (1) {
		int tmp = 500 * ADCValue[1]/512;
		for (i = 0; i < 6; i++)
			synth_channels[i].freq = tmp*(i+1);

	}
	while (1) /* Loop forever */
	{
		/* Each time we wake up... */
		/* Check TimeTick to see whether to set or clear the LED I/O pin */
		if ((timer32_0_counter % LED_TOGGLE_TICKS) < (LED_TOGGLE_TICKS / 2)) {
			GPIOSetValue(LED_PORT, LED_BIT, LED_OFF);
		} else {
			GPIOSetValue(LED_PORT, LED_BIT, LED_ON);
		}
		/* Go to sleep to save power between timer interrupts */
		__WFI();
	}
}

/******************************************************************************
 *
 * 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);
}

/*..........................................................................*/
void BSP_displayPhilStat(uint8_t n, char const *stat) {
    if (stat[0] == 'e') {
        GPIOSetValue(LED_PORT, LED_BIT, LED_ON);                 /* LED on  */
    }
    else {
        GPIOSetValue(LED_PORT, LED_BIT, LED_OFF);                /* LED off */
    }

    QS_BEGIN(PHILO_STAT, AO_Philo[n])  /* application-specific record begin */
        QS_U8(1, n);                                  /* Philosopher number */
        QS_STR(stat);                                 /* Philosopher status */
    QS_END()
}

void setDrive(int speed, int turn)
{
	// positive turn means turning right, positive speed means driving forward
	int left, right;
	left = speed-turn;
	right = speed+turn;

	// Set motor speeds
	// Left motor
	if( left >= 0)
	{
		GPIOSetValue( LEFT_DIR_PORT, LEFT_DIR_PIN, INVERT_LEFT_MOTOR );
		if(left > MAX_MOTOR_SPEED) // Speed limit :-)
		{
			left = MAX_MOTOR_SPEED;
		}
		set_speed_left(left);
	}
	else
	{
		GPIOSetValue( LEFT_DIR_PORT, LEFT_DIR_PIN, (INVERT_LEFT_MOTOR ^ 0x01) );
		left = -left;
		if(left > MAX_MOTOR_SPEED) // Speed limit :-)
		{
			left = MAX_MOTOR_SPEED;
		}
		set_speed_left(left);
	}

	// Right motor
	if( right >= 0)
	{
		GPIOSetValue( RIGHT_DIR_PORT, RIGHT_DIR_PIN, INVERT_RIGHT_MOTOR );
		if(right > MAX_MOTOR_SPEED) // Speed limit :-)
		{
			right = MAX_MOTOR_SPEED;
		}
		set_speed_right(right);
	}
	else
	{
		right = -right;
		GPIOSetValue( RIGHT_DIR_PORT, RIGHT_DIR_PIN, (INVERT_RIGHT_MOTOR ^ 0x01) );
		if(right > MAX_MOTOR_SPEED) // Speed limit :-)
		{
			right = MAX_MOTOR_SPEED;
		}
		set_speed_right(right);
	}
}

void
blink (uint8_t times)
{
  while (times)
    {
      times--;

      GPIOSetValue (1, 1, 1);
      pmu_sleep_ms (100);
      GPIOSetValue (1, 1, 0);
      pmu_sleep_ms (200);
    }
  pmu_sleep_ms (500);
}