void vPortExitCritical( void )
{
	uxCriticalNesting--;
	if( uxCriticalNesting == portNO_CRITICAL_NESTING )
	{
		portENABLE_INTERRUPTS();
	}
}

void Hardware_init( void ) {
	portDISABLE_INTERRUPTS();

	NP2_LEDInit();		//Initialise Blue LED
	NP2_LEDOff();		//Turn off Blue LED

	portENABLE_INTERRUPTS();
}

void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime )
{
    TickType_t wakeupTime;

    /* Make sure the SysTick reload value does not overflow the counter. */
    if( xExpectedIdleTime > portNRF_RTC_MAXTICKS - configEXPECTED_IDLE_TIME_BEFORE_SLEEP )
    {
        xExpectedIdleTime = portNRF_RTC_MAXTICKS - configEXPECTED_IDLE_TIME_BEFORE_SLEEP;
    }
    /* Block the scheduler now */
    portDISABLE_INTERRUPTS();

    /* Stop tick events */
    nrf_rtc_int_disable(portNRF_RTC_REG, NRF_RTC_INT_TICK_MASK);

    /* Configure CTC interrupt */
    wakeupTime = nrf_rtc_counter_get(portNRF_RTC_REG) + xExpectedIdleTime;
    wakeupTime &= portNRF_RTC_MAXTICKS;
    nrf_rtc_cc_set(portNRF_RTC_REG, 0, wakeupTime);
    nrf_rtc_event_clear(portNRF_RTC_REG, NRF_RTC_EVENT_COMPARE_0);
    nrf_rtc_int_enable(portNRF_RTC_REG, NRF_RTC_INT_COMPARE0_MASK);

    if( eTaskConfirmSleepModeStatus() == eAbortSleep )
    {
        portENABLE_INTERRUPTS();
    }
    else
    {
        TickType_t xModifiableIdleTime = xExpectedIdleTime;
        configPRE_SLEEP_PROCESSING( xModifiableIdleTime );
        if( xModifiableIdleTime > 0 )
        {
            __DSB();
            do{
                __WFE();
            } while(0 == (NVIC->ISPR[0]));
        }
        configPOST_SLEEP_PROCESSING( xExpectedIdleTime );
        portENABLE_INTERRUPTS();
    }
    // We can do operations below safely, because when we are inside vPortSuppressTicksAndSleep
    // scheduler is already suspended.
    nrf_rtc_int_disable(portNRF_RTC_REG, NRF_RTC_INT_COMPARE0_MASK);
    nrf_rtc_int_enable (portNRF_RTC_REG, NRF_RTC_INT_TICK_MASK);
}

/*
 * The function called by the FreeRTOS IRQ handler, after it has managed
 * interrupt entry.  This function creates a local copy of pxISRFunction before
 * re-enabling interrupts and actually calling the handler pointed to by
 * pxISRFunction.
 */
void vApplicationIRQHandler( void )
{
ISRFunction_t pxISRToCall = pxISRFunction;

	portENABLE_INTERRUPTS();

	/* Call the installed ISR. */
	pxISRToCall();
}

void vPortExitCritical( void )
{
    configASSERT( uxCriticalNesting );
    uxCriticalNesting--;
    if( uxCriticalNesting == 0 )
    {
        portENABLE_INTERRUPTS();
    }
}

static void prvCheckDelayedList( void )
{
CRCB_t *pxCRCB;

	xPassedTicks = xTaskGetTickCount() - xLastTickCount;
	while( xPassedTicks )
	{
		xCoRoutineTickCount++;
		xPassedTicks--;

		/* If the tick count has overflowed we need to swap the ready lists. */
		if( xCoRoutineTickCount == 0 )
		{
			List_t * pxTemp;

			/* Tick count has overflowed so we need to swap the delay lists.  If there are
			any items in pxDelayedCoRoutineList here then there is an error! */
			pxTemp = pxDelayedCoRoutineList;
			pxDelayedCoRoutineList = pxOverflowDelayedCoRoutineList;
			pxOverflowDelayedCoRoutineList = pxTemp;
		}

		/* See if this tick has made a timeout expire. */
		while( listLIST_IS_EMPTY( pxDelayedCoRoutineList ) == pdFALSE )
		{
			pxCRCB = ( CRCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedCoRoutineList );

			if( xCoRoutineTickCount < listGET_LIST_ITEM_VALUE( &( pxCRCB->xGenericListItem ) ) )
			{
				/* Timeout not yet expired. */
				break;
			}

			portDISABLE_INTERRUPTS();
			{
				/* The event could have occurred just before this critical
				section.  If this is the case then the generic list item will
				have been moved to the pending ready list and the following
				line is still valid.  Also the pvContainer parameter will have
				been set to NULL so the following lines are also valid. */
				( void ) uxListRemove( &( pxCRCB->xGenericListItem ) );

				/* Is the co-routine waiting on an event also? */
				if( pxCRCB->xEventListItem.pvContainer )
				{
					( void ) uxListRemove( &( pxCRCB->xEventListItem ) );
				}
			}
			portENABLE_INTERRUPTS();

			prvAddCoRoutineToReadyQueue( pxCRCB );
		}
	}

	xLastTickCount = xCoRoutineTickCount;
}

void SysTick_Handler( void )
{

    portDISABLE_INTERRUPTS();

    tpl_call_counter_tick();

    portENABLE_INTERRUPTS();

}

bool timer_is_ppm_valid(void)
{
  uint16_t sample_len;

  portDISABLE_INTERRUPTS();
  sample_len = g_ppm_state.sample_len;
  portENABLE_INTERRUPTS();

  return sample_len != 0;
}

/*-----------------------------------------------------------*/
void vPortExitCritical(void) {
 /* Interrupts are disabled so we can access the nesting count directly.  If the
  * nesting is found to be 0 (no nesting) then we are leaving the critical
  * section and interrupts can be re-enabled.
  */
  uxCriticalNesting--;
  if (uxCriticalNesting == 0)  {
    portENABLE_INTERRUPTS();
  }
}

void __asmSaveContextAndCallScheduler()
{
	//SysTickIntEnable();
	/* Set a PendSV to request a context switch. */
	*(portNVIC_INT_CTRL) |= portNVIC_PENDSVSET;

	/* This function is also called in response to a Yield(), so we want
	the yield to occur immediately. */
	portENABLE_INTERRUPTS();
}

void vPortExitCritical( void )
{
    if(ulCriticalNesting > portNO_CRITICAL_NESTING) {
        ulCriticalNesting--;
        if( ulCriticalNesting == portNO_CRITICAL_NESTING ) {
            /* Enable all interrupt/exception. */
            portENABLE_INTERRUPTS();
        }
    }
}

void vPortExitCritical( void )
{
	uxCriticalNesting--;
	if( uxCriticalNesting == 0 )
	{
		portENABLE_INTERRUPTS();
		__asm( " dsb" );
		__asm( " isb" );
	}
}

void vPortExitCritical( void )
{
portBASE_TYPE xRunningPrivileged = prvRaisePrivilege();

	uxCriticalNesting--;
	if( uxCriticalNesting == 0 )
	{
		portENABLE_INTERRUPTS();
	}
	portRESET_PRIVILEGE( xRunningPrivileged );
}

/**
  * @brief  Hardware Initialisation.
  * @param  None
  * @retval None
  */
void Hardware_init( void ) {
	
	
	portDISABLE_INTERRUPTS();	//Disable interrupts

	BRD_LEDInit();				//Initialise Blue LED
	BRD_LEDOff();				//Turn off Blue LED

	portENABLE_INTERRUPTS();	//Enable interrupts
	s4295255_sysmon_init();  	//Initialise the system monitor
}

void ICACHE_FLASH_ATTR
vPortExitCritical1( void )
{
	if(NMIIrqIsOn == 0)
	{		
		if( ClosedLv1Isr ==1 )
		{
			ClosedLv1Isr = 0;
			portENABLE_INTERRUPTS();
		}
	}
}

int main( void )
{
	// configure the system
	SystemInit();
	configRedLed();

	servos_Init();
	mems_Init();

    // configure the general UART
    configSerial();

    // configure the mux select line
    muxControlInit();

    // configure the analog sensors
    analogSensorsInit();
    // create the LED task
    if(xTaskCreate(vLedTask, (signed portCHAR*) "LED",128,NULL, 1, &taskHandles[0]) != pdPASS)
    {
    	//TODO: the task was not created, do something
    }

    // create the serialPort task
    if(xTaskCreate(vSerialTask, (signed portCHAR*) "SERIAL",1024,NULL, 1, &taskHandles[1]) != pdPASS)
    {
    	//TODO: the task was not created, do something
    }

    // create the MUX task
    if(xTaskCreate(vMuxTask, (signed portCHAR*) "LED",128,NULL, 1, &taskHandles[2]) != pdPASS)
    {
    	//TODO: the task was not created, do something
    }
    // create the demo task
	if(xTaskCreate(vDemoTask, (signed portCHAR*) "DEMO",1024,NULL, 1, &taskHandles[2]) != pdPASS)
	{

	}
    taskHandles[4] = 0; //TODO: will need to change when we know how many tasks there will be

    // enable the interrupts
    portENABLE_INTERRUPTS();

    // start the scheduler
	vTaskStartScheduler();

    // will only get here if there was insufficient memory to create the idle
    // task.  The idle task is created within vTaskStartScheduler().
	for( ;; );

	return 0; // never gets here
}

void vPortExitCritical( void )
{
BaseType_t xRunningPrivileged = xPortRaisePrivilege();

	configASSERT( uxCriticalNesting );
	uxCriticalNesting--;
	if( uxCriticalNesting == 0 )
	{
		portENABLE_INTERRUPTS();
	}
	vPortResetPrivilege( xRunningPrivileged );
}

void vPortExitCritical( void )
{
    BaseType_t xRunningPrivileged = prvRaisePrivilege();

    configASSERT( uxCriticalNesting );
    uxCriticalNesting--;
    if( uxCriticalNesting == 0 )
    {
        portENABLE_INTERRUPTS();
    }
    portRESET_PRIVILEGE( xRunningPrivileged );
}

void Hardware_init() {

	portDISABLE_INTERRUPTS();	//Disable interrupts

	BRD_LEDInit();
	BRD_LEDOn();
        
        tracks_init();
        
        wheelencoders_init();

	portENABLE_INTERRUPTS();	//Disable interrupts
}

bool getLightValue(unsigned short from, unsigned short inLen, void *inData, unsigned short *outLen, void *outData) {
    *outLen = 4;

    portDISABLE_INTERRUPTS();
    unsigned int lightTempL = lightCountL;
    unsigned int lightTempH = lightCountH;
    portENABLE_INTERRUPTS();

    unsigned long outVal = (((unsigned long) htons(lightTempL)) << 16) + htons(lightTempH);
    memcpy(outData, &outVal, 4);

    return true;
}