#include "sx127x_driver.h"
#include "sx127x_internal.h"
#include "sx127x_registers.h"

#include <esp_log.h>
#include <string.h>

const char *SX127X_TAG = "sx127x";

typedef struct sx127x_packet {
  char * data;
  size_t data_len;
} sx127x_packet_t;

esp_err_t sx127x_init(sx127x_config_t *config, sx127x_t **handle_ptr) {
  esp_err_t ret;
  sx127x_t *handle = malloc(sizeof(sx127x_t));
  SX127X_CHECK(handle != NULL, "malloc error", ESP_ERR_NO_MEM);

  handle->task_handle = NULL;
  handle->task_state  = SX127X_TASK_STOPPED;
  memcpy(&handle->config, config, sizeof(sx127x_config_t));

  ret = gpio_set_direction(config->rst_io_num, GPIO_MODE_OUTPUT);
  SX127X_ERROR_CHECK2(ret, gpio_set_direction)

  // perform reset
  gpio_set_level(config->rst_io_num, 0);
  vTaskDelay(RESET_DELAY);
  gpio_set_level(config->rst_io_num, 1);
  vTaskDelay(RESET_DELAY);

  spi_bus_config_t bus_config = {
      .mosi_io_num     = config->mosi_io_num,
      .miso_io_num     = config->miso_io_num,
      .sclk_io_num     = config->sck_io_num,
      .quadhd_io_num   = -1,
      .quadwp_io_num   = -1,
      .max_transfer_sz = SX127X_MAX_TRANSFER,
  };

  ret = spi_bus_initialize(config->spi_host, &bus_config, 1);
  SX127X_ERROR_CHECK2(ret, spi_bus_initialize)

  spi_device_interface_config_t device_config = {
      .command_bits     = 0,
      .address_bits     = 8,
      .dummy_bits       = 0,
      .mode             = 0,
      .duty_cycle_pos   = 0,
      .cs_ena_pretrans  = 0,
      .cs_ena_posttrans = 0,
      .clock_speed_hz   = SPI_CLOCK_HZ, // 8mhz
      .input_delay_ns   = 0,
      .spics_io_num     = config->cs_io_num,
      .flags            = 0,
      .queue_size       = 8,
      .pre_cb           = NULL,
      .post_cb          = NULL,
  };
  ret = spi_bus_add_device(config->spi_host, &device_config, &handle->device_handle);
  SX127X_ERROR_CHECK2(ret, spi_bus_add_device)

  // read version and check that it is compatible
  uint8_t version;
  ret = sx127x_read_register(handle, REG_VERSION, &version);
  SX127X_ERROR_CHECK2(ret, sx127x_read_register);
  SX127X_CHECK(version == 0x12, "unsupported version %#x", ESP_ERR_INVALID_VERSION, version);

  ret = sx127x_sleep(handle);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_frequency(handle, config->frequency);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_write_register(handle, REG_FIFO_TX_BASE_ADDR, 0);
  SX127X_ERROR_CHECK(ret);
  sx127x_write_register(handle, REG_FIFO_RX_BASE_ADDR, 0);
  SX127X_ERROR_CHECK(ret);

  uint8_t reg_lna;
  ret = sx127x_read_register(handle, REG_LNA, &reg_lna);
  SX127X_ERROR_CHECK(ret);
  reg_lna |= 0x03; // set LNA boost
  ret = sx127x_write_register(handle, REG_LNA, reg_lna);
  SX127X_ERROR_CHECK(ret);

  // set auto AGC
  ret = sx127x_write_register(handle, REG_MODEM_CONFIG_3, CONFIG3_AUTO_AGC);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_tx_power(handle, config->tx_power, true);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_spreading_factor(handle, config->spreading_factor);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_sync_word(handle, config->sync_word);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_crc(handle, config->crc);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_standby(handle);
  ESP_ERROR_CHECK(ret);

  *handle_ptr = handle;
  return ESP_OK;
}

esp_err_t sx127x_free(sx127x_t *handle) {
  esp_err_t ret;

  if (handle->task_handle) {
    ret = sx127x_stop(handle);
    SX127X_ERROR_CHECK(ret);
  }

  ret = spi_bus_remove_device(handle->device_handle);
  SX127X_ERROR_CHECK2(ret, spi_bus_remove_device)

  ret = spi_bus_free(handle->config.spi_host);
  SX127X_ERROR_CHECK2(ret, spi_bus_free)

  free(handle);

  return ESP_OK;
}

void sx127x_isr(void *arg) {
  sx127x_t *handle = (sx127x_t *)arg;
  ESP_LOGV(SX127X_TAG, "sx127x_isr");
  BaseType_t hpTaskWoken;
  xSemaphoreGiveFromISR(handle->intr_semaphore, &hpTaskWoken);
  if (hpTaskWoken) {
    portYIELD_FROM_ISR();
  }
}

void sx127x_do_tx(sx127x_t *handle, sx127x_packet_t *packet) {
  uint8_t op_mode, irq_flags, config_2;
  while (handle->task_state == SX127X_TASK_RUNNING) {
    sx127x_read_register(handle, REG_OP_MODE, &op_mode);
    uint8_t mode = op_mode & SX127X_MODE;
    if (mode != SX127X_MODE_TX && mode != SX127X_MODE_FS_TX) {
      break;
    }
    vTaskDelay(1); // wait for finish transmitting
  }

  sx127x_standby(handle);

  sx127x_read_register(handle, REG_IRQ_FLAGS, &irq_flags);
  if (irq_flags & IRQ_TX_DONE_MASK) {
    // clear tx done bit
    sx127x_write_register(handle, REG_IRQ_FLAGS, IRQ_TX_DONE_MASK);
  }

  sx127x_read_register(handle, REG_MODEM_CONFIG_2, &config_2);
  config_2 &= ~0x01; // set explicit header mode TODO: implicit header?
  sx127x_write_register(handle, REG_MODEM_CONFIG_2, config_2);

  sx127x_write_register(handle, REG_FIFO_ADDR_PTR, 0);
  sx127x_write_register(handle, REG_PAYLOAD_LENGTH, 0);

  sx127x_write_fifo(handle, packet->data, packet->data_len);
  sx127x_write_register(handle, REG_PAYLOAD_LENGTH, packet->data_len);

  sx127x_write_register(handle, REG_OP_MODE, SX127X_LONG_RANGE | SX127X_MODE_FS_TX);
  vTaskDelay(pdMS_TO_TICKS(1));
  sx127x_write_register(handle, REG_OP_MODE, SX127X_LONG_RANGE | SX127X_MODE_TX);

  // wait for transmission to finish
  while (true) {
    sx127x_read_register(handle, REG_IRQ_FLAGS, &irq_flags);
    if (irq_flags & IRQ_TX_DONE_MASK) { // if the transmission is done
      break;
    }
    vTaskDelay(1);
  }
  // clear tx done bit
  sx127x_write_register(handle, REG_IRQ_FLAGS, IRQ_TX_DONE_MASK);

  // go back to rx mode
  sx127x_write_register(handle, REG_OP_MODE, SX127X_LONG_RANGE | SX127X_MODE_RX_CONT);
}

void sx127x_task(void *arg) {
  sx127x_t *       handle     = (sx127x_t *)arg;
  TickType_t       delay_time = 0;
  QueueSetHandle_t qSet       = xQueueCreateSet(8);
  xQueueAddToSet(handle->intr_semaphore, qSet);
  xQueueAddToSet(handle->tx_packet_queue, qSet);
  sx127x_packet_t packet;
  while (handle->task_state == SX127X_TASK_RUNNING) {
    QueueSetMemberHandle_t queue = xQueueSelectFromSet(qSet, delay_time);
    if (queue == handle->intr_semaphore) {
      BaseType_t didRecv = xSemaphoreTake(handle->intr_semaphore, 0);
      if (didRecv) {
        ESP_LOGI(SX127X_TAG, "recv from isr: %d", didRecv);
      }
    } else if (queue == handle->tx_packet_queue) {
      BaseType_t didRecv = xQueueReceive(handle->tx_packet_queue, &packet, 0);
      if (didRecv) {
        ESP_LOGI(SX127X_TAG, "tx packet: %.*s", packet.data_len, packet.data);

        sx127x_do_tx(handle, &packet);

        free(packet.data);
      }
    }
  }
  ESP_LOGI(SX127X_TAG, "sx127x_task exiting");
  handle->task_state = SX127X_TASK_STOPPED;
  vTaskDelete(NULL); // must delete own task
}

esp_err_t sx127x_start(sx127x_t *handle) {
  esp_err_t ret;

  SX127X_CHECK(handle->task_handle == NULL, "task already running", ESP_ERR_INVALID_STATE);

  handle->intr_semaphore = xSemaphoreCreateBinary();
  // handle->recv_packet_queue = xQueueCreate(8, 10);
  handle->tx_packet_queue = xQueueCreate(4, sizeof(sx127x_packet_t));

  handle->task_state = SX127X_TASK_RUNNING;
  BaseType_t pdRet =
      xTaskCreate(sx127x_task, "sx127x_task", TASK_STACK_SIZE, (void *)handle, TASK_PRIORITY, &handle->task_handle);
  SX127X_CHECK(pdRet == pdPASS, "failed to create task", ESP_FAIL);

  ret = sx127x_write_register(handle, REG_DIO_MAPPING_1, 0x00);
  SX127X_ERROR_CHECK(ret);

  gpio_config_t irq_io_config;
  irq_io_config.intr_type    = GPIO_INTR_POSEDGE;
  irq_io_config.mode         = GPIO_MODE_INPUT;
  irq_io_config.pin_bit_mask = (1ULL << handle->config.irq_io_num);
  irq_io_config.pull_down_en = 0;
  irq_io_config.pull_up_en   = 0;
  ret                        = gpio_config(&irq_io_config);
  SX127X_ERROR_CHECK2(ret, gpio_config)

  ret = gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1);
  SX127X_ERROR_CHECK2(ret, gpio_install_isr_service);
  ret = gpio_isr_handler_add(handle->config.irq_io_num, sx127x_isr, (void *)handle);
  SX127X_ERROR_CHECK2(ret, gpio_isr_handler_add);

  return ESP_OK;
}

esp_err_t sx127x_stop(sx127x_t *handle) {
  esp_err_t ret;

  SX127X_CHECK(handle->task_handle != NULL, "task has not been started", ESP_ERR_INVALID_STATE);
  handle->task_state = SX127X_TASK_STOPPING;
  xTaskNotifyGive(handle->task_handle);

  ret = gpio_isr_handler_remove(handle->config.irq_io_num);
  SX127X_ERROR_CHECK2(ret, gpio_isr_handler_remove);
  gpio_uninstall_isr_service();

  while (handle->task_state != SX127X_TASK_STOPPED) {
    vTaskDelay(10);
  }
  handle->task_handle = NULL;

  return ESP_OK;
}

esp_err_t sx127x_send_packet(sx127x_t *handle, char *data, size_t data_len) {
  SX127X_CHECK(data_len < SX127_MAX_PACKET_LEN, "packet len too long: %d", ESP_FAIL, data_len);
  sx127x_packet_t packet;
  packet.data_len = data_len;
  packet.data     = heap_caps_malloc(data_len, MALLOC_CAP_DMA);
  SX127X_CHECK(packet.data != NULL, "malloc error", ESP_ERR_NO_MEM);
  memcpy(packet.data, data, data_len);
  BaseType_t pdRet = xQueueSend(handle->tx_packet_queue, &packet, 0);
  SX127X_CHECK(pdRet == pdTRUE, "tx queue full", ESP_FAIL);
  return ESP_OK;
}