/**
 * @brief   Common IRQ handler.
 *
 * @param[in] sdp       communication channel associated to the USART
 */
static void serve_interrupt(SerialDriver *sdp) {
  USART_TypeDef *u = sdp->usart;
  uint16_t cr1 = u->CR1;
  uint16_t sr = u->SR;  /* SR reset step 1.*/
  uint16_t dr = u->DR;  /* SR reset step 2.*/

  /* Error condition detection.*/
  if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE  | USART_SR_PE))
    set_error(sdp, sr);
  /* Special case, LIN break detection.*/
  if (sr & USART_SR_LBD) {
    chSysLockFromIsr();
    chnAddFlagsI(sdp, SD_BREAK_DETECTED);
    chSysUnlockFromIsr();
    u->SR &= ~USART_SR_LBD;
  }
  /* Data available.*/
  if (sr & USART_SR_RXNE) {
    chSysLockFromIsr();
    sdIncomingDataI(sdp, (uint8_t)dr);
    chSysUnlockFromIsr();
  }
  /* Transmission buffer empty.*/
  if ((cr1 & USART_CR1_TXEIE) && (sr & USART_SR_TXE)) {
    msg_t b;
    chSysLockFromIsr();
    b = chOQGetI(&sdp->oqueue);
    if (b < Q_OK) {
      chnAddFlagsI(sdp, CHN_OUTPUT_EMPTY);
      u->CR1 = (cr1 & ~USART_CR1_TXEIE) | USART_CR1_TCIE;
    }
    else
      u->DR = b;
    chSysUnlockFromIsr();
  }
  /* Physical transmission end.*/
  if (sr & USART_SR_TC) {
    chSysLockFromIsr();
    chnAddFlagsI(sdp, CHN_TRANSMISSION_END);
    chSysUnlockFromIsr();
    u->CR1 = cr1 & ~USART_CR1_TCIE;
    u->SR &= ~USART_SR_TC;
  }
}

/**
 * @brief   Default EP0 IN callback.
 * @details This function is used by the low level driver as default handler
 *          for EP0 IN events.
 *
 * @param[in] usbp      pointer to the @p USBDriver object
 * @param[in] ep        endpoint number, always zero
 *
 * @notapi
 */
void _usb_ep0in(USBDriver *usbp, usbep_t ep) {
  size_t max;

  (void)ep;
  switch (usbp->ep0state) {
  case USB_EP0_TX:
    max = usbFetchWord(&usbp->setup[6]);
    /* If the transmitted size is less than the requested size and it is a
       multiple of the maximum packet size then a zero size packet must be
       transmitted.*/
    if ((usbp->ep0n < max) && ((usbp->ep0n % usbp->epc[0]->in_maxsize) == 0)) {
      usbPrepareTransmit(usbp, 0, NULL, 0);
      chSysLockFromIsr();
      usbStartTransmitI(usbp, 0);
      chSysUnlockFromIsr();
      usbp->ep0state = USB_EP0_WAITING_TX0;
      return;
    }
    /* Falls into, it is intentional.*/
  case USB_EP0_WAITING_TX0:
    /* Transmit phase over, receiving the zero sized status packet.*/
    usbp->ep0state = USB_EP0_WAITING_STS;
    usbPrepareReceive(usbp, 0, NULL, 0);
    chSysLockFromIsr();
    usbStartReceiveI(usbp, 0);
    chSysUnlockFromIsr();
    return;
  case USB_EP0_SENDING_STS:
    /* Status packet sent, invoking the callback if defined.*/
    if (usbp->ep0endcb != NULL)
      usbp->ep0endcb(usbp);
    usbp->ep0state = USB_EP0_WAITING_SETUP;
    return;
  default:
    ;
  }
  /* Error response, the state machine goes into an error state, the low
     level layer will have to reset it to USB_EP0_WAITING_SETUP after
     receiving a SETUP packet.*/
  usb_lld_stall_in(usbp, 0);
  usb_lld_stall_out(usbp, 0);
  _usb_isr_invoke_event_cb(usbp, USB_EVENT_STALLED);
  usbp->ep0state = USB_EP0_ERROR;
}

static void cmd_color(BaseSequentialStream *chp, int argc, char *argv[]) {
    #define COLOR_USABLE_WITH_LOOP TRUE
    #if !COLOR_USABLE_WITH_LOOP 
    uint32_t loop_times=1;
    uint32_t loop_delay_ms=100;
    if(argc > 0){
        if(strcmp("-l", argv[0])==0){
            if(argc > 1){
                if(str_is_valid_num(argv[1])){ 
                    loop_times = atol(argv[1]);
                    if(argc > 2){
                        if(strcmp("-s", argv[2])==0){
                            if(argc > 3){
                                if(str_is_valid_num(argv[3])){
                                    loop_delay_ms = atol(argv[3]);
                                } else {
                                    chprintf(chp,invalid_num_str,argv[3]);
                                }
                            } else {
                                chprintf(chp,"please enter millisecond delay\n\r");
                            }
                        } else {
                            chprintf(chp,invalid_arg_str,argv[2]);
                        }
                    }  
                } else {
                    chprintf(chp,invalid_num_str, argv[1]);
                }
            } else {
                chprintf(chp, "please enter a number of times to loop\n\r");
            }
        } else if(strcmp("-h",argv[0])==0){
            chprintf(chp, "printing help:\n\r");
            chprintf(chp, color_usage);
        } else {
            chprintf(chp, invalid_arg_str, argv[0]);
            chprintf(chp, color_usage);
        }
    }
    chprintf(chp, "looping %U times with delay of %U milliseconds:\n\r", 
        loop_times,loop_delay_ms);
    while(loop_times--){
    #endif
        #if !USE_I2C_POLL_THD 
        request_color_data();
        #endif
        chSysLockFromIsr();
        get_lx_from_cnts();
        chSysUnlockFromIsr();
        chprintf(chp, "color_lx: y=%U\tr=%U\tg=%U\tb=%U\n\r",
            y_lx_val,r_lx_val,g_lx_val,b_lx_val);
    #if !COLOR_USABLE_WITH_LOOP 
        chThdSleepMilliseconds(loop_delay_ms);
    }
    #endif
}

/**
 * @brief   Wake up ISR handler.
 *
 * @param[in] canp      pointer to the @p CANDriver object
 *
 * @notapi
 */
static void can_lld_wakeup_handler(CANDriver *canp) {

  /* Wakeup event.*/
  canp->state = CAN_READY;
  can_lld_wakeup(canp);
  chSysLockFromIsr();
  chEvtBroadcastI(&canp->wakeup_event);
  chSysUnlockFromIsr();

}

/*
 * GPT1 callback.
 */
static void gpt1cb(GPTDriver *gptp) {
  msg_t msg;

  (void)gptp;
  chSysLockFromIsr();
  msg = chMBPostI(&mb[0], MSG_SEND_RIGHT);
  if (msg != RDY_OK)
    saturated = TRUE;
  chSysUnlockFromIsr();
}

static void pwm3cb(PWMDriver *pwmp) {

  chSysLockFromIsr();
  if (pwm_cycles > 8) {
	  pwmDisableChannelI(pwmp, 2);
	  pwmDisableChannelI(pwmp, 3);
	  tmp1 = (&GPT_DRIVER)->tim->CNT;
  }
  chSysUnlockFromIsr();
}

// =============================== I2C =========================================
void i2cDmaIrqHandler(void *p, uint32_t flags) {
    chSysLockFromIsr();
    //Uart.Printf("===T===");
    Thread *PThd = ((i2c_t*)p)->PRequestingThread;
    if (PThd != NULL) {
        ((i2c_t*)p)->PRequestingThread = NULL;
        chSchReadyI(PThd);
    }
    chSysUnlockFromIsr();
}

/*! Triggered when the pin changes state (both edges)
 *
 * Challenge: Add de-bouncing
 */
void extdetail_launch_detect(EXTDriver *extp, expchannel_t channel) {

    (void)extp;
    (void)channel;

    chSysLockFromIsr();
    chEvtBroadcastI(&extdetail_launch_detect_event);

    chSysUnlockFromIsr();
}

void TIM2_IRQHandler(void) {
    CH_IRQ_PROLOGUE();
    chSysLockFromIsr();
    if(TIM2->SR & TIM_SR_UIF) {
        TIM2->SR &= ~TIM_SR_UIF;
        Adc.IIrqSmpHandler();
    }
    chSysUnlockFromIsr();
    CH_IRQ_EPILOGUE();
}

 void AdcTxIrq(void *p, uint32_t flags) {
     dmaStreamDisable(ADC_DMA);
     ADC1->CR2 = 0;  // Disable ADC
     // Signal event to thread
     if(IPThread != nullptr) {
         chSysLockFromIsr();
         chEvtSignalI(IPThread, IEvt);
         chSysUnlockFromIsr();
     }
 }

/**
 * @brief Process system timer interrupts, if present.
 *
 * @notapi
 */
static void systimer_serve_interrupt( void )
{
  // Update the system time
  chSysLockFromIsr();
  chSysTimerHandlerI();
  chSysUnlockFromIsr();

  // Clear timer interrupt
  ARM_TIMER_CLI = 0;
}

/*
 * GPT2 callback.
 */
static void gpt2cb(GPTDriver *gptp) {
  msg_t msg;

  (void)gptp;
  chSysLockFromIsr();
  msg = chMBPostI(&mb[NUM_THREADS - 1], MSG_SEND_LEFT);
  if (msg != RDY_OK)
    saturated = TRUE;
  chSysUnlockFromIsr();
}

void AdcTxIrq(void *p, uint32_t flags) {
    dmaStreamDisable(ADC_DMA);
    // Resume thread if any
    chSysLockFromIsr();
    if(PAdcThread != NULL) {
        if(PAdcThread->p_state == THD_STATE_SUSPENDED) chSchReadyI(PAdcThread);
        PAdcThread = NULL;
    }
    chSysUnlockFromIsr();
}

/*
 * This callback is invoked when a transmission has physically completed.
 */
static void txend2(UARTDriver *uartp) {

  (void)uartp;
  palSetPad(IOPORT3, GPIOC_LED);
  chSysLockFromIsr();
  if (chVTIsArmedI(&vt1))
    chVTResetI(&vt1);
  chVTSetI(&vt1, MS2ST(5000), restart, NULL);
  chSysUnlockFromIsr();
}

/* Triggered when the button is pressed or released. The LED is set to ON.*/
static void extcb1(EXTDriver *extp, expchannel_t channel) {

  (void)extp;
  (void)channel;
  palClearPad(GPIOC, GPIOC_LED);
  chSysLockFromIsr();
  if (!chVTIsArmedI(&vt))
    chVTSetI(&vt, MS2ST(200), ledoff, NULL);
  chSysUnlockFromIsr();
}

/*
 * This callback is invoked when a transmission has physically completed.
 */
static void txend2(UARTDriver *uartp) {

  (void)uartp;
  palClearPad(GPIOE, GPIOE_LED3_RED);
  chSysLockFromIsr();
  if (chVTIsArmedI(&vt1))
    chVTResetI(&vt1);
  chVTSetI(&vt1, MS2ST(5000), restart, NULL);
  chSysUnlockFromIsr();
}

/*
 * Handles the USB driver global events.
 */
static void usb_event(USBDriver *usbp, usbevent_t event) {
  uint8_t ep;

  switch (event) {
  case USB_EVENT_RESET:
    return;
  case USB_EVENT_ADDRESS:
    return;
  case USB_EVENT_CONFIGURED:
    chSysLockFromIsr();

    for (ep = 1; ep < 10; ep++)
    {
      // Make sure the endpoint are initialized from 1 to 9
      if (ep == CDC1_NOTIFICATION_EPADDR)
        usbInitEndpointI(usbp, CDC1_NOTIFICATION_EPADDR, &cdc1_ep_int_config);
      if (ep == CDC1_TX_EPADDR)
        usbInitEndpointI(usbp, CDC1_TX_EPADDR, &cdc1_ep_in_config);
      if (ep == CDC1_RX_EPADDR)
        usbInitEndpointI(usbp, CDC1_RX_EPADDR, &cdc1_ep_out_config);

      if (ep == CDC2_NOTIFICATION_EPADDR)
        usbInitEndpointI(usbp, CDC2_NOTIFICATION_EPADDR, &cdc2_ep_int_config);
      if (ep == CDC2_TX_EPADDR)
        usbInitEndpointI(usbp, CDC2_TX_EPADDR, &cdc2_ep_in_config);
      if (ep == CDC2_RX_EPADDR)
        usbInitEndpointI(usbp, CDC2_RX_EPADDR, &cdc2_ep_out_config);

#if HAL_USE_MSD
      if (ep == MASS_STORAGE_IN_EPADDR)
        usbInitEndpointI(usbp, MASS_STORAGE_IN_EPADDR, &ms_ep_in_config);
      if (ep == MASS_STORAGE_OUT_EPADDR)
        usbInitEndpointI(usbp, MASS_STORAGE_OUT_EPADDR, &ms_ep_out_config);
#endif
    }

    /* Resetting the state of the CDC subsystem.*/
    sduConfigureHookI(usbp);
#if HAL_USE_MSD
    msdConfigureHookI(usbp);
#endif
    chSysUnlockFromIsr();
    return;
  case USB_EVENT_SUSPEND:
#if HAL_USE_MSD
    msdSuspendHookI(usbp);
#endif
    return;
  case USB_EVENT_WAKEUP:
    return;
  case USB_EVENT_STALLED:
    return;
  }
  return;
}

/*
 * Handles the USB driver global events.
 * These events are triggered by the USB driver.
 */
static void usb_event(USBDriver *usbp, usbevent_t event) {
  (void) usbp;
  switch (event) {
  case USB_EVENT_RESET:
    /* Signals when a USB reset event has occurred.
     * Typically this will happen at startup and when the cable
     * is connected or disconnected.
     * After a reset, the usb system will not be operational
     * until it is configured.
     */
    chSysLockFromIsr();
    chEvtBroadcastFlagsI(&esUsbReset, (flagsmask_t)0);
    chSysUnlockFromIsr();
    return;
  case USB_EVENT_ADDRESS:
    return;
  case USB_EVENT_CONFIGURED:

    /* Enables the endpoints specified into the configuration.
     * Note, this callback is invoked from an ISR so I-Class functions
     * must be used.
     */
    chSysLockFromIsr();
    usbInitEndpointI(usbp, 1, &ep1config);
    usbInitEndpointI(usbp, 2, &ep2config);

    /* Signals that the configuration is complete, and the USB system
     * is ready to be used.
     */
    chEvtBroadcastFlagsI(&esUsbConfigured, (flagsmask_t)0);
    chSysUnlockFromIsr();

    return;
  case USB_EVENT_SUSPEND:
    return;
  case USB_EVENT_WAKEUP:
    return;
  case USB_EVENT_STALLED:
    return;
  }
  return;
}

/**
 * @brief   Common IRQ handler.
 * @note    Tries hard to clear all the pending interrupt sources, we don't
 *          want to go through the whole ISR and have another interrupt soon
 *          after.
 *
 * @param[in] u         pointer to an UART I/O block
 * @param[in] sdp       communication channel associated to the UART
 */
static void serve_interrupt(SerialDriver *sdp) {
  LPC_USART_TypeDef *u = sdp->uart;

  while (u->INTSTAT) {

    if (u->INTSTAT & STAT_RXRDY) {
      chSysLockFromIsr();
      if (chIQIsEmptyI(&sdp->iqueue))
        chnAddFlagsI(sdp, CHN_INPUT_AVAILABLE);
      if (chIQPutI(&sdp->iqueue, u->RXDATA) < Q_OK)
        chnAddFlagsI(sdp, SD_OVERRUN_ERROR);
      chSysUnlockFromIsr();
    }

    if (u->INTSTAT & STAT_TXRDY) {
      msg_t b;

      chSysLockFromIsr();
      b = chOQGetI(&sdp->oqueue);
      chSysUnlockFromIsr();

      if (b < Q_OK) {
        u->INTENCLR = STAT_TXRDY;
        chSysLockFromIsr();
        chnAddFlagsI(sdp, CHN_OUTPUT_EMPTY);
        chSysUnlockFromIsr();
        break;
      }
      else {
         u->TXDATA = b;
      }
    }

    if (u->INTSTAT & (STAT_OVERRUN | STAT_DELTARXBRK |
                      STAT_FRAMERR | STAT_PARITYERR) ) {
      IOREG32 stat = u->STAT;
      set_error(sdp, stat);
      u->STAT = stat;
    }

  }
}

/*
 * SPI end transfer callback.
 */
static void spicb(SPIDriver *spip) {
 
    if(spip->state == SPI_COMPLETE){
    /* On transfer end just releases the slave select line.*/
    chSysLockFromIsr();
    SPI_done = TRUE;
    spiUnselectI(spip);
//    palSetPad(GPIOC,4);
    chSysUnlockFromIsr();
    }
}