/*..........................................................................*/
void QK_onIdle(void) {

    /* toggle the User LED on and then off, see NOTE01 */
    QF_INT_DISABLE();
    GPIOF->DATA_Bits[USER_LED] = USER_LED; /* turn the User LED on  */
    GPIOF->DATA_Bits[USER_LED] = 0;        /* turn the User LED off */
    QF_INT_ENABLE();

#ifdef Q_SPY
    if ((UART0->FR & UART_FR_TXFE) != 0) { /* TX done? */
        uint16_t fifo = UART_TXFIFO_DEPTH; /* max bytes we can accept */
        uint8_t const *block;

        QF_INT_DISABLE();
        block = QS_getBlock(&fifo); /* try to get next block to transmit */
        QF_INT_ENABLE();

        while (fifo-- != 0) {  /* any bytes in the block? */
            UART0->DR = *block++; /* put into the FIFO */
        }
    }
#elif defined NDEBUG
    /* Put the CPU and peripherals to the low-power mode.
    * you might need to customize the clock management for your application,
    * see the datasheet for your particular MCU.
    */
    __WFI(); /* Wait-For-Interrupt */
#endif
}

/*..........................................................................*/
__ramfunc
void QK_onIdle(void) {

    /* toggle first LED on and off, see NOTE01 */
    QF_INT_DISABLE();
    LED_ON(3);  /* turn LED on  */
    LED_OFF(3); /* turn LED off */
    QF_INT_ENABLE();

#ifdef Q_SPY
    /* use the idle cycles for QS transmission... */

    if ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) != 0) { /* not busy? */
        uint16_t nBytes = 0xFFFFU; /* get all available bytes */
        uint8_t const *block;

        QF_INT_DISABLE();
        if ((block = QS_getBlock(&nBytes)) != (uint8_t *)0) { /* new block? */
            AT91C_BASE_DBGU->DBGU_TPR = (uint32_t)block;
            AT91C_BASE_DBGU->DBGU_TCR = (uint32_t)nBytes;
            nBytes = 0xFFFFU; /* get all available bytes */
            if ((block = QS_getBlock(&nBytes)) != (uint8_t *)0) {/*another? */
                AT91C_BASE_DBGU->DBGU_TNPR = (uint32_t)block;
                AT91C_BASE_DBGU->DBGU_TNCR = (uint32_t)nBytes;
            }
        }
        QF_INT_ENABLE();
    }
#elif defined NDEBUG /* only if not debugging (idle mode hinders debugging) */
    AT91C_BASE_PMC->PMC_SCDR = 1;/* Power-Management: disable the CPU clock */
    /* NOTE: an interrupt starts the CPU clock again */
#endif
}

/*..........................................................................*/
void QK_onIdle(void) {

    QF_INT_DISABLE();
    SLED_ON();                          /* switch the System LED on and off */
    asm(" nop");
    asm(" nop");
    asm(" nop");
    asm(" nop");
    SLED_OFF();
    QF_INT_ENABLE();

#ifdef Q_SPY

    if (CSL_FEXT(l_uartObj.uartRegs->LSR, UART_LSR_THRE)) {
        uint16_t b;

        QF_INT_DISABLE();
        b = QS_getByte();
        QF_INT_ENABLE();

        if (b != QS_EOD) {                              /* not End-Of-Data? */
            CSL_FSET(l_uartObj.uartRegs->THR, 7U, 0U, b);
        }
    }

#elif defined NDEBUG
    /* Put the CPU and peripherals to the low-power mode.
    * you might need to customize the clock management for your application,
    * see the datasheet for your particular TMS320C5500 device.
    */
    asm(" IDLE");
#endif
}

/*..........................................................................*/
void QK_onIdle(void) {
                         /* toggle the LED on and then off, see NOTE01 */
    QF_INT_DISABLE();
    omxEval_led_on(LED_1);
    omxEval_led_off(LED_1);
    QF_INT_ENABLE();

#ifdef Q_SPY

    if ((USART3->SR & USART_FLAG_TXE) != 0) {              /* is TXE empty? */
        uint16_t b;

        QF_INT_DISABLE();
        b = QS_getByte();
        QF_INT_ENABLE();

        if (b != QS_EOD) {                              /* not End-Of-Data? */
           USART3->DR = (b & 0xFF);             /* put into the DR register */
        }
    }

#elif defined NDEBUG
    __WFI();                                          /* wait for interrupt */
#endif
}

/*..........................................................................*/
void QS_onFlush(void) {
    uint16_t b;
    QF_INT_DISABLE();
    while ((b = QS_getByte()) != QS_EOD) { /* next QS byte available? */
        QF_INT_ENABLE();
        while ((IFG2 & UCA0TXIFG) == 0U) { /* TX not ready? */
        }
        UCA0TXBUF = (uint8_t)b; /* stick the byte to the TX BUF */
        QF_INT_DISABLE();
    }
    QF_INT_ENABLE();
}

/*..........................................................................*/
static void initialize(void) {
    uint8_t p;
    QActive *a;
                         /* set priorities all registered active objects... */
    for (p = (uint8_t)1; p <= (uint8_t)QF_MAX_ACTIVE; ++p) {
        a = QF_ROM_ACTIVE_GET_(p);
        Q_ASSERT(a != (QActive *)0);    /* QF_active[p] must be initialized */
        a->prio = p;               /* set the priority of the active object */
    }
         /* trigger initial transitions in all registered active objects... */
    for (p = (uint8_t)1; p <= (uint8_t)QF_MAX_ACTIVE; ++p) {
        a = QF_ROM_ACTIVE_GET_(p);
#ifndef QF_FSM_ACTIVE
        QHsm_init(&a->super);                              /* initial tran. */
#else
        QFsm_init(&a->super);                              /* initial tran. */
#endif
    }

    QF_INT_DISABLE();
    QK_currPrio_ = (uint8_t)0;     /* set the priority for the QK idle loop */
    p = QK_schedPrio_();
    if (p != (uint8_t)0) {
        QK_sched_(p);                 /* process all events produced so far */
    }
    QF_INT_ENABLE();
}

/*..........................................................................*/
void Q_onAssert(char const * const file, int line) {
    (void)file;                                   /* avoid compiler warning */
    (void)line;                                   /* avoid compiler warning */
    QF_INT_DISABLE();         /* make sure that all interrupts are disabled */
    for (;;) {       /* NOTE: replace the loop with reset for final version */
    }
}

/*..........................................................................*/
void QF_onIdle(void) {       /* called with interrupts disabled, see NOTE01 */

    //QF_INT_DISABLE();
    //GPIOSetValue(LED_PORT, LED_BIT, LED_ON);                     /* LED on  */
    //GPIOSetValue(LED_PORT, LED_BIT, LED_OFF);                    /* LED off */
    //QF_INT_ENABLE();

#ifdef Q_SPY

    QF_INT_ENABLE();
    if ((LPC_UART->LSR & LSR_THRE) != 0) {                 /* is THR empty? */
        uint16_t b;

        QF_INT_DISABLE();
        b = QS_getByte();
        QF_INT_ENABLE();

        if (b != QS_EOD) {                              /* not End-Of-Data? */
           LPC_UART->THR = (b & 0xFF);         /* put into the THR register */
        }
    }

#elif defined NDEBUG
    /* Put the CPU and peripherals to the low-power mode.
    * you might need to customize the clock management for your application,
    * see the datasheet for your particular Cortex-M3 MCU.
    */
    __WFI();                                          /* Wait-For-Interrupt */
    QF_INT_ENABLE();
#else
    QF_INT_ENABLE();
#endif
}

/*..........................................................................*/
void QF_onIdle(void) {          /* entered with interrupts disabled, NOTE01 */

    LED_ON (IDLE_LED);                    /* blink the IDLE LED, see NOTE02 */
    LED_OFF(IDLE_LED);

#ifdef Q_SPY
    QF_INT_ENABLE();                       /* enable interrupts, see NOTE01 */

    while (U2STAbits.UTXBF == 0U) {                  /* TX Buffer not full? */
        uint16_t b;

        QF_INT_DISABLE();
        b = QS_getByte();
        QF_INT_ENABLE();

        if (b == QS_EOD) {                          /* End-Of-Data reached? */
            break;                                 /* break out of the loop */
        }
        U2TXREG = (uint8_t)b;   /* stick the byte to TXREG for transmission */
    }
#elif defined NDEBUG
    __asm__ volatile("disi #0x0001");
    Idle();                          /* transition to Idle mode, see NOTE03 */
#else
    QF_INT_ENABLE();                       /* enable interrupts, see NOTE01 */
#endif
}

/*..........................................................................*/
void QV_onIdle(void) {   /* called with interrupts DISABLED, see NOTE1 */
    /* toggle the User LED, see NOTE2 , not enough LEDs to implement! */
    //PORTB |= LED_L;
    //PORTB &= ~LED_L;

#ifdef Q_SPY
    QF_INT_ENABLE();
    if ((UCSR0A & (1U << UDRE0)) != 0U) {
        uint16_t b;

        QF_INT_DISABLE();
        b = QS_getByte();
        QF_INT_ENABLE();

        if (b != QS_EOD) {
            UDR0 = (uint8_t)b; /* stick the byte to the TX UDR0 */
        }
    }
#elif defined NDEBUG
    /* Put the CPU and peripherals to the low-power mode.
    * you might need to customize the clock management for your application,
    * see the datasheet for your particular AVR MCU.
    */
    SMCR = (0 << SM0) | (1 << SE); /* idle mode, adjust to your project */
    QV_CPU_SLEEP();  /* atomically go to sleep and enable interrupts */
#else
    QF_INT_ENABLE(); /* just enable interrupts */
#endif
}

/*..........................................................................*/
void QV_onIdle(void) { /* NOTE: called with interrutps DISABLED, see NOTE1 */
    /* toggle LED2 on and then off, see NOTE2 */
    P1OUT |=  LED2;  /* turn LED2 on */
    P1OUT &= ~LED2;  /* turn LED2 off */

#ifdef Q_SPY
    QF_INT_ENABLE();
    if (((IFG2 & UCA0TXIFG)) != 0U) { /* UART not transmitting? */
        uint16_t b;

        QF_INT_DISABLE();
        b = QS_getByte();
        QF_INT_ENABLE();

        if (b != QS_EOD) {
            UCA0TXBUF = (uint8_t)b; /* stick the byte to the TX BUF */
        }
    }
#elif defined NDEBUG
    /* Put the CPU and peripherals to the low-power mode.
    * you might need to customize the clock management for your application,
    * see the datasheet for your particular MSP430 MCU.
    */
    __low_power_mode_1(); /* Enter LPM1; also ENABLES interrupts */
#else
    QF_INT_ENABLE(); /* just enable interrupts */
#endif
}

/* NOTE: the QK scheduler is entered and exited with interrupts DISABLED    */
void QK_sched_(uint8_t p) {
    uint8_t pin = QK_currPrio_;                /* save the initial priority */
    QActive *a;
#ifdef QK_TLS                                 /* thread-local storage used? */
    uint8_t pprev = pin;
#endif
    do {
        QEvt const *e;
        a = QF_active_[p];                  /* obtain the pointer to the AO */

        QK_currPrio_ = p;         /* this becomes the current task priority */

#ifdef QK_TLS                                 /* thread-local storage used? */
        if (p != pprev) {                       /* are we changing threads? */
            QK_TLS(a);                   /* switch new thread-local storage */
            pprev = p;
        }
#endif
        QS_BEGIN_NOCRIT_(QS_QK_SCHEDULE, QS_priv_.aoObjFilter, a)
            QS_TIME_();                                        /* timestamp */
            QS_U8_(p);                            /* the priority of the AO */
            QS_U8_(pin);                          /* the preempted priority */
        QS_END_NOCRIT_()

        QF_INT_ENABLE();               /* unconditionally enable interrupts */

        e = QActive_get_(a);              /* get the next event for this AO */
        QMSM_DISPATCH(&a->super, e);                  /* dispatch to the AO */
        QF_gc(e);                /* garbage collect the event, if necessary */

        QF_INT_DISABLE();                             /* disable interrupts */

#if (QF_MAX_ACTIVE <= 8)                /* new highest-prio AO ready to run */
        QPSet8_findMax(&QK_readySet_, p);
#else
        QPSet64_findMax(&QK_readySet_, p);
#endif

        if (p <= pin) {          /* below the current preemption threshold? */
            p = (uint8_t)0;
        }
#ifndef QK_NO_MUTEX
        else if (p <= QK_ceilingPrio_) {        /* below the mutex ceiling? */
            p = (uint8_t)0;
        }
        else {
            /* empty */
        }
#endif
    } while (p != (uint8_t)0);

    QK_currPrio_ = pin;                     /* restore the initial priority */

#ifdef QK_TLS                                 /* thread-local storage used? */
    if (pin != (uint8_t)0) {       /* no extended context for the idle loop */
        a = QF_active_[pin];             /* the pointer to the preempted AO */
        QK_TLS(a);                              /* restore the original TLS */
    }
#endif
}

void QK_onIdle(void) {
#ifdef Q_SPY                     /* use the idle cycles for QS transmission */

    if ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) != 0) {  /* not busy? */
        uint16_t nBytes = 0xFFFF;
        uint8_t const *block;

        QF_INT_DISABLE();
        if ((block = QS_getBlock(&nBytes)) != (uint8_t *)0) { /* new block? */
            AT91C_BASE_DBGU->DBGU_TPR = (uint32_t)block;
            AT91C_BASE_DBGU->DBGU_TCR = (uint32_t)nBytes;
            nBytes = 0xFFFF;
            if ((block = QS_getBlock(&nBytes)) != (uint8_t *)0) {/*another? */
                AT91C_BASE_DBGU->DBGU_TNPR = (uint32_t)block;
                AT91C_BASE_DBGU->DBGU_TNCR = (uint32_t)nBytes;
            }
        }
        QF_INT_ENABLE();
    }

#elif defined NDEBUG /* only if not debugging (idle mode hinders debugging) */
    AT91C_BASE_PMC->PMC_SCDR = 1;/* Power-Management: disable the CPU clock */
    /* NOTE: an interrupt starts the CPU clock again */
#endif
}

/*..........................................................................*/
void Q_onAssert(char const Q_ROM * const file, int line) {
    /* implement the error-handling policy for your application!!! */
    QF_INT_DISABLE(); /* disable all interrupts */

    /* cause the reset of the CPU... */
    WDTCTL = WDTPW | WDTHOLD;
    __asm("    push &0xFFFE");
    /* return from function does the reset */
}

//****************************************************************************
static void thread_idle(void * const /* par */) { // idle thread handler
    QF_INT_DISABLE();
    QF::onStartup(); // application-specific startup callback
    QF_INT_ENABLE();

    for (;;) {
        QXK::onIdle();
    }
}

static void uart_tx1() {
	static uint16_t b;
    QF_INT_DISABLE(); {
    	b = QS_getByte();
    } QF_INT_ENABLE();
    if (b != QS_EOD) {  /* Have a byte to TX? */
    	nrf_drv_uart_tx((const uint8_t*)&b //This cast works only on LE
    			, 1);
    }
}

/**
 * 获取系统当前TICK数
 *
 * @return
 */
u32 GetCurTicks()
{
	u32 ticks = 0;

	QF_INT_DISABLE();
	ticks = s_Ticks;
	QF_INT_ENABLE();

	return ticks;
}

//............................................................................
void vApplicationIdleHook(void) {
    // toggle the User LED on and then off, see NOTE2
    QF_INT_DISABLE();
    BSP_LED_On(LED3);
    BSP_LED_On(LED3);
    QF_INT_ENABLE();

    // Some flating point code is to exercise the VFP...
    double x = 1.73205;
    x = x * 1.73205;

#ifdef Q_SPY
    QP::QS::rxParse();  // parse all the received bytes

    if ((DPP::l_uartHandle.Instance->ISR & UART_FLAG_TXE) != 0U) {//TXE empty?
        uint16_t b;

        QF_INT_DISABLE();
        b = QP::QS::getByte();
        QF_INT_ENABLE();

        if (b != QP::QS_EOD) {  // not End-Of-Data?
            DPP::l_uartHandle.Instance->TDR = (b & 0xFFU); // put into TDR
        }
    }
#elif defined NDEBUG
    // Put the CPU and peripherals to the low-power mode.
    // you might need to customize the clock management for your application,
    // see the datasheet for your particular Cortex-M MCU.
    //
    // !!!CAUTION!!!
    // The WFI instruction stops the CPU clock, which unfortunately disables
    // the JTAG port, so the ST-Link debugger can no longer connect to the
    // board. For that reason, the call to __WFI() has to be used with CAUTION.
    //
    // NOTE: If you find your board "frozen" like this, strap BOOT0 to VDD and
    // reset the board, then connect with ST-Link Utilities and erase the part.
    // The trick with BOOT(0) is it gets the part to run the System Loader
    // instead of your broken code. When done disconnect BOOT0, and start over.
    //__WFI(); // Wait-For-Interrupt
#endif
}

/*..........................................................................*/
void QS_onFlush(void) {
    uint16_t b;
    QF_INT_DISABLE();
    while ((b = QS_getByte()) != QS_EOD) { /* next QS trace byte available? */
        QF_INT_ENABLE();
        while ((UCSR0A & (1U << UDRE0)) == 0U) {  /* TX UDR not empty? */
        }
        UDR0 = (uint8_t)b; /* stick the byte to the TX UDR */
    }
    QF_INT_ENABLE();
}

/* ISRs and ISR "wrappers" ================================================*/
__ramfunc
void BSP_irq(void) {
    IntVector vect = (IntVector)AT91C_BASE_AIC->AIC_IVR; /* read the IVR */
    AT91C_BASE_AIC->AIC_IVR = (AT91_REG)vect; /* write AIC_IVR if protected */

    QF_INT_ENABLE();    /* allow nesting interrupts */
    (*vect)();          /* call the IRQ ISR via the pointer to function */
    QF_INT_DISABLE();   /* disable interrups for the exit sequence */

    AT91C_BASE_AIC->AIC_EOICR = 0; /* write AIC_EOICR to clear interrupt */
}