#include <string.h>

#include "freertos/FreeRTOS.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 "esp32-lora.h"

#define READ_REG     0x7F
#define WRITE_REG    0x80

#define PIN_NUM_MISO 19
#define PIN_NUM_MOSI 27
#define PIN_NUM_CLK  5

#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.8e+3, 10.4e+3, 15.6e+3, 20.8e+3, 31.25e+3, 41.7e+3, 62.5e+3, 125e+3, 250e+3 };

const char *TAG = "LoRa32";

lora32_cfg_t lora32_create() {
  static spi_device_handle_t spi;

  return (lora32_cfg_t){
    .bandwidth = bandwidths[B125],
    .codingRate = DEFAULT_CR,
    .dio0 = CONFIG_LORA32_DIO0_PIN,
    .implicitHeader = false,
    .nss = CONFIG_LORA32_NSS_PIN,
    .reset = CONFIG_LORA32_RESET_PIN,
    .frequency = 866000000,
    .preamble = DEFAULT_PREAMBLE,
    .spreadingFactor = DEFAULT_SF,
    .spi = spi,
    .receive = NULL,
    .useCRC = false,
    .fifoIdx = 0
  };
}

void lora32_send_cmd(lora32_cfg_t lora, uint8_t cmd, uint8_t value, uint8_t len, uint8_t *rx_buffer) {
  uint8_t tx_buffer[2];

  tx_buffer[0] = cmd;
  tx_buffer[1] = value;

  spi_transaction_t t;
  memset(&t, 0, sizeof(t));
  t.length = 8 * len;
  t.rxlength = 8 * len;
  t.tx_buffer = &tx_buffer;
  t.rx_buffer = rx_buffer;

  ESP_ERROR_CHECK(spi_device_transmit(lora.spi, &t));

  //ESP_LOGI(TAG, "send_cmd rx_data: 0x%2X 0x%2X", rx_buffer[0], rx_buffer[1]);
}

uint8_t lora32_read_reg(lora32_cfg_t *lora, uint8_t address) {
	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_LOGD(TAG, "<%2X<%2X", address, t.rx_data[1]);

	return t.rx_data[1];
}

void lora32_write_reg(lora32_cfg_t *lora, uint8_t address, uint8_t value) {
  spi_device_handle_t spi = lora->spi;

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

  ESP_LOGD(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));
};

void lora23_set_explicit_header(lora32_cfg_t *lora) {
  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) {
  lora->implicitHeader = true;

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

void lora32_idle(lora32_cfg_t *lora) {
  lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_STANDBY);
}

void lora32_sleep(lora32_cfg_t *lora) {
  lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_SLEEP);

  //ESP_LOGI(TAG, "REG_OP_MODE: 0x%2X", lora32_read_reg(spi, REG_OP_MODE));
}

void lora32_enable_tx(lora32_cfg_t *lora) {
  lora32_idle(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) {
  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);

  ESP_LOGD(TAG, "lora32_send waiting for TX to finish");

  // can be made async by waiting for DIO0 and checking for IRQ_TX_DONE_MASK
  while((lora32_read_reg(lora, REG_IRQ_FLAGS) & IRQ_TX_DONE_MASK) == 0) {
    vTaskDelay(1 / portTICK_PERIOD_MS);
  }

  ESP_LOGD(TAG, "lora32_send TX done");

  lora32_write_reg(lora, REG_IRQ_FLAGS, IRQ_TX_DONE_MASK);

  // questionable....
  lora32_enable_continous_rx(lora);
}

void lora32_set_frequency(lora32_cfg_t *lora, long 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) & 0xFF);
  lora32_write_reg(lora, REG_FRF_MID, (uint8_t)(frf >>  8) & 0xFF);
  lora32_write_reg(lora, REG_FRF_LSB, (uint8_t)(frf >>  0) & 0xFF);

  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_tx_power(lora32_cfg_t *lora, uint8_t level, uint8_t output) {
  ESP_LOGI(TAG, "set_tx_power(%d, %d)", level, output);

  if(output == PA_OUTPUT_RFO_PIN) {
    if(level > 14) level = 14;

    lora32_write_reg(lora, REG_PA_CONFIG, 0x70 | level);
  } else {
    if(level < 2) level = 2;
    else if(level > 17) level = 17;

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

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

uint8_t lora32_parse_packet(lora32_cfg_t *lora, uint8_t size) {
  uint8_t length = 0;
  uint8_t irqs = lora32_read_reg(lora, REG_IRQ_FLAGS);

  if(size > 0) {
    lora23_set_implicit_header(lora);

    lora32_write_reg(lora, REG_PAYLOAD_LENGTH, size & 0xFF);
  } else {
    lora23_set_explicit_header(lora);
  }

  lora32_write_reg(lora, REG_IRQ_FLAGS, irqs);

  //ESP_LOGI(TAG, "irqs: 0x%2X", irqs);
  //ESP_LOGI(TAG, "irqs: 0x%2X", lora32_read_reg(spi, REG_IRQ_FLAGS));

  if ((irqs & IRQ_RX_DONE_MASK) && (irqs & IRQ_PAYLOAD_CRC_ERROR_MASK) == 0) {
    lora->fifoIdx = 0;

    if(lora->implicitHeader) {
      length = lora32_read_reg(lora, REG_PAYLOAD_LENGTH);
    } else {
      length = lora32_read_reg(lora, REG_RX_NB_BYTES);
    }

    lora32_write_reg(lora, REG_FIFO_ADDR_PTR, lora32_read_reg(lora, REG_FIFO_RX_CURRENT_ADDR));

    lora32_idle(lora);
  } else if(lora32_read_reg(lora, REG_OP_MODE) != (MODE_LONG_RANGE_MODE | MODE_RX_SINGLE)) {
    lora32_write_reg(lora, REG_FIFO_ADDR_PTR, 0);
    //lora32_write_reg(lora, REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_RX_SINGLE);
  } else {
    //ESP_LOGI(TAG, "no irqs?");
  }

  return length;
}

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) {
  char endline[17] = {0};

  for(uint8_t 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
}

void lora32_set_spreadfactor(lora32_cfg_t *lora, uint8_t factor) {
  ESP_LOGI(TAG, "lora32_set_spreadfactor: %d", 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);
  }

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

void lora32_enable_continuous_rx(lora32_cfg_t *lora) {
  ESP_LOGD(TAG, "enabling continuous receive");

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

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

  uint8_t cr = d  - 4;

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

static void IRAM_ATTR lora32_receive_task(void *arg) {
  lora32_cfg_t *lora = (lora32_cfg_t*)arg;
  ESP_LOGD(TAG, "starting DIO0 handler task");

  while(1) {
		EventBits_t evbits = xEventGroupWaitBits(lora->handle.events, EV_DIO0, pdTRUE, pdFALSE, 5000 / portTICK_PERIOD_MS);

    // timed out, loop and continue to wait
    if(evbits == 0) continue;

    ESP_LOGD(TAG, "handling DIO0");

    // read IRQ flags
    ESP_LOGD(TAG, "reading irqs");
    uint8_t irqs = lora32_read_reg(lora, REG_IRQ_FLAGS);
    // clear IRQ flags
    ESP_LOGD(TAG, "clearing irqs");
    lora32_write_reg(lora, REG_IRQ_FLAGS, irqs);

    uint8_t len = lora32_read_reg(lora, (lora->implicitHeader ? REG_PAYLOAD_LENGTH : REG_RX_NB_BYTES));
    ESP_LOGD(TAG, "lora32_receive_task 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_receive_task current FIFO address: %d", fifo_addr);

    lora32_write_reg(lora, REG_FIFO_ADDR_PTR, fifo_addr);

    lora->receive(len);
  }
}

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);
    printf("%02X (%c) ", data[i], (data[i] > 32 ? data[i] : ' '));
  }

  printf("\n");
}

static void IRAM_ATTR lora32_on_dio0(void *arg) {
  BaseType_t woken = pdFALSE;

  xEventGroupSetBitsFromISR(((lora32_cfg_t*)arg)->handle.events, EV_DIO0, &woken);
}

uint8_t lora32_init(lora32_cfg_t *lora) {
  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);

  // init spi
  ESP_LOGI(TAG, "Initializing SPI bus");
  ESP_LOGI(TAG, "\n                     MISO: %d\nMOSI: %d\nCLK: %d\nNSS: %d", PIN_NUM_MISO, PIN_NUM_MOSI, PIN_NUM_CLK, lora->nss);

  spi_bus_config_t buscfg = {
    .miso_io_num = PIN_NUM_MISO,
    .mosi_io_num = PIN_NUM_MOSI,
    .sclk_io_num = PIN_NUM_CLK,
    .quadwp_io_num = -1,
    .quadhd_io_num = -1
  };

  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_initialize(HSPI_HOST, &buscfg, 0));
  ESP_ERROR_CHECK(spi_bus_add_device(HSPI_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);
  assert(version == 0x12);

  // 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);
  ESP_LOGI(TAG, "lora32_set_frequency: %lu", lora->frequency);

  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");
  
  uint8_t lna = lora32_read_reg(lora, REG_LNA);
  lora32_write_reg(lora, REG_LNA, lna | 0x03);
  ESP_LOGI(TAG, "set lna: 0x%2X", lna | 0x03);

  lora32_write_reg(lora, REG_MODEM_CONFIG_3, 0x04);
  //ESP_LOGI(TAG, "REG_MODEM_CONFIG_3: 0x%2X", lora32_read_reg(spi, REG_MODEM_CONFIG_3));

  lora32_set_tx_power(lora, 17, PA_OUTPUT_PA_BOOST_PIN);
  ESP_LOGI(TAG, "lora32_set_tx_power");

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

  //ESP_LOGI(TAG, "REG_OP_MODE: 0x%2X", lora32_read_reg(spi, REG_OP_MODE));

  if(lora->receive != NULL) {
    ESP_LOGI(TAG, "Setting callback handler");

    io_conf.intr_type = GPIO_PIN_INTR_POSEDGE;
    io_conf.pin_bit_mask = (1ULL << CONFIG_LORA32_DIO0_PIN);
    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(CONFIG_LORA32_DIO0_PIN, GPIO_INTR_POSEDGE);
    gpio_install_isr_service(0);

    gpio_isr_handler_add(CONFIG_LORA32_DIO0_PIN, lora32_on_dio0, (void*)lora);

    // this should probably be high priority
    xTaskCreate(&lora32_receive_task, "lora32_receive_task", 14048, (void*)lora, 6, NULL);

    // TODO comment and probably change 0x00 to a macro
    lora32_write_reg(lora, REG_DIO_MAPPING_1, 0x00);

    lora32_enable_continous_rx(lora);
  }

  return 1;
};