//
// Created by cfif on 13.11.2023.
//
#include "SerialPort_USB.h"
#include "SystemDelayInterface.h"
#include "CmsisRtosThreadUtils.h"

#if (OTG1_USB_ID == 1)
otg_core_type otg1_core_struct;
tSerialPortUsbArtery *SerialPortUsbArteryOtg1 = NULL;
#endif

#if (OTG2_USB_ID == 1)
otg_core_type otg2_core_struct;
tSerialPortUsbArtery *SerialPortUsbArteryOtg2 = NULL;
#endif


void usb_delay_ms(uint32_t ms) {
    SystemDelayMs(ms);
}

#if (OTG1_USB_ID == 1)

void OTG1_IRQ_HANDLER(void) {
    usbd_irq_handler(&otg1_core_struct);

    if (SerialPortUsbArteryOtg1 != NULL) {
        uint16_t data_len = usb_vcp_get_rxdata(&otg1_core_struct.dev, SerialPortUsbArteryOtg1->usb_buffer);

        if (data_len > 0) {
            for (uint16_t i = 0; i < data_len; ++i) {
                osMessageQueuePut(SerialPortUsbArteryOtg1->rxDataQueue, &SerialPortUsbArteryOtg1->usb_buffer[i], 0x0,
                                  0U);

                if (SerialPortUsbArteryOtg1->rxDataSnifferQueue) {
                    osMessageQueuePut(SerialPortUsbArteryOtg1->rxDataSnifferQueue,
                                      &SerialPortUsbArteryOtg1->usb_buffer[i], 0x0, 0U);
                }

            }
        }

    }

}

#endif

#if (OTG2_USB_ID == 1)

void OTG2_IRQ_HANDLER(void) {
    usbd_irq_handler(&otg2_core_struct);

    if (SerialPortUsbArteryOtg2 != NULL) {
        uint16_t data_len = usb_vcp_get_rxdata(&otg2_core_struct.dev, SerialPortUsbArteryOtg2->usb_buffer);

        if (data_len > 0) {
            for (uint16_t i = 0; i < data_len; ++i) {
                osMessageQueuePut(SerialPortUsbArteryOtg2->rxDataQueue, &SerialPortUsbArteryOtg2->usb_buffer[i], 0x0,
                                  0U);

                if (SerialPortUsbArteryOtg2->rxDataSnifferQueue) {
                    osMessageQueuePut(SerialPortUsbArteryOtg2->rxDataSnifferQueue,
                                      &SerialPortUsbArteryOtg2->usb_buffer[i], 0x0, 0U);
                }

            }
        }

    }

}

#endif

void usb_clock48m_select(usb_clk48_s clk_s, uint8_t USB_ID) {
    if (clk_s == USB_CLK_HICK) {
        crm_usb_clock_source_select(CRM_USB_CLOCK_SOURCE_HICK);

        /* enable the acc calibration ready interrupt */
        crm_periph_clock_enable(CRM_ACC_PERIPH_CLOCK, TRUE);

        /* update the c1\c2\c3 value */
        acc_write_c1(7980);
        acc_write_c2(8000);
        acc_write_c3(8020);

        if (USB_ID == 1) {
            acc_sof_select(ACC_SOF_OTG1);
        }

        if (USB_ID == 2) {
            acc_sof_select(ACC_SOF_OTG2);
        }

        // open acc calibration
        acc_calibration_mode_enable(ACC_CAL_HICKTRIM, TRUE);
    } else {
        switch (system_core_clock) {
            // 48MHz
            case 48000000:
                crm_usb_clock_div_set(CRM_USB_DIV_1);
                break;

                // 72MHz
            case 72000000:
                crm_usb_clock_div_set(CRM_USB_DIV_1_5);
                break;

                // 96MHz
            case 96000000:
                crm_usb_clock_div_set(CRM_USB_DIV_2);
                break;

                // 120MHz
            case 120000000:
                crm_usb_clock_div_set(CRM_USB_DIV_2_5);
                break;

                // 144MHz
            case 144000000:
                crm_usb_clock_div_set(CRM_USB_DIV_3);
                break;

                // 168MHz
            case 168000000:
                crm_usb_clock_div_set(CRM_USB_DIV_3_5);
                break;

                // 192MHz
            case 192000000:
                crm_usb_clock_div_set(CRM_USB_DIV_4);
                break;

                // 216MHz
            case 216000000:
                crm_usb_clock_div_set(CRM_USB_DIV_4_5);
                break;

                // 240MHz
            case 240000000:
                crm_usb_clock_div_set(CRM_USB_DIV_5);
                break;

                // 264MHz
            case 264000000:
                crm_usb_clock_div_set(CRM_USB_DIV_5_5);
                break;

                // 288MHz
            case 288000000:
                crm_usb_clock_div_set(CRM_USB_DIV_6);
                break;

            default:
                break;

        }
    }
}

void usb1_gpio_config(void) {
    gpio_init_type gpio_init_struct;

    crm_periph_clock_enable(OTG1_PIN_GPIO_CLOCK, TRUE);
    gpio_default_para_init(&gpio_init_struct);

    gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
    gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
    gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
    gpio_init_struct.gpio_pull = GPIO_PULL_NONE;

    // dp and dm
    gpio_init_struct.gpio_pins = OTG1_PIN_DP | OTG1_PIN_DM;
    gpio_init(OTG1_PIN_GPIO, &gpio_init_struct);

    gpio_pin_mux_config(OTG1_PIN_GPIO, OTG1_PIN_DP_SOURCE, OTG1_PIN_MUX);
    gpio_pin_mux_config(OTG1_PIN_GPIO, OTG1_PIN_DM_SOURCE, OTG1_PIN_MUX);

#ifdef USB_SOF_OUTPUT_ENABLE
    crm_periph_clock_enable(OTG_PIN_SOF_GPIO_CLOCK, TRUE);
  gpio_init_struct.gpio_pins = OTG_PIN_SOF;
  gpio_init(OTG_PIN_SOF_GPIO, &gpio_init_struct);
  gpio_pin_mux_config(OTG_PIN_SOF_GPIO, OTG_PIN_SOF_SOURCE, OTG_PIN_MUX);
#endif

#ifndef USB_VBUS_IGNORE
    gpio_init_struct.gpio_pins = OTG_PIN_VBUS;
  gpio_init_struct.gpio_pull = GPIO_PULL_DOWN;
  gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_VBUS_SOURCE, OTG_PIN_MUX);
  gpio_init(OTG_PIN_GPIO, &gpio_init_struct);
#endif


}

void usb2_gpio_config(void) {
    gpio_init_type gpio_init_struct;

    crm_periph_clock_enable(OTG2_PIN_GPIO_CLOCK, TRUE);
    gpio_default_para_init(&gpio_init_struct);

    gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
    gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
    gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
    gpio_init_struct.gpio_pull = GPIO_PULL_NONE;

    // dp and dm
    gpio_init_struct.gpio_pins = OTG2_PIN_DP | OTG2_PIN_DM;
    gpio_init(OTG2_PIN_GPIO, &gpio_init_struct);

    gpio_pin_mux_config(OTG2_PIN_GPIO, OTG2_PIN_DP_SOURCE, OTG2_PIN_MUX);
    gpio_pin_mux_config(OTG2_PIN_GPIO, OTG2_PIN_DM_SOURCE, OTG2_PIN_MUX);

#ifdef USB_SOF_OUTPUT_ENABLE
    crm_periph_clock_enable(OTG_PIN_SOF_GPIO_CLOCK, TRUE);
  gpio_init_struct.gpio_pins = OTG_PIN_SOF;
  gpio_init(OTG_PIN_SOF_GPIO, &gpio_init_struct);
  gpio_pin_mux_config(OTG_PIN_SOF_GPIO, OTG_PIN_SOF_SOURCE, OTG_PIN_MUX);
#endif

#ifndef USB_VBUS_IGNORE
    gpio_init_struct.gpio_pins = OTG_PIN_VBUS;
  gpio_init_struct.gpio_pull = GPIO_PULL_DOWN;
  gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_VBUS_SOURCE, OTG_PIN_MUX);
  gpio_init(OTG_PIN_GPIO, &gpio_init_struct);
#endif


}

#ifdef USB_LOW_POWER_WAKUP
void usb_low_power_wakeup_config(void)
{
  exint_init_type exint_init_struct;

  crm_periph_clock_enable(CRM_SCFG_PERIPH_CLOCK, TRUE);
  exint_default_para_init(&exint_init_struct);

  exint_init_struct.line_enable = TRUE;
  exint_init_struct.line_mode = EXINT_LINE_INTERRUPUT;
  exint_init_struct.line_select = OTG_WKUP_EXINT_LINE;
  exint_init_struct.line_polarity = EXINT_TRIGGER_RISING_EDGE;
  exint_init(&exint_init_struct);

  nvic_irq_enable(OTG_WKUP_IRQ, 0, 0);
}

/**
  * @brief  this function handles otgfs wakup interrupt.
  * @param  none
  * @retval none
  */
void OTG_WKUP_HANDLER(void)
{
  exint_flag_clear(OTG_WKUP_EXINT_LINE);
}
#endif

static uint16_t vSerialPortReceiveQueue(
        tSerialPortUsbArtery *env, uint8_t *data, uint16_t size, uint32_t timeout, osMessageQueueId_t queueId
) {
    uint16_t received = 0;

    if (timeout) {
        uint32_t endTimeout = SystemGetMs() + timeout;
        uint32_t currentTimeout;

        while (size && ((timeout == SystemWaitForever) || (endTimeout > SystemGetMs()))) {
            currentTimeout = endTimeout - SystemGetMs();
            if (osMessageQueueGet(queueId, data, NULL, currentTimeout) == osOK) {
                --size;
                ++received;
                ++data;
            }
        }
    } else {
        while (size) {
            if (osMessageQueueGet(queueId, data, NULL, 0) == osOK) {
                --size;
                ++received;
                ++data;
            } else {
                return received;
            }
        }
    }

    return received;
}

static uint16_t vSerialPortUsbTransmit(tSerialPortUsbArtery *env, uint8_t *data, uint16_t size, uint32_t timeout) {

    if (env->USB_ID == 1) {
        if (usb_vcp_send_data(&otg1_core_struct.dev, data, size) == SUCCESS) {
            return size;
        }
    }

    if (env->USB_ID == 2) {
        if (usb_vcp_send_data(&otg2_core_struct.dev, data, size) == SUCCESS) {
            return size;
        }
    }


    return 0;
}

static uint16_t vSerialPortUsbReceive(tSerialPortUsbArtery *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortReceiveQueue(env, data, size, timeout, env->rxDataQueue);
}

static uint16_t
vSerialPortUsbSnifferReceive(tSerialPortUsbArtery *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return env->rxDataSnifferQueue
           ? vSerialPortReceiveQueue(env, data, size, timeout, env->rxDataSnifferQueue)
           : 0;
}

/*
static _Noreturn void UsbDriver_Thread(tSerialPortUsbArtery *env) {
    for (;;) {
        uint16_t data_len = usb_vcp_get_rxdata(&otg_core_struct.dev, env->usb_buffer);

        if (data_len > 0) {
            for (uint16_t i = 0; i < data_len; ++i) {
                osMessageQueuePut(env->rxDataQueue, &env->usb_buffer[i], 0x0, 0U);

                if (env->rxDataSnifferQueue) {
                    osMessageQueuePut(env->rxDataSnifferQueue, &env->usb_buffer[i], 0x0, 0U);
                }

            }
        }

//        SystemDelayMs(1);
    }
}

void UsbDriver_StartThread(tSerialPortUsbArtery *env) {
    if (!env->thread.id) {
        env->thread.id = osThreadNew((osThreadFunc_t) (UsbDriver_Thread), (void *) (env), &env->thread.attr);
    } else {
        osThreadResume(env->thread.id);
    }
}
*/
void SerialPortUsb_Init(
        tSerialPortUsbArtery *env,
        uint8_t USB_ID,
        uint32_t rxBufferLength,
        uint32_t rxSnifferLength
) {
    env->USB_ID = USB_ID;

    env->rxDataQueue = osMessageQueueNew(rxBufferLength, 1, NULL);

    if (rxSnifferLength) {
        env->rxDataSnifferQueue = osMessageQueueNew(rxBufferLength, 1, NULL);
    } else {
        env->rxDataSnifferQueue = 0;
    }


    if (env->USB_ID == 1) {

#if (OTG1_USB_ID == 1)
        SerialPortUsbArteryOtg1 = env;
        // usb gpio config
        usb1_gpio_config();
#endif

#ifdef USB_LOW_POWER_WAKUP
        usb_low_power_wakeup_config();
#endif

#if (OTG1_USB_ID == 1)
        // enable otgfs clock
        crm_periph_clock_enable(OTG1_CLOCK, TRUE);

        // select usb 48m clcok source
        usb_clock48m_select(USB_CLK_HEXT, env->USB_ID);

        // enable otgfs irq
        //nvic_irq_enable(OTG_IRQ, 0, 0);
        NVIC_EnableIRQ(OTG1_IRQ);
        NVIC_SetPriority(OTG1_IRQ, 0xFF);

        // init usb
        usbd_init(&otg1_core_struct,
                  USB_FULL_SPEED_CORE_ID,
                  USB1_ID,
                  &cdc_class_handler,
                  &cdc_desc_handler);

//    InitThreadAtrStatic(&env->thread.attr, "UsbDriver", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
//    env->thread.id = 0;
#endif
    }

    if (env->USB_ID == 2) {
#if (OTG2_USB_ID == 1)
        SerialPortUsbArteryOtg2 = env;
        // usb gpio config
        usb2_gpio_config();
#endif

#ifdef USB_LOW_POWER_WAKUP
        usb_low_power_wakeup_config();
#endif

#if (OTG2_USB_ID == 1)
        // enable otgfs clock
        crm_periph_clock_enable(OTG2_CLOCK, TRUE);

        // select usb 48m clcok source
        usb_clock48m_select(USB_CLK_HEXT, env->USB_ID);

        // enable otgfs irq
        //nvic_irq_enable(OTG_IRQ, 0, 0);
        NVIC_EnableIRQ(OTG2_IRQ);
        NVIC_SetPriority(OTG2_IRQ, 0xFF);

        // init usb
        usbd_init(&otg2_core_struct,
                  USB_FULL_SPEED_CORE_ID,
                  USB2_ID,
                  &cdc_class_handler,
                  &cdc_desc_handler);

//    InitThreadAtrStatic(&env->thread.attr, "UsbDriver", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
//    env->thread.id = 0;
#endif
    }
    //UsbDriver_StartThread(env);
}

tSerialPortIO SerialPortUsb_GetIo(tSerialPortUsbArtery *env) {
    tSerialPortIO io = {
            .env = env,
            .receive = (SerialPortIOTransaction) vSerialPortUsbReceive,
            .transmit = (SerialPortIOTransaction) vSerialPortUsbTransmit
    };
    return io;
}

tSerialPortIO SerialPortUsb_GetSnifferIo(tSerialPortUsbArtery *env) {
    tSerialPortIO io = {
            .env = env,
            .receive = (SerialPortIOTransaction) vSerialPortUsbSnifferReceive,
            .transmit =  (SerialPortIOTransaction) vSerialPortUsbTransmit
    };
    return io;
}