#include <string.h>
#include <math.h>

#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
#include "freertos/queue.h"

#include "esp_log.h"
#include "esp_heap_caps.h"
#include "driver/gpio.h"
#include "driver/spi_common.h"
#include "driver/spi_master.h"
#include "driver/spi_common_internal.h"

#include "esp32-lora.h"

#define READ_REG     0x7F
#define WRITE_REG    0x80

#define PA_OUTPUT_RFO_PIN      0
#define PA_OUTPUT_PA_BOOST_PIN 1

const long long frequencies[] = { 433e+6, 866e+6, 915e+6 };
const long bandwidths[] = { 7.8e3, 10.4e3, 15.6e3, 20.8e3, 31.25e3, 41.7e3, 62.5e3, 125e3, 250e3, 500e3 };

const char *TAG = "LoRa32";

static QueueHandle_t dio_event_queue;
static TaskHandle_t dio_task_handle;
static SemaphoreHandle_t spi_semaphore;

uint8_t lora32_read_reg(lora32_cfg_t *lora, uint8_t address) {
  xSemaphoreTake(spi_semaphore, portMAX_DELAY);

	spi_transaction_t t;
  memset(&t, 0, sizeof(spi_transaction_t));

  t.length = 16;
  t.flags = SPI_TRANS_USE_TXDATA | SPI_TRANS_USE_RXDATA;
  t.tx_data[0] = address & READ_REG;
  t.tx_data[1] = 0x00;

	ESP_ERROR_CHECK(spi_device_transmit(lora->spi, &t));

  ESP_LOGV(TAG, "<%2X<%2X", address, t.rx_data[1]);

  xSemaphoreGive(spi_semaphore);

	return t.rx_data[1];
}

void lora32_write_reg(lora32_cfg_t *lora, uint8_t address, uint8_t value) {
  xSemaphoreTake(spi_semaphore, portMAX_DELAY);

  spi_device_handle_t spi = lora->spi;

  spi_transaction_t t;
  memset(&t, 0, sizeof(spi_transaction_t));

  ESP_LOGV(TAG, ">%2X>%2X", address, value);

  t.length = 16;
  t.flags = SPI_TRANS_USE_TXDATA;
  t.tx_data[0] = address | WRITE_REG;
  t.tx_data[1] = value;

  ESP_ERROR_CHECK(spi_device_transmit(spi, &t));

  xSemaphoreGive(spi_semaphore);
};

double lora32_calc_datarate(lora32_cfg_t *lora) {
  double cr = (4.0 / (long)lora->codingRate);
  double sf = pow(2, lora->spreadingFactor);
  double c2 = sf / bandwidths[lora->bandwidth];
  ESP_LOGI(TAG, "codingRate: %d cr: %f sf: %f c2: %f", lora->codingRate, cr, sf, c2);

  return lora->spreadingFactor * cr / c2 * 1000;
}

void lora23_set_explicit_header(lora32_cfg_t *lora) {
  ESP_LOGD(TAG, "setting explicit header");

  lora->implicitHeader = false;

  lora32_write_reg(lora, REG_MODEM_CONFIG_1, lora32_read_reg(lora, REG_MODEM_CONFIG_1) & 0xFE);
}

void lora23_set_implicit_header(lora32_cfg_t *lora) {
  ESP_LOGD(TAG, "setting implicit header");

  lora->implicitHeader = true;

  lora32_write_reg(lora, REG_MODEM_CONFIG_1, lora32_read_reg(lora, REG_MODEM_CONFIG_1) | 0x01);
}

void lora32_standby(lora32_cfg_t *lora) {
  ESP_LOGV(TAG, "MODE_STANDBY");

  lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_STANDBY);
}

void lora32_sleep(lora32_cfg_t *lora) {
  ESP_LOGV(TAG, "MODE_SLEEP");

  lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_SLEEP);
}

void lora32_enable_fhss(lora32_cfg_t *lora, uint8_t period) {
  lora->enableFHSS = true;

  lora32_write_reg(lora, REG_HOP_PERIOD, period);
}

void lora32_disable_fhss(lora32_cfg_t *lora) {
  lora->enableFHSS = false;

  lora32_write_reg(lora, REG_HOP_PERIOD, 0);
}

void lora32_enable_tx(lora32_cfg_t *lora) {
  lora32_standby(lora);

  if(lora->implicitHeader)
    lora23_set_implicit_header(lora);
  else
    lora23_set_explicit_header(lora);

  // zero out receive buffer
  lora32_write_reg(lora, REG_FIFO_ADDR_PTR, 0);
  lora32_write_reg(lora, REG_PAYLOAD_LENGTH, 0);
}

void lora32_send(lora32_cfg_t *lora, uint8_t *data, uint8_t len) {
  ESP_ERROR_CHECK(spi_device_acquire_bus(lora->spi, portMAX_DELAY));

  lora32_write_reg(lora, REG_DIO_MAPPING_1, DIO0_MODE_TXDONE);

  lora32_enable_tx(lora);

  uint8_t i = 0;
  for(; (i < len && i < MAX_PKT_LENGTH); i++)
    lora32_write_reg(lora, REG_FIFO, data[i]);

  lora32_write_reg(lora, REG_PAYLOAD_LENGTH, len);

  lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_TX);

  spi_device_release_bus(lora->spi);
}

void lora32_set_frequency(lora32_cfg_t *lora, long frequency) {
  ESP_LOGI(TAG, "lora32_set_frequency: %lu", frequency);
  uint64_t frf = ((uint64_t)frequency << 19) / 32000000;

  ESP_LOGI(TAG, "REG_FRF_MSB: 0x%2X", (uint8_t)(frf >> 16));
  ESP_LOGI(TAG, "REG_FRF_MID: 0x%2X", (uint8_t)(frf >>  8));
  ESP_LOGI(TAG, "REG_FRF_LSB: 0x%2X", (uint8_t)(frf >>  0));

  lora32_write_reg(lora, REG_FRF_MSB, (uint8_t)(frf >> 16));
  lora32_write_reg(lora, REG_FRF_MID, (uint8_t)(frf >>  8));
  lora32_write_reg(lora, REG_FRF_LSB, (uint8_t)(frf >>  0));

  ESP_LOGD(TAG, "REG_FRF_MSB: 0x%2X", lora32_read_reg(lora, REG_FRF_MSB));
  ESP_LOGD(TAG, "REG_FRF_MID: 0x%2X", lora32_read_reg(lora, REG_FRF_MID));
  ESP_LOGD(TAG, "REG_FRF_LSB: 0x%2X", lora32_read_reg(lora, REG_FRF_LSB));
}

void lora32_set_ocp(lora32_cfg_t *lora, uint8_t ma) {
}

void lora32_set_tx_power(lora32_cfg_t *lora, uint8_t level, uint8_t output) {
  if(output == PA_OUTPUT_RFO_PIN) {
    if(level > 14) level = 14;

    lora32_write_reg(lora, REG_PA_CONFIG, 0x70 | level);
  } else {
    if(level > 17) {
      // cap power level to 20
      if(level > 20) level = 20;

      level -= 3;

      lora32_write_reg(lora, REG_PA_DAC, 0x07);
      // TODO: set over current protection
    } else {
      if(level < 2) level = 2;

      lora32_write_reg(lora, REG_PA_DAC, 0x04);
      // TODO: set over current protection
    }

    lora32_write_reg(lora, REG_PA_CONFIG, PA_BOOST | (level - 2));
  }

  ESP_LOGI(TAG, "set_tx_power(%d, %d)", level, output);

  ESP_LOGD(TAG, "REG_PA_CONFIG: 0x%2X", lora32_read_reg(lora, REG_PA_CONFIG));
}

uint8_t lora32_data_available(lora32_cfg_t *lora) {
  return lora32_read_reg(lora, REG_RX_NB_BYTES) - lora->fifoIdx;
}

void lora32_dump_regs(lora32_cfg_t *lora) {
  uint8_t i;
  char endline[17] = {0};

  printf("      ");

  for(i = 0; i < 16; i++)
    printf(" %X%s", i, (i % 2 == 1 ? " " : ""));
  printf("\n");

  for(i = 0; i < 127; i++) {
    if(i % 16 == 0)
      printf("0x%02X: ", i);

    char c = lora32_read_reg(lora, i);
    endline[i % 16] = (c >= 32 ? c : '.');

    printf("%02X%s", c, i % 2 == 1 ? " " : "");

    if(i % 16 == 15)
      printf("  %s\n", endline);
  }

  printf("\n");
}

void lora32_toggle_reset(lora32_cfg_t *config) {
  // toggle reset (L/H)
  ESP_LOGI(TAG, "Toggling reset pin %d", config->reset);

  gpio_set_level(config->reset, 0);
  vTaskDelay(100 / portTICK_PERIOD_MS); // requires 100us

  gpio_set_level(config->reset, 1);
  vTaskDelay(100 / portTICK_PERIOD_MS); // 5ms before available
}

uint8_t lora32_get_spreadingfactor(lora32_cfg_t *lora) {
  return (lora32_read_reg(lora, REG_MODEM_CONFIG_2) >> 4);
}

void lora32_set_spreadfactor(lora32_cfg_t *lora, uint8_t factor) {
  if(factor <= 6) {
    factor = 6;

    lora32_write_reg(lora, REG_DETECTION_OPTIMIZE, DETECT_OPT_SF6);
    lora32_write_reg(lora, REG_DETECTION_THRESHOLD, DETECT_THRES_SF6);
  } else {
    if(factor > 12) factor = 12;

    lora32_write_reg(lora, REG_DETECTION_OPTIMIZE, DETECT_OPT_OTHER);
    lora32_write_reg(lora, REG_DETECTION_THRESHOLD, DETECT_THRES_OTHER);
  }

  ESP_LOGI(TAG, "lora32_set_spreadfactor: %d", factor);

  lora32_write_reg(lora, REG_MODEM_CONFIG_2, (lora32_read_reg(lora, REG_MODEM_CONFIG_2) & 0x0F) | ((factor << 4) & 0xF0));
}

void lora32_enable_single_rx(lora32_cfg_t *lora) {
  ESP_LOGD(TAG, "MODE_RX_SINGLE");

  if(lora->receive != NULL) {
    lora32_write_reg(lora, REG_DIO_MAPPING_1, DIO0_MODE_RXDONE);
  }

  lora32_standby(lora);

  lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_RX_SINGLE);
}

void lora32_enable_continuous_rx(lora32_cfg_t *lora) {
  ESP_LOGD(TAG, "MODE_RX_CONTINUOUS");

  if(lora->receive != NULL) {
    lora32_write_reg(lora, REG_DIO_MAPPING_1, DIO0_MODE_RXDONE);
  }

  lora32_standby(lora);

  lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_RX_CONTINUOUS);
}

void lora32_enable_cad(lora32_cfg_t *lora) {
  if((lora->cad_done != NULL) | (lora->cad_detected != NULL)) {
    ESP_LOGD(TAG, "Setting DIO0 to CAD Detect");

    lora32_write_reg(lora, REG_DIO_MAPPING_1, DIO0_MODE_CADDET);
  }

  ESP_LOGD(TAG, "MODE_CAD_DETECT");

  lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_CAD_DETECT);
}

long lora32_get_bandwidth(lora32_cfg_t *lora) {
  uint8_t bw = (lora32_read_reg(lora, REG_MODEM_CONFIG_1) >> 4);
  ESP_LOGI(TAG, "lora32_get_bandwidth: %d", bw);

  if(bw > 9) return -1;

  ESP_LOGI(TAG, "lora32_get_bandwidth: %ld", bandwidths[bw]);

  return bandwidths[bw];
}

void lora32_set_ldo_flag(lora32_cfg_t *lora) {
  long symbolDuration = 1000 / (lora32_get_bandwidth(lora) / (1L << lora32_get_spreadingfactor(lora)));

  ESP_LOGI(TAG, "symbolDuration: %ld", symbolDuration);

  bool ldoOn = symbolDuration > 16;
  ESP_LOGI(TAG, "ldoOn: %d", ldoOn);

  uint8_t modem_config_3 = lora32_read_reg(lora, REG_MODEM_CONFIG_3);
  ESP_LOGI(TAG, "modem_config_3: %d", modem_config_3);
  modem_config_3 |= ldoOn << 3;
  ESP_LOGI(TAG, "modem_config_3: %d", modem_config_3);
  lora32_write_reg(lora, REG_MODEM_CONFIG_3, modem_config_3);
}

void lora32_set_bandwidth(lora32_cfg_t *lora, uint8_t bw) {
  ESP_LOGI(TAG, "lora32_set_bandwidth: %d", bw);

  uint8_t modem_config_1 = lora32_read_reg(lora, REG_MODEM_CONFIG_1);
  lora32_write_reg(lora, REG_MODEM_CONFIG_1, (modem_config_1 & 0x0F) | (bw << 4));

  lora32_set_ldo_flag(lora);
}

void lora32_set_coding_rate(lora32_cfg_t *lora, uint8_t cr) {
  if(cr < 5) cr = 5;
  else if(cr > 8) cr = 8;

  cr = cr  - 4;

  ESP_LOGI(TAG, "lora32_set_coding_rate: %d", cr + 4);

  lora32_write_reg(lora, REG_MODEM_CONFIG_1, (lora32_read_reg(lora, REG_MODEM_CONFIG_1) & 0xF1) | (cr << 1));
}

void lora32_set_lna(lora32_cfg_t *lora, uint8_t gain) {
  // clamp gain values between 0b001 & 0b110
  // NOTE: don't clamp until full REG_LNA is implmented
  //if(gain == 0) gain = 1;
  //else if(gain > 6) gain = 6;

  uint8_t lna = lora32_read_reg(lora, REG_LNA);

  lora32_write_reg(lora, REG_LNA, lna | gain);
  ESP_LOGI(TAG, "set lna: 0x%2X", lna | gain);
}

static void lora32_handle_receive(lora32_cfg_t *lora) {
  uint8_t len = lora32_read_reg(lora, (lora->implicitHeader ? REG_PAYLOAD_LENGTH : REG_RX_NB_BYTES));
  ESP_LOGD(TAG, "lora32_handle_receive packet length: %d", len);

  // TODO: set FIFO address to RX address
  uint8_t fifo_addr = lora32_read_reg(lora, REG_FIFO_RX_CURRENT_ADDR);
  ESP_LOGD(TAG, "lora32_handle_receive current FIFO address: %d", fifo_addr);

  lora32_write_reg(lora, REG_FIFO_ADDR_PTR, fifo_addr);

  lora->receive(lora, len);
}

static void IRAM_ATTR lora32_dio_task(void *arg) {
  // allocate lora32_cfg_t to receive config from Queu
  lora32_cfg_t *lora = malloc(sizeof(lora32_cfg_t));
  ESP_LOGI(TAG, "starting DIO handler task");

  while(1) {
    // wait for event over Queue
    if(xQueueReceive(dio_event_queue, (void*)lora, portMAX_DELAY) != pdPASS) continue;

    // need a better way to log which event and from which config
    //ESP_LOGI(TAG, "handling DIO0 on GPIO%d", lora->dio0);

    spi_device_acquire_bus(lora->spi, portMAX_DELAY);

    // read IRQ flags
    uint8_t irqs = lora32_read_reg(lora, REG_IRQ_FLAGS);
    ESP_LOGD(TAG, "reading irqs: %02X", irqs);

    // clear IRQ flags by writing mask back
    ESP_LOGD(TAG, "clearing irqs");
    lora32_write_reg(lora, REG_IRQ_FLAGS, irqs);

    spi_device_release_bus(lora->spi);

    // TODO handle header validation
    if((irqs & IRQ_RX_DONE) == IRQ_RX_DONE) {
      lora32_handle_receive(lora);
    }

    if((irqs & IRQ_TX_DONE) == IRQ_TX_DONE) {
      if(lora->tx_done != NULL) lora->tx_done(lora);
    }

    bool cad_detected = false;

    if((irqs & IRQ_CAD_DETECTED) == IRQ_CAD_DETECTED) {
      // this is for the next clause, CAD Done callback gets true/false
      cad_detected = true;

      // no need for arg, cad_detected callback is always presummed true
      if(lora->cad_detected != NULL) lora->cad_detected(lora);
    }

    if((irqs & IRQ_CAD_DONE) == IRQ_CAD_DONE) {
      // cad_done gets true/false from above, when activity is detected
      // these *should* fire at the same time, defaults to false
      if(lora->cad_done != NULL) lora->cad_done(lora, cad_detected);
    }

    if((irqs & IRQ_FHSS_CHANGE) == IRQ_FHSS_CHANGE) {
      ESP_LOGI(TAG, "switching channel %d: %d", lora->channel, lora->channels[lora->channel]);

      if(lora->channel == 0 && lora->channels[lora->channel] == 0) continue;

      if(lora->channels[lora->channel] == 0) {
        ESP_LOGI(TAG, "reseting to channel 0");
        lora->channel = 0;
      }

      lora32_set_frequency(lora, lora->frequency + (lora->channels[lora->channel] * bandwidths[lora->bandwidth]));

      lora->channel++;
    }
  }
}

void lora32_read_data(lora32_cfg_t *lora, uint8_t *data) {
  uint8_t i = 0;
  uint8_t len = lora32_read_reg(lora, (lora->implicitHeader ? REG_PAYLOAD_LENGTH : REG_RX_NB_BYTES));

  ESP_LOGI(TAG, "Reading %d bytes", len);

  for(i = 0; i < len; i++) {
    data[i] = lora32_read_reg(lora, REG_FIFO);
  }
}

static void IRAM_ATTR lora32_on_dio(void *arg) {
  xQueueSend(dio_event_queue, arg, (TickType_t)0);
}

uint8_t lora32_spi_init(lora32_cfg_t *lora) {
  ESP_LOGI(TAG, "Initializing SPI bus");

  esp_err_t err = ESP_OK;

  spi_bus_config_t buscfg = {
    .miso_io_num = lora->cipo,
    .mosi_io_num = lora->copi,
    .sclk_io_num = lora->clk,
    .quadwp_io_num = -1,
    .quadhd_io_num = -1
  };

  if(spi_bus_get_attr(lora->spi_host) == NULL) {
    // 2 should be SPI_DMA_CH_AUTO ??? but it doesn't seem to be defined
    err = spi_bus_initialize(lora->spi_host, &buscfg, 2);
  }

  return err;
}

uint8_t lora32_init(lora32_cfg_t *lora) {
  if(spi_semaphore == NULL) {
    spi_semaphore = xSemaphoreCreateMutex();
  }

  gpio_config_t io_conf;
  io_conf.intr_type = GPIO_PIN_INTR_DISABLE;
  io_conf.mode = GPIO_MODE_OUTPUT;

	io_conf.pin_bit_mask = (1ULL<<lora->reset)|(1ULL<<lora->nss);
	io_conf.pull_down_en = 0;
	io_conf.pull_up_en = 0;
	gpio_config(&io_conf);

  lora32_toggle_reset(lora);
  // set NSS high
  ESP_LOGI(TAG, "Bringing NSS high: %d", lora->nss);
  gpio_set_level(lora->nss, 1);

  vTaskDelay(10 / portTICK_PERIOD_MS);

  // SPI device setup
  spi_device_interface_config_t devcfg = {
    .clock_speed_hz = 8E6,
    .flags = 0,
    .mode = 0,
    .spics_io_num = lora->nss,
    .queue_size = 7,
  };

  ESP_ERROR_CHECK(spi_bus_add_device(lora->spi_host, &devcfg, &lora->spi));

  // initialize event groups
  lora->handle.events = xEventGroupCreate();

  uint8_t version = lora32_read_reg(lora, REG_VERSION);
  ESP_LOGD(TAG, "lora32_get_id() == 0x%2X", version);

  // if ID does not match, something is likely wrong on the SPI bus
  if(version != 0x12) {
    ESP_LOGD(TAG, "REG_VERSION returned incorrectly. Expected 0x12 got 0x%02X", version);

    return ERR_LOR_VERSION_MISMATCH;
  }

  // TODO: confirm this is happening. Before/after power measurements?
  lora32_sleep(lora);
  ESP_LOGI(TAG, "lora32_sleep");

  // TODO: VERIFY
  lora32_set_frequency(lora, lora->frequency);
  lora32_set_spreadfactor(lora, lora->spreadingFactor);
  lora32_set_bandwidth(lora, lora->bandwidth);
  lora32_set_coding_rate(lora, lora->codingRate);

  lora32_write_reg(lora, REG_FIFO_TX_BASE_ADDR, 0x00);
  lora32_write_reg(lora, REG_FIFO_RX_BASE_ADDR, 0x00);
  ESP_LOGI(TAG, "clear rx/tx fifos");
  
  lora32_set_lna(lora, 0x03);

  // enable auto AGC
  lora32_write_reg(lora, REG_MODEM_CONFIG_3, 0x04);

  // TODO make based on config
  lora32_set_tx_power(lora, 17, PA_OUTPUT_PA_BOOST_PIN);
  ESP_LOGI(TAG, "lora32_set_tx_power");

  lora32_standby(lora);
  ESP_LOGI(TAG, "lora32_standby");

  if(lora->implicitHeader)
    lora23_set_implicit_header(lora);
  else
    lora23_set_explicit_header(lora);

  // TODO setup shouldn't be based on just receive callback
  if(lora->receive != NULL) {
    ESP_LOGI(TAG, "Setting GPIO Interrupt");

    // TODO check at least one DIOx pin is not NULL

    io_conf.intr_type = GPIO_PIN_INTR_POSEDGE;
    io_conf.pin_bit_mask = ((1ULL << lora->dio0) | (1ULL << lora->dio1) | (1ULL << lora->dio2));
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pull_down_en = 0;
    io_conf.pull_up_en = 0;
    gpio_config(&io_conf);

    gpio_set_intr_type(lora->dio0, GPIO_INTR_POSEDGE);
    gpio_set_intr_type(lora->dio1, GPIO_INTR_POSEDGE);
    gpio_set_intr_type(lora->dio2, GPIO_INTR_POSEDGE);

    // the DIO interrupt handling for every device is done from one task
    if(dio_task_handle == NULL) {
      ESP_LOGI(TAG, "Setting callback handler and ISR service");

      // enable global ISR service
      gpio_install_isr_service(0);

      dio_event_queue = xQueueCreate(10, sizeof(lora32_cfg_t));

      // this should probably be high priority
      xTaskCreate(&lora32_dio_task, "lora32_dio_task", 4096, NULL, 6, &dio_task_handle);
    }

    // add ISR handler to the global service started (once) above
    ESP_LOGI(TAG, "Installing ISR handler for GPIO%d", lora->dio0);
    if(lora->dio0 > -1)
      gpio_isr_handler_add(lora->dio0, lora32_on_dio, (void*)lora);

    if(lora->dio1 > -1)
      gpio_isr_handler_add(lora->dio1, lora32_on_dio, (void*)lora);

    if(lora->dio2 > -1)
      gpio_isr_handler_add(lora->dio2, lora32_on_dio, (void*)lora);
  }

  return 1;
};