/**
 * Initialize the driver, must be called before any other routines.
 *
 * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
 * m25p16_getGeometry().
 */
bool m25p16_init(ioTag_t csTag)
{
    /*
        if we have already detected a flash device we can simply exit

        TODO: change the init param in favour of flash CFG when ParamGroups work is done
        then cs pin can be specified in hardware_revision.c or config.c (dependent on revision).
    */
    if (geometry.sectors) {
        return true;
    }

    if (csTag) {
        m25p16CsPin = IOGetByTag(csTag);
    } else {
#ifdef M25P16_CS_PIN
        m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN));
#else
        return false;
#endif
    }
    IOInit(m25p16CsPin, OWNER_FLASH_CS, 0);
    IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG);

    DISABLE_M25P16;

#ifndef M25P16_SPI_SHARED
    //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz
    spiSetDivisor(M25P16_SPI_INSTANCE, SPI_CLOCK_FAST);
#endif

    return m25p16_readIdentification();
}

uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t options)
{
    uartPort_t *s;
    static volatile uint8_t rx5Buffer[UART5_RX_BUFFER_SIZE];
    static volatile uint8_t tx5Buffer[UART5_TX_BUFFER_SIZE];
    NVIC_InitTypeDef NVIC_InitStructure;

    s = &uartPort5;
    s->port.vTable = uartVTable;

    s->port.baudRate = baudRate;

    s->port.rxBufferSize = UART5_RX_BUFFER_SIZE;
    s->port.txBufferSize = UART5_TX_BUFFER_SIZE;
    s->port.rxBuffer = rx5Buffer;
    s->port.txBuffer = tx5Buffer;

    s->USARTx = UART5;

    RCC_ClockCmd(RCC_APB1(UART5), ENABLE);

    serialUARTInit(IOGetByTag(IO_TAG(UART5_TX_PIN)), IOGetByTag(IO_TAG(UART5_RX_PIN)), mode, options, GPIO_AF_5, 5);

    NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART5);
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART5);
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    return s;
}

void adcHardwareInit(drv_adc_config_t *init)
{
    UNUSED(init);
    int configuredAdcChannels = 0;

    for (int i = ADC_CHN_1; i < ADC_CHN_COUNT; i++) {
        if (!adcConfig[i].tag)
            continue;

        adcDevice_t * adc = &adcHardware[adcConfig[i].adcDevice];

        IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_CH1 + (i - ADC_CHN_1), 0);
        IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));

        adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
        adcConfig[i].dmaIndex = adc->usedChannelCount++;
        adcConfig[i].sampleTime = ADC_SampleTime_601Cycles5;
        adcConfig[i].enabled = true;

        adc->enabled = true;
        configuredAdcChannels++;
    }

    if (configuredAdcChannels == 0)
        return;

    RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256);  // 72 MHz divided by 256 = 281.25 kHz

    for (int i = 0; i < ADCDEV_COUNT; i++) {
        if (adcHardware[i].enabled) {
            adcInstanceInit(i);
        }
    }
}

void serialInputPortConfig(ioTag_t pin, uint8_t portIndex)
{
    IOInit(IOGetByTag(pin), OWNER_SOFTSERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(portIndex));
#ifdef STM32F1
    IOConfigGPIO(IOGetByTag(pin), IOCFG_IPU);
#else
    IOConfigGPIO(IOGetByTag(pin), IOCFG_AF_PP_UP);
#endif
}

serialPort_t *usbVcpOpen(void)
{
    vcpPort_t *s;

    IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0);
    IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0);

#if defined(STM32F4)
    usbGenerateDisconnectPulse();

    switch (usbDevConfig()->type) {
#ifdef USE_USB_CDC_HID
    case COMPOSITE:
        USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_HID_CDC_cb, &USR_cb);
        break;
#endif
    default:
        USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
        break;
    }
#elif defined(STM32F7)
    usbGenerateDisconnectPulse();

    /* Init Device Library */
    USBD_Init(&USBD_Device, &VCP_Desc, 0);

    /* Add Supported Class */
    switch (usbDevConfig()->type) {
#ifdef USE_USB_CDC_HID
    case COMPOSITE:
    	USBD_RegisterClass(&USBD_Device, USBD_HID_CDC_CLASS);
        break;
#endif
    default:
        USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS);
        break;
    }

    /* HID Interface doesn't have any callbacks... */
    /* Add CDC Interface Class */
    USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops);

    /* Start Device Process */
    USBD_Start(&USBD_Device);
#else
    Set_System();
    Set_USBClock();
    USB_Init();
    USB_Interrupts_Config();
#endif

    s = &vcpPort;
    s->port.vTable = usbVTable;

    return (serialPort_t *)s;
}

void i2cInit(I2CDevice device)
{
    UNUSED(device);

    scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL));
    sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA));

    IOConfigGPIO(scl, IOCFG_OUT_OD);
    IOConfigGPIO(sda, IOCFG_OUT_OD);
}

static inline void mma8451ConfigureInterrupt(void)
{
#ifdef MMA8451_INT_PIN
    IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0);
    // TODO - maybe pullup / pulldown ?
    IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING); 
#endif

    i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
    i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
    i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
}

serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, portOptions_t options)
{
    softSerial_t *softSerial = &(softSerialPorts[portIndex]);

#ifdef USE_SOFTSERIAL1
    if (portIndex == SOFTSERIAL1) {
        softSerial->rxTimerHardware = &(timerHardware[SOFTSERIAL_1_TIMER_RX_HARDWARE]);
        softSerial->txTimerHardware = &(timerHardware[SOFTSERIAL_1_TIMER_TX_HARDWARE]);
    }
#endif

#ifdef USE_SOFTSERIAL2
    if (portIndex == SOFTSERIAL2) {
        softSerial->rxTimerHardware = &(timerHardware[SOFTSERIAL_2_TIMER_RX_HARDWARE]);
        softSerial->txTimerHardware = &(timerHardware[SOFTSERIAL_2_TIMER_TX_HARDWARE]);
    }
#endif

    softSerial->port.vTable = softSerialVTable;
    softSerial->port.baudRate = baud;
    softSerial->port.mode = MODE_RXTX;
    softSerial->port.options = options;
    softSerial->port.callback = callback;

    resetBuffers(softSerial);

    softSerial->isTransmittingData = false;

    softSerial->isSearchingForStartBit = true;
    softSerial->rxBitIndex = 0;

    softSerial->transmissionErrors = 0;
    softSerial->receiveErrors = 0;

    softSerial->softSerialPortIndex = portIndex;

    softSerial->txIO = IOGetByTag(softSerial->txTimerHardware->tag);
    serialOutputPortConfig(softSerial->txTimerHardware->tag, portIndex);

    softSerial->rxIO = IOGetByTag(softSerial->rxTimerHardware->tag);
    serialInputPortConfig(softSerial->rxTimerHardware->tag, portIndex);

    setTxSignal(softSerial, ENABLE);
    delay(50);

    serialTimerTxConfig(softSerial->txTimerHardware, portIndex, baud);
    serialTimerRxConfig(softSerial->rxTimerHardware, portIndex, options);

    return &softSerial->port;
}

static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg)
{
    ioTag_t tag = timhw->tag;

    if (!tag) {
        return;
    }

    IOInit(IOGetByTag(tag), OWNER_MOTOR, 0);
#ifdef STM32F7
    IOConfigGPIOAF(IOGetByTag(tag), cfg, timhw->alternateFunction);
#else
    IOConfigGPIO(IOGetByTag(tag), cfg);
#endif
}

void mpuIntExtiInit(void)
{
    static bool mpuExtiInitDone = false;

    if (mpuExtiInitDone || !mpuIntExtiConfig) {
        return;
    }

#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)

    IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);

#ifdef ENSURE_MPU_DATA_READY_IS_LOW
    uint8_t status = IORead(mpuIntIO);
    if (status) {
        return;
    }
#endif

    IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
    IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING);   // TODO - maybe pullup / pulldown ?

    EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
    EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
    EXTIEnable(mpuIntIO, true);
#endif

    mpuExtiInitDone = true; 
}

uint8_t detectSpiDevice(void)
{
#ifdef NAZE_SPI_CS_PIN
    nazeSpiCsPin = IOGetByTag(IO_TAG(NAZE_SPI_CS_PIN));
#endif

    uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 };
    uint8_t in[4];
    uint32_t flash_id;

    // try autodetect flash chip
    delay(50); // short delay required after initialisation of SPI device instance.
    ENABLE_SPI_CS;
    spiTransfer(NAZE_SPI_INSTANCE, in, out, sizeof(out));
    DISABLE_SPI_CS;

    flash_id = in[1] << 16 | in[2] << 8 | in[3];
    if (flash_id == FLASH_M25P16_ID)
        return SPI_DEVICE_FLASH;


    // try autodetect MPU
    delay(50);
    ENABLE_SPI_CS;
    spiTransferByte(NAZE_SPI_INSTANCE, MPU_RA_WHO_AM_I | MPU6500_BIT_RESET);
    in[0] = spiTransferByte(NAZE_SPI_INSTANCE, 0xff);
    DISABLE_SPI_CS;

    if (in[0] == MPU6500_WHO_AM_I_CONST)
        return SPI_DEVICE_MPU;

    return SPI_DEVICE_NONE;
}

void max7456Init(const vcdProfile_t *pVcdProfile)
{
#ifdef MAX7456_SPI_CS_PIN
    max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN));
#endif
    IOInit(max7456CsPin, OWNER_OSD_CS, 0);
    IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG);

    spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
    // force soft reset on Max7456
    ENABLE_MAX7456;
    max7456Send(MAX7456ADD_VM0, MAX7456_RESET);
    DISABLE_MAX7456;

    // Setup values to write to registers
    videoSignalCfg = pVcdProfile->video_system;
    hosRegValue = 32 - pVcdProfile->h_offset;
    vosRegValue = 16 - pVcdProfile->v_offset;

#ifdef MAX7456_DMA_CHANNEL_TX
    dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0);
#endif

    // Real init will be made later when driver detect idle.
}

void servoInit(const servoConfig_t *servoConfig)
{
    for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
        const ioTag_t tag = servoConfig->ioTags[servoIndex];

        if (!tag) {
            break;
        }

        servos[servoIndex].io = IOGetByTag(tag);

        IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
        IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);

        const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);

        if (timer == NULL) {
            /* flag failure and disable ability to arm */
            break;
        }

        pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
        servos[servoIndex].enabled = true;
    }
}

static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag)
{
#ifdef USE_MAG_DATA_READY_SIGNAL
    if (mag->magIntExtiTag == IO_TAG_NONE) {
        return;
    }

    const IO_t magIntIO = IOGetByTag(mag->magIntExtiTag);

#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
    uint8_t status = IORead(magIntIO);
    if (!status) {
        return;
    }
#endif

#if defined (STM32F7)
    IOInit(magIntIO, OWNER_COMPASS_EXTI, 0);
    EXTIHandlerInit(&mag->exti, hmc5883_extiHandler);
    EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL));
    EXTIEnable(magIntIO, true);
#else
    IOInit(magIntIO, OWNER_COMPASS_EXTI, 0);
    IOConfigGPIO(magIntIO, IOCFG_IN_FLOATING);
    EXTIHandlerInit(&mag->exti, hmc5883_extiHandler);
    EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
    EXTIEnable(magIntIO, true);
#endif
#else
    UNUSED(mag);
#endif
}

void sdcardInsertionDetectDeinit(void)
{
#ifdef SDCARD_DETECT_PIN
    sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN));
    IOInit(sdCardDetectPin, OWNER_FREE, RESOURCE_NONE, 0);
    IOConfigGPIO(sdCardDetectPin, IOCFG_IN_FLOATING); 
#endif
}

void sdcardInsertionDetectInit(void)
{
#ifdef SDCARD_DETECT_PIN
    sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN));
    IOInit(sdCardDetectPin, OWNER_SDCARD, RESOURCE_INPUT, 0);
    IOConfigGPIO(sdCardDetectPin, IOCFG_IPU); 
#endif
}

static void initInverter(ioTag_t ioTag)
{
    IO_t pin = IOGetByTag(ioTag);
    IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 0);
    IOConfigGPIO(pin, IOCFG_OUT_PP);

    inverterSet(pin, false);
}

void usbCableDetectInit(void)
{
#ifdef USB_DETECT_PIN
    usbDetectPin = IOGetByTag(IO_TAG(USB_DETECT_PIN));

    IOInit(usbDetectPin, OWNER_USB_DETECT, 0);
    IOConfigGPIO(usbDetectPin, IOCFG_OUT_PP);
#endif
}

void rxSpiDeviceInit(rx_spi_type_e spiType)
{
    static bool hardwareInitialised = false;

    if (hardwareInitialised) {
        return;
    }

#ifdef USE_RX_SOFTSPI
    if (spiType == RX_SPI_SOFTSPI) {
        useSoftSPI = true;
        softSpiInit(&softSPIDevice);
    }
    const SPIDevice rxSPIDevice = SOFT_SPIDEV_1;
#else
    UNUSED(spiType);
    const SPIDevice rxSPIDevice = spiDeviceByInstance(RX_SPI_INSTANCE);
    IOInit(IOGetByTag(IO_TAG(RX_NSS_PIN)), OWNER_SPI, RESOURCE_SPI_CS, rxSPIDevice + 1);
#endif // USE_RX_SOFTSPI

#if defined(STM32F10X)
    RCC_AHBPeriphClockCmd(RX_NSS_GPIO_CLK_PERIPHERAL, ENABLE);
    RCC_AHBPeriphClockCmd(RX_CE_GPIO_CLK_PERIPHERAL, ENABLE);
#endif

#ifdef RX_CE_PIN
    // CE as OUTPUT
    IOInit(IOGetByTag(IO_TAG(RX_CE_PIN)), OWNER_RX_SPI, RESOURCE_RX_CE, rxSPIDevice + 1);
#if defined(STM32F10X)
    IOConfigGPIO(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG);
#elif defined(STM32F3) || defined(STM32F4)
    IOConfigGPIOAF(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG, 0);
#endif
    RX_CE_LO();
#endif // RX_CE_PIN
    DISABLE_RX();

#ifdef RX_SPI_INSTANCE
    spiSetDivisor(RX_SPI_INSTANCE, SPI_CLOCK_STANDARD);
#endif
    hardwareInitialised = true;
}

serialPort_t *usbVcpOpen(void)
{
    vcpPort_t *s;

#ifdef STM32F4
    IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, RESOURCE_INPUT, 0);
    IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, RESOURCE_OUTPUT, 0);
    USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
#else
    Set_System();
    Set_USBClock();
    USB_Interrupts_Config();
    USB_Init();
#endif

    s = &vcpPort;
    s->port.vTable = usbVTable;

    return (serialPort_t *)s;
}