#include "sx1262.h"

spi_device_handle_t spiSXko;

#define TAG_SPIDUMP "[SPI]"

void sx1262_init()
{
  ESP_LOGI(TAG, "Initializing SX1262...");

  gpio_set_direction(HSPI_LORA_CS, GPIO_MODE_OUTPUT);
  gpio_set_direction(LORA_BUSY, GPIO_MODE_INPUT);

  gpio_set_level(HSPI_LORA_CS, 1);
  mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_LORA_RST, 1);

  spi_bus_config_t buscfg;
  memset(&buscfg, 0, sizeof(spi_bus_config_t));
  buscfg.miso_io_num = HSPI_MISO_GPIO;
  buscfg.mosi_io_num = HSPI_MOSI_GPIO;
  buscfg.sclk_io_num = HSPI_SCK_GPIO;
  buscfg.max_transfer_sz = 32;
  buscfg.quadwp_io_num = -1;
  buscfg.quadhd_io_num = -1;

  spi_device_interface_config_t devcfg = {
      .command_bits = 0,
      .address_bits = 0,
      .mode = 0,
      .cs_ena_pretrans = 10,
      .clock_speed_hz = 8 * 1000 * 1000,
      .spics_io_num = HSPI_LORA_CS,
      .flags = 0,
      .queue_size = 1};

  ESP_LOGI(TAG, "Adding SPI device...");
  esp_err_t ret = spi_bus_add_device(LORA_SPI_HOST, &devcfg, &spiSXko);
  if (ret != ESP_OK)
  {
    ESP_LOGE(TAG, "SPI device init failed!");
    return;
  }

  ESP_LOGI(TAG, "SX1262 Initialized");
  sx1262_reset();
}

void sx1262_reset()
{
  ESP_LOGI(TAG, "Resetting SX1262...");
  vTaskDelay(pdMS_TO_TICKS(10));
  mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_LORA_RST, 0);
  vTaskDelay(pdMS_TO_TICKS(20));
  mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_LORA_RST, 1);
  vTaskDelay(pdMS_TO_TICKS(10));
  sx1262_wait_for_busy();
  ESP_LOGI(TAG, "SX1262 Reset complete.");
}

void sx1262_wait_for_busy()
{
  ESP_LOGV("WAIT", "Waiting for SX1262 to be ready...");
  while (gpio_get_level(LORA_BUSY) == 1)
  {
    vTaskDelay(pdMS_TO_TICKS(10));
  }
  ESP_LOGV("WAIT", "SX1262 is ready.");
}

void sx1262_write_command(uint8_t cmd, uint8_t *data, uint8_t len)
{
  ESP_LOGV(TAG, "Writing command 0x%02X with length %d", cmd, len);
  spi_transaction_t t;
  memset(&t, 0, sizeof(t));
  t.length = (len + 1) * 8;
  uint8_t *tx_data = malloc(len + 1);
  memset(tx_data, 0, len + 1);
  tx_data[0] = cmd;
  if (len)
  {
    memcpy(tx_data + 1, data, len);
  }
  t.tx_buffer = tx_data;

  sx1262_wait_for_busy();

  ESP_LOGI(TAG_SPIDUMP, "SPI TX (cmd 0x%02X):", cmd);
  ESP_LOG_BUFFER_HEXDUMP(TAG_SPIDUMP, tx_data, len + 1, ESP_LOG_INFO);

  esp_err_t ret = spi_device_transmit(spiSXko, &t);

  if (ret != ESP_OK)
  {
    ESP_LOGE(TAG, "SPI write failed!");
  }
  else
  {
    ESP_LOGV(TAG, "SPI write successful");
  }
  free(tx_data);
}

uint8_t SX1262_STATUS = 0;

void sx1262_read_command(uint8_t cmd, uint8_t *tx_payload_buffer, uint8_t tx_payload_len, uint8_t *rx_buffer, uint8_t len)
{
  ESP_LOGV(TAG, "Reading command 0x%02X with expected length %d", cmd, len);
  spi_transaction_t t;
  memset(&t, 0, sizeof(t));
  t.length = (len + 2) * 8;
  uint8_t *tx_data = malloc(len + 2);
  uint8_t *rx_data = malloc(len + 2);
  memset(tx_data, 0, len + 2);
  memset(rx_data, 0, len + 2);

  memset(tx_data, 0, sizeof(tx_data));
  memset(rx_data, 0, sizeof(rx_data));
  if (tx_payload_buffer != NULL && tx_payload_len > 0)
  {
    if (tx_payload_len > len + 1)
    {
      tx_payload_len = len + 1;
    }
    memcpy(tx_data + 1, tx_payload_buffer, tx_payload_len);
  }
  tx_data[0] = cmd;

  t.tx_buffer = tx_data;
  t.rx_buffer = rx_data;

  sx1262_wait_for_busy();

  ESP_LOGI(TAG_SPIDUMP, "SPI TX (cmd 0x%02X):", cmd);
  ESP_LOG_BUFFER_HEXDUMP(TAG_SPIDUMP, tx_data, len + 1, ESP_LOG_INFO);

  esp_err_t ret = spi_device_transmit(spiSXko, &t);

  if (ret == ESP_OK)
  {
    SX1262_STATUS = rx_data[1];
    memcpy(rx_buffer, &rx_data[2], len);
    ESP_LOGI(TAG_SPIDUMP, "SPI RX (cmd 0x%02X):", cmd);
    ESP_LOG_BUFFER_HEXDUMP(TAG_SPIDUMP, rx_data, len + 2, ESP_LOG_INFO);
    ESP_LOGV(TAG_SPIDUMP, "SPI read successful, status: 0x%02X", SX1262_STATUS);
  }
  else
  {
    ESP_LOGE(TAG, "SPI read failed!");
  }
  free(rx_data);
  free(tx_data);
}

void resolve_status_byte(uint8_t status, uint16_t op_error)
{
  const char *chip_modes[] = {
      "Unused", "RFU", "STBY_RC", "STBY_XOSC",
      "FS", "RX", "TX", "RFU"};

  const char *command_statuses[] = {
      "Reserved", "RFU", "Data is available to host", "Command timeout",
      "Command processing error", "Failure to execute command", "Command TX done", "RFU"};

  uint8_t chip_mode = (status >> 4) & 0x07;      // Bits 6:4
  uint8_t command_status = (status >> 1) & 0x07; // Bits 3:1

  const char *chip_mode_str = chip_modes[chip_mode];
  const char *command_status_str = command_statuses[command_status];

  const char *op_errors[] = {
      "RC64k calibration failed",
      "RC13M calibration failed",
      "PLL calibration failed",
      "ADC calibration failed",
      "IMG calibration failed",
      "XOSC failed to start",
      "PLL failed to lock",
      "RFU",
      "PA ramping failed"};

  char errorStr[1024];
  memset(errorStr, 0, sizeof(errorStr));

  if (command_status == 0x05)
  {
    strcat(errorStr, "Command error: ");
  }

  for (int i = 0; i < 9; i++)
  {
    if (op_error & (1 << i))
    {
      strcat(errorStr, op_errors[i]);
      strcat(errorStr, ", ");
      ESP_LOGV(TAG, "OpError: %s", op_errors[i]);
    }
  }

  ESP_LOGI(TAG, "Chip Mode: %s(0x%02X), Command Status: %s(0x%02X) %s", chip_mode_str, chip_mode, command_status_str, command_status, errorStr);
}

sx1262_status_t sx1262_get_status()
{
  ESP_LOGV(TAG, "Getting SX1262 status...");
  sx1262_status_t status;
  uint8_t buffer[1] = {0};
  sx1262_read_command(0xC0, NULL, 0, buffer, 1);
  status.status = buffer[0];
  status.error = sx1262_getDeviceErrors();
  sx1262_clearDeviceErrors();
  resolve_status_byte(status.status, status.error);
  return status;
}

void sx1262_setSleep(uint8_t sleepCFG)
{
  sx1262_write_command(0x84, &sleepCFG, 1);
}

void sx1262_setStandby(uint8_t standbyConf)
{
  sx1262_write_command(0x80, &standbyConf, 1);
}

void sx1262_setFrequencySynthesis()
{
  sx1262_write_command(0xC1, NULL, 0);
}

void sx1262_setMode(uint8_t mode, uint32_t timeout)
{

  uint32_t timeoutUnits = 0;

  if (timeout != 0)
  {
    uint64_t timeoutInUs = (uint64_t)timeout * 1000;      // Convert ms → us
    timeoutUnits = (uint32_t)((timeoutInUs * 64) / 1000); // Convert us → SX1262 units
  }

  uint8_t buffer[4] = {
      (uint8_t)((timeoutUnits >> 24) & 0xFF),
      (uint8_t)((timeoutUnits >> 16) & 0xFF),
      (uint8_t)((timeoutUnits >> 8) & 0xFF),
      (uint8_t)((timeoutUnits) & 0xFF)};
  if (mode)
  {
    sx1262_write_command(0x83, buffer, 3);
  }
  else
  {
    sx1262_write_command(0x82, buffer, 3);
  }
}

void sx1262_stopTimerOnPreamble(uint8_t enable)
{
  sx1262_write_command(0x9F, &enable, 1);
}

void sx1262_setRxDutyCycle(uint32_t rxPeriod, uint32_t sleepPeriod)
{
  uint8_t payload[6];
  payload[0] = (rxPeriod >> 16) & 0xFF;
  payload[1] = (rxPeriod >> 8) & 0xFF;
  payload[2] = rxPeriod & 0xFF;
  payload[3] = (sleepPeriod >> 16) & 0xFF;
  payload[4] = (sleepPeriod >> 8) & 0xFF;
  payload[5] = sleepPeriod & 0xFF;
  sx1262_write_command(0x94, payload, 6);
}

void sx1262_setChannelActivityDetection(void)
{
  sx1262_write_command(0xC5, NULL, 0);
}

void sx1262_setTxContinuousWave(void)
{
  sx1262_write_command(0xD1, NULL, 0);
}

void sx1262_setTxInfinitePreamble(void)
{
  sx1262_write_command(0xD2, NULL, 0);
}

void sx1262_setRegulatorMode(uint8_t mode)
{
  sx1262_write_command(0x96, &mode, 1);
}

void sx1262_calibrate(uint8_t calibParam)
{
  sx1262_write_command(0x89, &calibParam, 1);
}

void sx1262_calibrateImage(uint8_t freq1, uint8_t freq2)
{
  uint8_t payload[2] = {freq1, freq2};
  sx1262_write_command(0x98, payload, 2);
}

void sx1262_setRxTXFallbackMode(uint8_t fallbackMode)
{
  sx1262_write_command(0x93, &fallbackMode, 1);
}

// Write to register function
void sx1262_writeRegister(uint16_t address, const uint8_t *data, size_t length)
{
  uint8_t payload[length + 2];
  payload[0] = (address >> 8) & 0xFF;
  payload[1] = address & 0xFF;
  for (size_t i = 0; i < length; i++)
  {
    payload[i + 2] = data[i];
  }
  sx1262_write_command(0x0D, payload, length + 2);
}

// Read from register function
void sx1262_readRegister(uint16_t address, uint8_t *data, size_t length)
{
  uint8_t payload[2];
  payload[0] = (address >> 8) & 0xFF;
  payload[1] = address & 0xFF;
  uint8_t rx_payload[2 + length];
  sx1262_read_command(0x1D, payload, 2, rx_payload, 2 + length);
  for (size_t i = 0; i < length; i++)
  {
    data[i] = rx_payload[i + 2];
  }
}

// Write to buffer function
void sx1262_writeBuffer(uint8_t offset, const uint8_t *data, size_t length)
{
  uint8_t payload[length + 1];
  payload[0] = offset;
  for (size_t i = 0; i < length; i++)
  {
    payload[i + 1] = data[i];
  }
  sx1262_write_command(0x0E, payload, length + 1);
}

// Read from buffer function
void sx1262_readBuffer(uint8_t offset, uint8_t *data, size_t length)
{
  uint8_t tx_payload[1];
  uint8_t payload[1 + length];
  tx_payload[0] = offset;
  sx1262_read_command(0x1E, tx_payload, 1, payload, 1 + length);
  for (size_t i = 0; i < length; i++)
  {
    data[i] = payload[i + 1];
  }
}

void sx1262_setDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask)
{
  uint8_t payload[8] = {
      (uint8_t)((irqMask >> 8) & 0xFF), (uint8_t)((irqMask) & 0xFF),
      (uint8_t)((dio1Mask >> 8) & 0xFF), (uint8_t)(dio1Mask & 0xFF),
      (uint8_t)((dio2Mask >> 8) & 0xFF), (uint8_t)(dio2Mask & 0xFF),
      (uint8_t)((dio3Mask >> 8) & 0xFF), (uint8_t)(dio3Mask & 0xFF)};
  sx1262_write_command(0x08, payload, 8);
}

uint16_t sx1262_getIrqStatus(void)
{
  uint8_t response[2] = {0};
  sx1262_read_command(0x12, NULL, 0, response, 2);
  return (response[0] << 8) | response[1];
}

void sx1262_clearIrqStatus(uint16_t clearIrqParam)
{
  uint8_t payload[2] = {
      (uint8_t)((clearIrqParam >> 8) & 0xFF),
      (uint8_t)(clearIrqParam & 0xFF)};
  sx1262_write_command(0x02, payload, 2);
}

void sx1262_setDIO2AsRfSwitchCtrl(uint8_t enable)
{
  uint8_t buf[1];
  buf[0] = enable;
  sx1262_write_command(0x9D, buf, 1);
}

void sx1262_setDIO3AsTCXOCtrl(uint8_t tcxoVoltage, uint32_t delay)
{
  uint8_t payload[4] = {
      tcxoVoltage,
      (uint8_t)((delay >> 16) & 0xFF),
      (uint8_t)((delay >> 8) & 0xFF),
      (uint8_t)(delay & 0xFF)};
  sx1262_write_command(0x97, payload, 4);
}

void sx1262_setFrequency(uint32_t frequency)
{
  uint32_t freq_reg = frequency * FREQ_STEP;
  uint8_t buffer[4] = {
      (uint8_t)((freq_reg >> 24) & 0xFF),
      (uint8_t)((freq_reg >> 16) & 0xFF),
      (uint8_t)((freq_reg >> 8) & 0xFF),
      (uint8_t)((freq_reg) & 0xFF)};
  sx1262_write_command(0x86, buffer, 4);
}

void sx1262_setPacketType(uint8_t packetType)
{
  uint8_t data[1];
  data[0] = packetType;
  sx1262_write_command(0x8A, data, 1);
}

void sx1262_setLoRaModulationParams(const sx1262_LoRaModulationParams_t *params)
{
  uint8_t payload[4] = {
      params->spreadingFactor,
      params->bandwidth,
      params->codingRate,
      params->lowDataRateOpt};
  sx1262_write_command(0x8B, payload, sizeof(payload));
}

void sx1262_setGFSKModulationParams(const sx1262_GFSKModulationParams_t *params)
{
  uint8_t payload[8] = {
      (uint8_t)((params->bitRate >> 16) & 0xFF),
      (uint8_t)((params->bitRate >> 8) & 0xFF),
      (uint8_t)(params->bitRate & 0xFF),
      params->pulseShape,
      params->bandwidth,
      (uint8_t)((params->frequencyDev >> 16) & 0xFF),
      (uint8_t)((params->frequencyDev >> 8) & 0xFF),
      (uint8_t)(params->frequencyDev & 0xFF)};
  sx1262_write_command(0x8B, payload, sizeof(payload));
}

uint8_t sx1262_getPacketType()
{
  uint8_t response;
  sx1262_read_command(0x11, NULL, 0, &response, 1);
  return response;
}

void sx1262_configure_tx_power(uint8_t paDutyCycle, uint8_t hpMax, uint8_t paLut, int8_t power, uint8_t rampTime)
{
  if (paDutyCycle > 0x04)
    paDutyCycle = 0x04;
  if (rampTime > 0x07)
    rampTime = 0x07;
  if (hpMax > 0x07)
    hpMax = 0x07;

  uint8_t deviceSel;
  if (power >= -9)
  {
    deviceSel = 0x00; // High Power PA
    if (power > 22)
      power = 22;
    if (power < -9)
      power = -9;
  }
  else
  {
    deviceSel = 0x01; // Low Power PA
    if (power > 14)
      power = 14;
    if (power < -17)
      power = -17;
  }

  uint8_t paConfig[4] = {paDutyCycle, hpMax, deviceSel, paLut};
  sx1262_write_command(0x95, paConfig, 4);

  // Direct cast keeps correct 2's complement representation
  uint8_t txPower[2] = {(uint8_t)power, rampTime};
  sx1262_write_command(0x8E, txPower, 2);
}

void sx1262_setLoRaPacketParams(sx1262_LoRaPacketParams_t *params)
{
  uint8_t payload[6] = {
      (uint8_t)((params->preambleLength >> 8) & 0xFF),
      (uint8_t)(params->preambleLength & 0xFF),
      params->headerType,
      params->payloadLength,
      params->crcType,
      params->invertIQ};
  sx1262_write_command(0x8C, payload, sizeof(payload));
}

void sx1262_setCadParams(uint8_t cadSymbolNum, uint8_t cadDetPeak, uint8_t cadDetMin, uint8_t cadExitMode, uint32_t cadTimeout)
{
  uint8_t payload[7] = {
      cadSymbolNum,
      cadDetPeak,
      cadDetMin,
      cadExitMode,
      (uint8_t)((cadTimeout >> 16) & 0xFF),
      (uint8_t)((cadTimeout >> 8) & 0xFF),
      (uint8_t)(cadTimeout & 0xFF)};
  sx1262_write_command(0x88, payload, sizeof(payload));
}

void sx1262_setBufferBaseAddress(uint8_t txBaseAddr, uint8_t rxBaseAddr)
{
  uint8_t payload[2] = {txBaseAddr, rxBaseAddr};
  sx1262_write_command(0x8F, payload, sizeof(payload));
}

void sx1262_setLoRaSymbNumTimeout(uint8_t symbNum)
{
  sx1262_write_command(0xA0, &symbNum, 1);
}

void sx1262_getStatus(uint8_t *status)
{
  sx1262_read_command(0xC0, NULL, 0, status, 0);
}

void sx1262_getRxBufferStatus(uint8_t *payloadLengthRx, uint8_t *rxStartBufferPointer)
{
  uint8_t response[2];
  sx1262_read_command(0x13, NULL, 0, response, sizeof(response));
  *payloadLengthRx = response[0];
  *rxStartBufferPointer = response[1];
}

void sx1262_getPacketStatus(uint8_t *rssi, uint8_t *snr, uint8_t *signalRssi)
{
  uint8_t response[3];
  sx1262_read_command(0x14, NULL, 0, response, sizeof(response));
  *rssi = response[0];
  *snr = response[1];
  *signalRssi = response[2];
}

uint8_t sx1262_getRssiInst(uint8_t *rssiInst)
{
  uint8_t response;
  sx1262_read_command(0x15, NULL, 0, &response, 1);
  return response;
}

void sx1262_getStats(uint16_t *pktReceived, uint16_t *pktCrcError, uint16_t *pktHeaderErr)
{
  uint8_t response[6];
  sx1262_read_command(0x10, NULL, 0, response, sizeof(response));
  *pktReceived = (response[0] << 8) | response[1];
  *pktCrcError = (response[2] << 8) | response[3];
  *pktHeaderErr = (response[4] << 8) | response[5];
}

void sx1262_resetStats(void)
{
  uint8_t payload[6] = {0};
  sx1262_write_command(0x00, payload, sizeof(payload));
}

uint16_t sx1262_getDeviceErrors()
{
  uint8_t response[2];
  sx1262_read_command(0x17, NULL, 0, response, sizeof(response));
  return (((uint16_t)response[0]) << 8) | (uint16_t)response[1];
}

void sx1262_clearDeviceErrors(void)
{
  uint8_t payload[2] = {0x00, 0x00};
  sx1262_write_command(0x07, payload, sizeof(payload));
}