#include "tusb_stdio_driver.h"
#include "tusb.h"

#include "class/cdc/cdc_device.h"
#include "device/usbd.h"
#include "pico/sem.h"
#include "pico/stdio/driver.h"
#include "pico/time.h"


#include "hardware/irq.h"
#include "hardware/timer.h"

#define STDIO_DUAL_USB_STDOUT_TIMEOUT_US 10000

semaphore_t stdio_dual_usb_mutex;
static uint8_t low_priority_irq_num;

bool stdio_dual_usb_connected() {
  const int itf = get_core_num();
  return tud_cdc_n_connected(itf);
}

static void low_priority_irq_worker(void) {
  if (sem_try_acquire(&stdio_dual_usb_mutex)) {
    tud_task();
    sem_release(&stdio_dual_usb_mutex);
  }
}

static void stdio_dual_usb_out_chars(const char *buf, int length) {
  static uint64_t last_avail_time[2] = {};
  const int itf = get_core_num();
  // if (!sem_acquire_timeout_ms(&stdio_dual_usb_mutex, 500)) {
  //   return;
  // }
  sem_acquire_blocking(&stdio_dual_usb_mutex);
  if (stdio_dual_usb_connected()) {
    for (int i = 0; i < length;) {
      int n = length - i;
      int avail = (int)tud_cdc_n_write_available(itf);
      if (n > avail)
        n = avail;
      if (n) {
        int n2 = (int)tud_cdc_n_write(itf, buf + i, (uint32_t)n);
        tud_task();
        tud_cdc_n_write_flush(itf);
        i += n2;
        last_avail_time[itf] = time_us_64();
      } else {
        tud_task();
        tud_cdc_n_write_flush(itf);
        if (!stdio_dual_usb_connected() ||
            (!tud_cdc_write_available() &&
             time_us_64() >
                 last_avail_time[itf] + STDIO_DUAL_USB_STDOUT_TIMEOUT_US)) {
          break;
        }
      }
    }
  } else {
    last_avail_time[itf] = 0;
    tud_task();
  }
  sem_release(&stdio_dual_usb_mutex);
}

static void stdio_dual_usb_out_flush(void) {
  const int itf = get_core_num();
  // if (!sem_acquire_timeout_ms(&stdio_dual_usb_mutex, 500)) {
  //   return;
  // }
  sem_acquire_blocking(&stdio_dual_usb_mutex);
  int last_time = time_us_64();
  do {
    tud_task();
  } while (tud_cdc_n_write_flush(itf) && time_us_64() > last_time + STDIO_DUAL_USB_STDOUT_TIMEOUT_US);
  sem_release(&stdio_dual_usb_mutex);
}

int stdio_dual_usb_in_chars(char *buf, int length) {
  const int itf = get_core_num();
  int rc = PICO_ERROR_NO_DATA;
  // if (!sem_acquire_timeout_ms(&stdio_dual_usb_mutex, 500)) {
  //   return rc;
  // }
  sem_acquire_blocking(&stdio_dual_usb_mutex);
  if (stdio_dual_usb_connected() && tud_cdc_n_available(itf)) {
    int count = (int)tud_cdc_n_read(itf, buf, (uint32_t)length);
    rc = count ? count : PICO_ERROR_NO_DATA;
  } else {
    tud_task();
  }
  sem_release(&stdio_dual_usb_mutex);
  return rc;
}

void irq_handler() { irq_set_pending(low_priority_irq_num); }

stdio_driver_t stdio_dual_usb = {
    .out_chars = stdio_dual_usb_out_chars,
    .out_flush = stdio_dual_usb_out_flush,
    .in_chars = stdio_dual_usb_in_chars,
    .crlf_enabled = PICO_STDIO_DEFAULT_CRLF,
};

bool stdio_dual_usb_init(void) {
  if (get_core_num() != alarm_pool_core_num(alarm_pool_get_default())) {
    assert(false);
    return false;
  }

  assert(tud_inited());

  sem_init(&stdio_dual_usb_mutex, 1,1);
  // low_priority_irq_num = user_irq_claim_unused(true);
  // irq_set_exclusive_handler(low_priority_irq_num, low_priority_irq_worker);
  // irq_set_enabled(low_priority_irq_num, true);

  // if (irq_has_handler(USBCTRL_IRQ)) {
  //   irq_add_shared_handler(USBCTRL_IRQ, irq_handler,
  //                          PICO_SHARED_IRQ_HANDLER_LOWEST_ORDER_PRIORITY);
  // } else {
  //   return false;
  // }

  stdio_set_driver_enabled(&stdio_dual_usb, true);

  return true;
}