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

  (void)ep;
  switch (usbp->ep0state) {
  case USB_EP0_RX:
    /* Receive phase over, sending the zero sized status packet.*/
    usbp->ep0state = USB_EP0_SENDING_STS;
    usbPrepareTransmit(usbp, 0, NULL, 0);
    chSysLockFromIsr();
    usbStartTransmitI(usbp, 0);
    chSysUnlockFromIsr();
    return;
  case USB_EP0_WAITING_STS:
    /* Status packet received, it must be zero sized, invoking the callback
       if defined.*/
    if (usbGetReceiveTransactionSizeI(usbp, 0) != 0)
      break;
    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 lsm9ds0_drdy_cb(EXTDriver *extp, expchannel_t channel)
{
  uint8_t rdy = 0;

  (void)extp;

  switch(channel) {
  case SC_LSM9DS0_INT1_XM_PIN:
	  rdy = SENSOR_RDY_ACC;
    break;
  case SC_LSM9DS0_INT2_XM_PIN:
	  rdy = SENSOR_RDY_MAGN;
    break;
  case SC_LSM9DS0_DRDY_G_PIN:
	  rdy = SENSOR_RDY_GYRO;
    break;
  default:
    chDbgAssert(0, "Invalid channel", "#1");
    return;
  }

  chSysLockFromIsr();
  // No need to lock mutex inside chSysLockFromIsr()
  sensors_ready |= rdy;

  chBSemSignalI(&lsm9ds0_drdy_sem);

  chSysUnlockFromIsr();
}

void Endpoint_t::PrepareTransmit(uint8_t *Ptr, uint32_t Len) {
    if(IsTransmitting) return;  // do not start if transmitting already
    //Uart.Printf("TX: %A\r\n", Ptr, Len, ' ');
    Uart.Printf("PrepTX L=%u\r\n", Len);
    Tx.Ptr = Ptr;
    Tx.Sz = Len;
    Tx.Cnt = 0;
    uint32_t Dieptsiz = OTG_FS->ie[SelfN].DIEPTSIZ;
    Dieptsiz &= ~((3<<19) | 0x7F);
    uint32_t DieptCtl = OTG_FS->ie[SelfN].DIEPCTL;

    // Prepare transmission
    if(Len == 0) {  // Send zero pkt
        //OTG_FS->ie[SelfN].DIEPTSIZ = DIEPTSIZ_PKTCNT(1) | DIEPTSIZ_XFRSIZ(0);
        Dieptsiz |= DIEPTSIZ_PKTCNT(1) | DIEPTSIZ_XFRSIZ(0);
    }
    else {
        uint32_t Cnt = (Len + InMaxSz - 1) / InMaxSz;
        //OTG_FS->ie[SelfN].DIEPTSIZ = DIEPTSIZ_PKTCNT(Cnt) | DIEPTSIZ_XFRSIZ(Len);
        //Uart.Printf("Cnt: %u\r\n", Cnt);
        Dieptsiz |= DIEPTSIZ_PKTCNT(Cnt) | DIEPTSIZ_XFRSIZ(Len);
    }
    //Uart.Printf("Dieptsiz: %X\r\n", Dieptsiz);
    DieptCtl |= DIEPCTL_EPENA | DIEPCTL_CNAK;

    // Start transfer
    chSysLockFromIsr();
    IsTransmitting = true;
    OTG_FS->ie[SelfN].DIEPTSIZ = Dieptsiz;
    OTG_FS->ie[SelfN].DIEPCTL  = DieptCtl;
    EnableInFifoEmptyIRQ();
    chSysUnlockFromIsr();
}

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

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

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

    /* Resetting the state of the CDC subsystem.*/
    sduConfigureHookI(&SDU1);

    chSysUnlockFromIsr();
    return;
  case USB_EVENT_SUSPEND:
    return;
  case USB_EVENT_WAKEUP:
    return;
  case USB_EVENT_STALLED:
    return;
  }
  return;
}

/*
 * SPI end transfer callback.
 */
static void spicb(SPIDriver *spip) {

    /* On transfer end just releases the slave select line.*/
    chSysLockFromIsr();
    spiUnselectI(spip);
    chSysUnlockFromIsr();
}

/*
 * Handles the USB driver global events.
 */
static void usb_event(USBDriver *usbp, usbevent_t event) {
  (void) usbp;
  switch (event) {
  case USB_EVENT_RESET:
    //with usbStatus==0 no new transfers will be initiated
    usbStatus = 0;
    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);
    chSysUnlockFromIsr();
    //allow the main thread to init the transfers
    initUSB =1;
    return;
  case USB_EVENT_SUSPEND:
    return;
  case USB_EVENT_WAKEUP:
    return;
  case USB_EVENT_STALLED:
    return;
  }
  return;
}

void vappendPrintf(Logging *logging, const char *fmt, va_list arg) {
	efiAssertVoid(getRemainingStack(chThdSelf()) > 16, "stack#5b");
	if (!intermediateLoggingBufferInited) {
		firmwareError("intermediateLoggingBufferInited not inited!");
		return;
	}
	int is_locked = isLocked();
	int icsr_vectactive = isIsrContext();
	if (is_locked) {
		vappendPrintfI(logging, fmt, arg);
	} else {
		if (icsr_vectactive == 0) {
			chSysLock()
			;
			vappendPrintfI(logging, fmt, arg);
			chSysUnlock()
			;
		} else {
			chSysLockFromIsr()
			;
			vappendPrintfI(logging, fmt, arg);
			chSysUnlockFromIsr()
			;
		}
	}
}

static void set_error(uint8_t sra, SerialDriver *sdp) {
  flagsmask_t sts = 0;
  uint8_t dor = 0;
  uint8_t upe = 0;
  uint8_t fe = 0;

#if USE_AVR_USART0
  if (&SD1 == sdp) {
    dor = (1 << DOR0);
    upe = (1 << UPE0);
    fe = (1 << FE0);
  }
#endif

#if USE_AVR_USART1
  if (&SD2 == sdp) {
    dor = (1 << DOR1);
    upe = (1 << UPE1);
    fe = (1 << FE1);
  }
#endif

  if (sra & dor)
    sts |= SD_OVERRUN_ERROR;
  if (sra & upe)
    sts |= SD_PARITY_ERROR;
  if (sra & fe)
    sts |= SD_FRAMING_ERROR;
  chSysLockFromIsr();
  chnAddFlagsI(sdp, sts);
  chSysUnlockFromIsr();
}

/**
 * @brief   MSC state machine initialization.
 *
 * @param[in] usbp      pointer to the @p USBDriver object
 */
static void msc_reset(USBDriver *usbp) {

  msc_state = MSC_IDLE;
  chSysLockFromIsr();
  usbStartReceiveI(usbp, MSC_DATA_OUT_EP, (uint8_t *)&CBW, sizeof CBW);
  chSysUnlockFromIsr();
}

static void mma8451_int2_cb(EXTDriver *extp, expchannel_t channel){
  (void)extp;
  (void)channel;
  chSysLockFromIsr();
  mma8451_sem.signalI();
  chSysUnlockFromIsr();
}

static bool_t connint(SerialDriver *sdp) {

    if (sdp->com_data == INVALID_SOCKET) {
        struct sockaddr addr;
        socklen_t addrlen = sizeof(addr);

        if ((sdp->com_data = accept(sdp->com_listen, &addr, &addrlen)) == INVALID_SOCKET)
            return FALSE;

        if (ioctl(sdp->com_data, FIONBIO, &nb) != 0) {
            printf("%s: Unable to setup non blocking mode on data socket\n", sdp->com_name);
            goto abort;
        }
        chSysLockFromIsr();
        chnAddFlagsI(sdp, CHN_CONNECTED);
        chSysUnlockFromIsr();
        return TRUE;
    }
    return FALSE;
abort:
    if (sdp->com_listen != INVALID_SOCKET)
        close(sdp->com_listen);
    if (sdp->com_data != INVALID_SOCKET)
        close(sdp->com_data);
    exit(1);
}

static void mag3110_cb(EXTDriver *extp, expchannel_t channel){
  (void)extp;
  (void)channel;
  chSysLockFromIsr();
  //lsm303_sem.signalI();
  chSysUnlockFromIsr();
}

static void bmp085_cb(EXTDriver *extp, expchannel_t channel){
  (void)extp;
  (void)channel;
  chSysLockFromIsr();
  bmp085_sem.signalI();
  chSysUnlockFromIsr();
}

static void gps_pps_cb(EXTDriver *extp, expchannel_t channel){
  (void)extp;
  (void)channel;
  chSysLockFromIsr();
  rtc_semp->signalI();
  chSysUnlockFromIsr();
}

/**
 * @brief   Reads from a dedicated packet buffer.
 *
 * @param[in] udp       pointer to a @p stm32_usb_descriptor_t
 * @param[in] iqp       pointer to an @p InputQueue object
 * @param[in] n         maximum number of bytes to copy. This value must
 *                      not exceed the maximum packet size for this endpoint.
 *
 * @notapi
 */
static void usb_packet_read_to_queue(stm32_usb_descriptor_t *udp,
                                     InputQueue *iqp, size_t n) {
  size_t nhw;
  uint32_t *pmap= USB_ADDR2PTR(udp->RXADDR0);

  nhw = n / 2;
  while (nhw > 0) {
    uint32_t w;

    w = *pmap++;
    *iqp->q_wrptr++ = (uint8_t)w;
    if (iqp->q_wrptr >= iqp->q_top)
      iqp->q_wrptr = iqp->q_buffer;
    *iqp->q_wrptr++ = (uint8_t)(w >> 8);
    if (iqp->q_wrptr >= iqp->q_top)
      iqp->q_wrptr = iqp->q_buffer;
    nhw--;
  }
  /* Last byte for odd numbers.*/
  if ((n & 1) != 0) {
    *iqp->q_wrptr++ = (uint8_t)*pmap;
    if (iqp->q_wrptr >= iqp->q_top)
      iqp->q_wrptr = iqp->q_buffer;
  }

  /* Updating queue.*/
  chSysLockFromIsr();
  iqp->q_counter += n;
  while (notempty(&iqp->q_waiting))
    chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK;
  chSysUnlockFromIsr();
}

static void msc_sendstatus(USBDriver *usbp) {

  msc_state = MSC_SENDING_CSW;
  chSysLockFromIsr();
  usbStartTransmitI(usbp, MSC_DATA_IN_EP, (uint8_t *)&CSW, sizeof CSW);
  chSysUnlockFromIsr();
}

/**
 * @brief   Insertion monitor timer callback function.
 *
 * @param[in] p         pointer to the @p BaseBlockDevice object
 *
 * @notapi
 */
static void tmrfunc(void *p) {
  BaseBlockDevice *bbdp = p;

  /* The presence check is performed only while the driver is not in a
     transfer state because it is often performed by changing the mode of
     the pin connected to the CS/D3 contact of the card, this could disturb
     the transfer.*/
  blkstate_t state = blkGetDriverState(bbdp);
  chSysLockFromIsr();
  if ((state != BLK_READING) && (state != BLK_WRITING)) {
    /* Safe to perform the check.*/
    if (cnt > 0) {
      if (blkIsInserted(bbdp)) {
        if (--cnt == 0) {
          chEvtBroadcastI(&inserted_event);
        }
      }
      else
        cnt = POLLING_INTERVAL;
    }
    else {
      if (!blkIsInserted(bbdp)) {
        cnt = POLLING_INTERVAL;
        chEvtBroadcastI(&removed_event);
      }
    }
  }
  chVTSetI(&tmr, MS2ST(POLLING_DELAY), tmrfunc, bbdp);
  chSysUnlockFromIsr();
}

/* The PWM Counter Reset will put the PWM system in "ACTIVE" state, which
 * is defined as the state when the channel is active and a compare event
 * has not yet taken place.
 */
static void cdPwmCounterReset(PWMDriver *pwmp) {
  (void) pwmp;

  chSysLockFromIsr();
  palWriteGroup (PWM_OUT_PORT, PWM_OUT_PORT_MASK, PWM_OUT_OFFSET,  bldc.pwmOutT0);

  // Calculate and initiate the state change
  // Consider moving this to a thread to further slim down the ISR callback
  if (!halIsCounterWithin(bldc.prevStateChange, bldc.nextStateChange)) {

    // Prepare next state
    if (bldc.directionFwd) {
      bldc.nextState++;
    }
    else {
      bldc.nextState--;
    }

    // Wrap the state counter
    bldc.nextState = (bldc.nextState+bldc.stateCount)%bldc.stateCount;

    // Prepare the next state change.
    bldc.prevStateChange = bldc.nextStateChange;
    bldc.nextStateChange += bldc.stateChangeInterval;
  }
  chSysUnlockFromIsr();
}

/**
 * @brief Stops the sound.
 *
 * @param[in] p pointer to the timer
 */
static void stop(void *p) {

  StopCounter((TC *)p);
  chSysLockFromIsr();
  chEvtBroadcastI(&BuzzerSilentEventSource);
  chSysUnlockFromIsr();
}

/*
 * ADC end conversion callback.
 * The PWM channels are reprogrammed using the latest ADC samples.
 * The latest samples are transmitted into a single SPI transaction.
 */
void adccb(ADCDriver *adcp, adcsample_t *buffer, size_t n) {

    (void) buffer;
    (void) n;
    /* Note, only in the ADC_COMPLETE state because the ADC driver fires an
       intermediate callback when the buffer is half full.*/
    if (adcp->state == ADC_COMPLETE) {
        adcsample_t avg_ch1, avg_ch2;

        /* Calculates the average values from the ADC samples.*/
        avg_ch1 = (samples[0] + samples[2] + samples[4] + samples[6]) / 4;
        avg_ch2 = (samples[1] + samples[3] + samples[5] + samples[7]) / 4;

        chSysLockFromIsr();

        /* Changes the channels pulse width, the change will be effective
           starting from the next cycle.*/
        pwmEnableChannelI(&PWMD3, 2, PWM_FRACTION_TO_WIDTH(&PWMD3, 4096, avg_ch1));
        pwmEnableChannelI(&PWMD3, 3, PWM_FRACTION_TO_WIDTH(&PWMD3, 4096, avg_ch2));

        /* SPI slave selection and transmission start.*/
        spiSelectI(&SPID1);
        spiStartSendI(&SPID1, ADC_GRP1_NUM_CHANNELS * ADC_GRP1_BUF_DEPTH, samples);

        chSysUnlockFromIsr();
    }
}