/**
This function disables filesystem (if enabled), serial port (if enabled) and
puts the processor in deep sleep mode.<br>
Wakeup occurs when PA.0 goes high, but instead of sleep(), a new boot happens.
<br>This function does not return.<br>
WARNING: close all files before using this function, since it unmounts the
filesystem.<br>
When in shutdown mode, power consumption of the miosix board is reduced to ~
5uA??, however, true power consumption depends on what is connected to the GPIO
pins. The user is responsible to put the devices connected to the GPIO pin in the
minimal power consumption mode before calling shutdown(). Please note that to
minimize power consumption all unused GPIO must not be left floating.
*/
void shutdown()
{
    ioctl(STDOUT_FILENO,IOCTL_SYNC,0);

    #ifdef WITH_FILESYSTEM
    FilesystemManager::instance().umountAll();
    #endif //WITH_FILESYSTEM

    disableInterrupts();
    //The display could be damaged if left on but without refreshing it
	typedef Gpio<GPIOB_BASE,8>  dispoff;//DISPOFF signal to display
	dispoff::high();
    
    /*
    Removed because low power mode causes issues with SWD programming
    RCC->APB1ENR |= RCC_APB1ENR_PWREN; //Fuckin' clock gating...  
    RCC_SYNC();
    PWR->CR |= PWR_CR_PDDS; //Select standby mode
    PWR->CR |= PWR_CR_CWUF;
    PWR->CSR |= PWR_CSR_EWUP; //Enable PA.0 as wakeup source
    
    SCB->SCR |= SCB_SCR_SLEEPDEEP;
    __WFE();
    NVIC_SystemReset();
    */
    for(;;) ;
}

/**
  * @brief  Main program.
  * @param  None
  * @retval None
  */
void main(void)
{
  u8 l_cnt;
  disableInterrupts();
  Config();
  Errors_Init();
  enableInterrupts();
  Goto_HALT();
  
  while (1)
  {
    if((btn_pressed != BUTTON1) && (btn_pressed != BUTTON2))
    {
      DELAY_STOP;
      goto sleep;
    }
    
    while(!DELAY_EXPIRED);           // wait for power-up delay to expire (~20ms)
    DELAY_STOP;
    
    if(btn_pressed == BUTTON1)
    {
      RF_Send(RFCMD_HEATING_ON);
    }
    else if(btn_pressed = BUTTON2)
    {
      RF_Send(RFCMD_HEATING_OFF);
    }
    
    sleep:
    Goto_HALT();
  }
}

void TIM2_IRQHandler(void)
{
  /* 10hz frequency */

  /* todo: reduce the scope by capturing variables */
  disableInterrupts();

  /* forward */
  if ((dir != 1) || (speed <= 0)) goto on_done;

  if (lenc < speed) lpwm += 2; /* I_up */
  else if (lenc > speed) lpwm -= 2;

  if (renc < speed) rpwm += 2;
  else if (renc > speed) rpwm -= 2;

  err = clamp(err + lenc - renc, -128, 128);

  /* reset encoder counters */
  lenc = 0;
  renc = 0;

  lpwm = clamp(CONFIG_START_POWER_FWD + lpwm - err, 0, 255);
  rpwm = clamp(CONFIG_START_POWER_FWD + rpwm - err, 0, 255);

  set_lpwm(lpwm);
  set_rpwm(rpwm);

 on_done:
  TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
  enableInterrupts();
}

IOReturn WLCard::
_reset()
{
    if (_doCommand(wlcInit, 0) != kIOReturnSuccess) {
        WLLogErr("WLCard::_reset: _doCommand(wlcInit, 0) failed\n");
        return kIOReturnError;
    }

    if (setValue(0xFC00, 0x3) != kIOReturnSuccess) {
        WLLogErr("WLCard::_reset: could not set port type\n");
        return kIOReturnError;
    }

    disableInterrupts();
    ackAllInterrupts();

    /*
     * Write and check a magic number to the Software0 register
     */
    UInt16 magic = 0x1ee7;
    setRegister(wlSwSupport0, magic);
    if (getRegister(wlSwSupport0) != 0x1ee7) {
        WLLogCrit("WLCard::_reset: Magic check failed\n");
        return kIOReturnError;
    }   
    
    /*
     * Set list of interesting events
     */
    _interrupts = wleRx;

    _enable();

    return kIOReturnSuccess;
}

/***    uint32_t WS2812TimerService(uint32_t curTime)
 *
 *    Parameters:
 *          The current core timer time
 *
 *    Return Values:
 *          The next core timer time to be called
 *
 *    Description:
 *          This is the CoreTimer routine to handle refreshing the 
 *          WS2812. This gets called every TICKSPERREFRESH
 *          unless it is behind on a refresh because of external factors
 *          then it is called every TICKSPERSHORTCHECK until it is refreshed.
 *
 * ------------------------------------------------------------ */
uint32_t WS2812TimerService(uint32_t curTime)
{
    uint32_t deltaTime = curTime - tWS2812LastRun;
 
    // it is time to refresh
    if(!fWS2812Updating && !DCH0CONbits.CHEN && deltaTime >= TICKSPERREFRESH)
    {
        uint32_t intState = disableInterrupts();
        DCH1CONbits.CHEN = 0;
        DCH0CONbits.CHEN = 1;
        restoreInterrupts(intState);

        tWS2812LastRun += (deltaTime / TICKSPERREFRESH) * TICKSPERREFRESH;
        return(tWS2812LastRun + TICKSPERREFRESH);
    }

    // if this is in a holding pattern for a really long time
    // don't let delta get too big and wrap the uint32_t counter
    if(deltaTime >= (2 * TICKSPERREFRESH))
    {
        tWS2812LastRun += TICKSPERREFRESH;
        deltaTime -= TICKSPERREFRESH;
    }

    // if we get here, we are trying to refresh and are running behind
    // check more often to get the refresh done.
    return(tWS2812LastRun + (((deltaTime / TICKSPERSHORTCHECK) + 1) * TICKSPERSHORTCHECK));
}

/**
  ******************************************************************************
  * @brief Modify the tick values for specific cases when the H/W timer doesn't
  * work (halt, ...).
  * @param[in] adjust_delay Time to add to the ticks (unit is 500µs).
  * @retval None
  ******************************************************************************
  */
void TSL_Timer_Adjust(uint32_t adjust_delay)
{

  disableInterrupts();

  do
  {
    if (adjust_delay > TICK_FACTOR_10MS)
    {
      TSL_Tick_Base += TICK_FACTOR_10MS;
      adjust_delay -= TICK_FACTOR_10MS;
      TSL_Timer_Check_10ms_Tick();
    }
    else
    {
      TSL_Tick_Base++;
      adjust_delay--;
      TSL_Timer_Check_10ms_Tick();
    }
  }
  while (adjust_delay);

  enableInterrupts();

}

/*
 Function: Measures actual radiation value during specified time. Maximum time is 60 seconds
 Returns: 
	RadiationValueCPM: radiation value in CPM
 Parameters: 
	time: time while radiation is measured. Time must be in milliseconds
 Values: 
*/
float WaspSensorRadiation::getCPM(long time){

	float k=0;
	float minute = 60000;
	long previous=millis();

	while( (millis()-previous<time) )
    {
		if( intFlag & RAD_INT){
			disableInterrupts(RAD_INT);
			intFlag &= ~(RAD_INT);
			countPulse();

			while(!digitalRead(RAD_INT_PIN_MON));
    			enableInterrupts(RAD_INT);
  		}
    
    	// Condition to avoid an overflow (DO NOT REMOVE)
    	if( millis()-previous < 0 ) previous=millis();
    }
	k = (minute/time);
   	radiationValueCPM = k*count  ;
   	timePreviousMeassure = millis();
	ledBar(k*count);  
   	count = 0;

	return radiationValueCPM;
}

/**
 * Disable Interrupts without without deallocating structures.
 */
void InterruptableSensorBase::DisableInterrupts() {
  if (StatusIsFatal()) return;
  wpi_assert(m_interrupt != nullptr);
  int32_t status = 0;
  disableInterrupts(m_interrupt, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

void mcs_turn(int32_t a)
{
  /* a the angle in degrees */

  /* degrees to ticks */
  uint32_t enc = (abs(a) * 10) / 37 - 3;

  disableInterrupts();

  /* turn left or right */
  if (a < 0)
  {
    ldir = 0;
    rdir = 1;
  }
  else
  {
    ldir = 1;
    rdir = 0;
  }

  do_encoder(enc);

  enableInterrupts();
}

/**
  * @brief ECS algorithm on a group of objects
  * The ECS is only performed if at least an object is in Release state and
  * if no objects are in active states (Prox, Detect or Touch)
  * An optional delay is added after the ECS condition (all sensors in Release state) is reached.
  * @param[in] objgrp Pointer to the objects group to process
  * @retval Status
  */
TSL_Status_enum_T TSL_ecs_Process(CONST TSL_ObjectGroup_T *objgrp)
{
  TSL_tKCoeff_T MyKcoeff;
  TSL_Status_enum_T retval;
  static TSL_tIndex_T exec = 0;
#if TSLPRM_ECS_DELAY > 0
  static TSL_tIndex_T wait = 0;
  static TSL_tTick_ms_T start_time;
#endif

  if ((objgrp->StateMask & TSL_STATE_RELEASE_BIT_MASK) && !(objgrp->StateMask & TSL_STATEMASK_ACTIVE))
  {
#if TSLPRM_ECS_DELAY > 0
    if (!wait)
    {
      disableInterrupts();
      start_time = TSL_Globals.Tick_ms; // Save the current time
      enableInterrupts();
      wait = 1;
      exec = 0;
    }
#else
    exec = 1;
#endif
  }
  else
  {
#if TSLPRM_ECS_DELAY > 0
    wait = 0;
#endif
    exec = 0;
  }

#if TSLPRM_ECS_DELAY > 0
  if ((wait) && (!exec))
  {
    // Execute the ECS only when the delay has elapsed
    if (TSL_tim_CheckDelay_ms(TSLPRM_ECS_DELAY, &start_time) == TSL_STATUS_OK)
    {
      exec = 1;
    }
  }
#endif

  if (exec)
  {
    // Calculate the K coefficient
    MyKcoeff = TSL_ecs_CalcK(objgrp, TSLPRM_ECS_K_SLOW, TSLPRM_ECS_K_FAST);
    // Process the objects
    TSL_ecs_ProcessK(objgrp, MyKcoeff);
    retval = TSL_STATUS_OK;
  }
  else
  {
    retval = TSL_STATUS_BUSY;
  }

  return retval;
}

void LowPower_::sleepWithTheDog() {
    WDTCONSET = 1<<15; // Turn on
    WDTCONSET = 0x01; // Kick the dog!
    int r = disableInterrupts();
    enterSleepMode();
    restoreInterrupts(r);
    WDTCONCLR = 1<<15; // Turn off
}

// Set a new PWM value for a given pin
// Primarily this consists of updating the PWMValue and then 
// re-sorting the list of active pins by removing and then adding
// the pin to the linked list.
int32_t SoftPWMServoRawWrite(uint32_t Pin, uint32_t Value, bool PinType)
{
    int i;
    int32_t intr;

    // Insert our ISR handler, if it's not already there
    if (!Initalized)
    {
        SoftPWMServoInit();
    }

    // Limit check the inputs
    if (Value > FrameTime)
    {
        Value = FrameTime;
    }
    if (Pin > SOFTPWMSERVO_MAX_PINS)
    {
        return SOFTPWMSERVO_ERROR;
    }
    
    // And if this pin already has this PWM Value, then don't do anything.
    if (Value == Chan[ActiveBuffer][Pin].PWMValue)
    {
        return SOFTPWMSERVO_OK;
    }

    // The easy way to prevent the ISR from doing a buffer swap while
    // we're in the middle of this is to disable interrupts during 
    // the time that we're mucking with the list.
    // TODO: Switch to using 3 buffers - one 'active' that the ISR
    // is currently using, one 'primed' that the mainline code doesn't
    // touch but has updates and is ready to be swapped before the
    // next rising edge, and then 'inactive' which is the one we modify
    // with here. Maybe we wouldn't need to disable interrupts then.
    intr = disableInterrupts();

    // If needed, copy the values from the previously active buffer
    // into the inactive buffer before we begin work on it.
    CopyBuffers();
 
    // Always make sure this pin's enabled
    SoftPWMServoPinEnable(Pin, PinType);

    // Update the PWM value for this pin
    Chan[InactiveBuffer][Pin].PWMValue = Value;
    Chan[InactiveBuffer][Pin].IsServo = PinType;
    
    // Remove it from the list
    Remove(Pin);
    
    // And add it back in the list, in the right place (in time)
    Add(Pin);    

    restoreInterrupts(intr);  

    return SOFTPWMSERVO_OK;
}

void errorHandler(Error e)
{
    // Here we must be careful since this function can be called within an
    // interrupt routine, and disabling interrupts within an interrupt
    // routine must be avoided.
    bool interrupts=areInterruptsEnabled();
    if(interrupts) disableInterrupts();

    bool isUnrecoverable=false;
    //Recoverable errors
    switch(e)
    {
    case INVALID_PARAMETERS:
        IRQerrorLog("\r\n***Invalid parameter\r\n");
        break;
    case PROPAGATED_EXCEPTION:
        IRQerrorLog("\r\n***An exception propagated through a thread\r\n");
        break;
    case MUTEX_UNLOCK_NOT_OWNER:
        IRQerrorLog("\r\n***unlock() called on a non locked mutex\r\n");
    default:
        isUnrecoverable=true;
    }
    //Unrecoverable errors
    switch(e)
    {

    case OUT_OF_MEMORY:
        IRQerrorLog("\r\n***Out of memory\r\n");
        break;
    case STACK_OVERFLOW:
        IRQerrorLog("\r\n***Stack overflow\r\n");
        break;
    case UNEXPECTED:
        IRQerrorLog("\r\n***Unexpected error\r\n");
        break;
    case PAUSE_KERNEL_NESTING:
        IRQerrorLog("\r\n***Pause kernel nesting\r\n");
        break;
    case DISABLE_INTERRUPTS_NESTING:
        IRQerrorLog("\r\n***Disable interrupt nesting\r\n");
        break;
    case MUTEX_DEADLOCK:
        IRQerrorLog("\r\n***Deadlock\r\n");
        break;
    case NESTING_OVERFLOW:
        IRQerrorLog("\r\n***Nesting overflow\r\n");
        break;
    case INTERRUPTS_ENABLED_AT_BOOT:
        IRQerrorLog("\r\n***Interrupts enabled at boot\r\n");
        break;
    default:
        break;
    }
    if(isUnrecoverable) miosix_private::IRQsystemReboot();
    if(interrupts) enableInterrupts();
}

void mcs_stop(void)
{
  disableInterrupts();

  set_lpwm(128);
  set_rpwm(128);

  enableInterrupts();
}

inline void Interpreter::subclassWindow(OTE* window, HWND hWnd)
{
	ASSERT(!ObjectMemoryIsIntegerObject(window));
	// As this is called from an external entry point, we must ensure that OT/stack overflows
	// are handled, and also that we catch the SE_VMCALLBACKUNWIND exceptions
	__try
	{
		bool bDisabled = disableInterrupts(true);
		Oop retVal = performWith(Oop(window), Pointers.subclassWindowSymbol, Oop(ExternalHandle::New(hWnd)));
		ObjectMemory::countDown(retVal);
		ASSERT(m_bInterruptsDisabled);
		disableInterrupts(bDisabled);
	}
	__except (callbackExceptionFilter(GetExceptionInformation()))
	{
		trace("WARNING: Unwinding Interpreter::subclassWindow(%#x, %#x)\n", window, hWnd);
	}
}

int inthook_UnhookInterrupt(unsigned char intnr)
{
	if (InterruptHook[intnr].hooked)
	{
		//it's hooked, try to unhook
		DbgPrint("cpu %d : interrupt %d is hooked\n",cpunr(),intnr);
		if (InterruptHook[intnr].dbvmInterruptEmulation)
		{
			if (intnr==1)
				vmx_redirect_interrupt1(virt_differentInterrupt, 1, 0, 0);
			else if (intnr==3)
				vmx_redirect_interrupt3(virt_differentInterrupt, 3, 0, 0); 
			else
				vmx_redirect_interrupt14(virt_differentInterrupt, 14, 0, 0); 

			return TRUE; //that's all we need
		}

		//still here so not a dbvm hook, unhook the old way and hope nothing has interfered

		{
			INT_VECTOR newVector;
			
			
			//newVector.bUnused=0;
			/*
			newVector.gatetype=6; //interrupt gate
			newVector.gatesize=1; //32-bit
			newVector.zero=0;
			newVector.DPL=0;
			newVector.P=1;
			*/
			newVector.wHighOffset=(WORD)((DWORD)(InterruptHook[intnr].originalEIP >> 16));
			newVector.wLowOffset=(WORD)InterruptHook[intnr].originalEIP;
			newVector.wSelector=(WORD)InterruptHook[intnr].originalCS;

#ifdef AMD64
			newVector.TopOffset=(InterruptHook[intnr].originalEIP >> 32);
			newVector.Reserved=0;
#endif

			{
				IDT idt;	
				GetIDT(&idt);

				
				newVector.bAccessFlags=idt.vector[intnr].bAccessFlags;

				disableInterrupts();
				idt.vector[intnr]=newVector;
				enableInterrupts();				
			}

			DbgPrint("Restored\n");
		}
	}

	/**
  * @brief  Writes buffer of bytes to memory
	* @param address : address of the first byte
	* @param buff : pointer to a data buffer
	* @param size : number of bytes to write
  * @retval : None
  */	
	void ee_writeBuff(uint32_t address,uint8_t *buff,uint16_t size){
		uint16_t i;
			disableInterrupts();
		FLASH_Unlock(FLASH_MEMTYPE_DATA);
		for (i=0;i<size;i++){
			ee_write(address+i,buff[i]);				
		}	
			FLASH_Lock(FLASH_MEMTYPE_DATA);
		enableInterrupts();
	}

void mcs_speed(uint32_t v)
{
  /* v the speed in [1:255] */

  disableInterrupts();

  speed = v;

  enableInterrupts();
}

void nRF24L01::regWrite(uint8_t reg, uint8_t *buffer, uint8_t len) {
    uint32_t s = disableInterrupts();
    digitalWrite(_csn, LOW);
    _status = _spi->transfer((reg & 0x1F) | 0x20);
    for (int i = 0; i < len; i++) {
        _spi->transfer(buffer[i]);
    }
    digitalWrite(_csn, HIGH);
    restoreInterrupts(s);
}

void reboot()
{
    ioctl(STDOUT_FILENO,IOCTL_SYNC,0);
    
    #ifdef WITH_FILESYSTEM
    FilesystemManager::instance().umountAll();
    #endif //WITH_FILESYSTEM

    disableInterrupts();
    miosix_private::IRQsystemReboot();
}