#include "e32_driver.hh"

#include <esp_log.h>

namespace ugv {
namespace e32 {

static constexpr size_t E32_BUF_SIZE         = 1024;
static constexpr size_t E32_UART_RX_BUF_SIZE = 1024;
static constexpr size_t E32_UART_TX_BUF_SIZE = 1024;

static constexpr size_t PARAMS_LEN               = 6;
static const uint8_t    CMD_WRITE_PARAMS_SAVE    = 0xC0;
static const uint8_t    CMD_READ_PARAMS[]        = {0xC1, 0xC1, 0xC1};
static const uint8_t    CMD_WRITE_PARAMS_NO_SAVE = 0xC2;
static const uint8_t    CMD_READ_VERSION[]       = {0xC3, 0xC3, 0xC3};
static const uint8_t    CMD_RESET[]              = {0xC4, 0xC4, 0xC4};

static const char* TAG = "e32_driver";

Config::Config() {
  uart_port   = UART_NUM_1;
  uart_parity = UART_PARITY_DISABLE;
  uart_tx_pin = UART_PIN_NO_CHANGE;
  uart_rx_pin = UART_PIN_NO_CHANGE;
  uart_baud   = 9600;
}

Params::Params() {
  // These are defaults for the 433T30D
  save_params   = true;
  address       = 0x0000;
  uart_partity  = UART_PARITY_DISABLE;
  uart_baud     = 9600;  // bps
  air_data_rate = 2400;  // bps
  comm_channel  = 0x17;
  tx_mode       = TxTransparent;
  io_mode       = IoPushPull;
  wake_up_time  = 250;  // ms
  fec_enabled   = true;
  tx_power      = 30;
}

E32_Driver::E32_Driver() : initialized_(false), config_(), params_() {}

E32_Driver::~E32_Driver() { Free(); }

esp_err_t E32_Driver::Init(Config config) {
  config_ = config;
  uart_config_t uart_config;
  uart_config.baud_rate           = config_.uart_baud;
  uart_config.data_bits           = UART_DATA_8_BITS;
  uart_config.parity              = config_.uart_parity;
  uart_config.stop_bits           = UART_STOP_BITS_1;
  uart_config.flow_ctrl           = UART_HW_FLOWCTRL_DISABLE;
  uart_config.rx_flow_ctrl_thresh = 122;
  uart_config.use_ref_tick        = false;

  esp_err_t ret;
  ret = uart_param_config(config_.uart_port, &uart_config);
  if (ret != ESP_OK) {
    goto error;
  }

  ret =
      uart_set_pin(config_.uart_port, config_.uart_tx_pin, config_.uart_rx_pin,
                   UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
  if (ret != ESP_OK) {
    goto error;
  }

  ret = uart_driver_install(config_.uart_port, E32_UART_RX_BUF_SIZE,
                            E32_UART_TX_BUF_SIZE, 0, NULL, 0);
  if (ret != ESP_OK) {
    goto error;
  }
  initialized_ = true;

  // ReadParams(params_);
  // if (ret != ESP_OK) {
    // goto error;
  // }

  return ESP_OK;

error:
  const char* error_name = esp_err_to_name(ret);
  ESP_LOGE(TAG, "E32_Driver::Init error: %s (%d)", error_name, ret);
  return ret;
}

esp_err_t E32_Driver::Free() {
  esp_err_t ret;
  if (initialized_) {
    ret          = uart_driver_delete(config_.uart_port);
    initialized_ = false;
  } else {
    ret = ESP_ERR_INVALID_STATE;
    ESP_LOGE(TAG, "Free called when not initialized");
  }
  return ret;
}

esp_err_t E32_Driver::ReadParams(Params& params) {
  esp_err_t ret;
  ret = RawWrite(CMD_READ_PARAMS, sizeof(CMD_READ_PARAMS));
  ret = WaitWriteDone();
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "error writing read params cmd");
    return ret;
  }
  uint8_t param_data[PARAMS_LEN];
  ret = Read(param_data, PARAMS_LEN, pdMS_TO_TICKS(1000));
  if (ret != PARAMS_LEN) {
    ESP_LOGE(TAG, "error reading params");
    return ret;
  }

  switch (param_data[0]) {
    case CMD_WRITE_PARAMS_SAVE: params.save_params = true; break;
    case CMD_WRITE_PARAMS_NO_SAVE: params.save_params = false; break;
    default:
      ESP_LOGE(TAG, "invalid params: ");
      ESP_LOG_BUFFER_HEX(TAG, param_data, PARAMS_LEN);
      return ESP_ERR_INVALID_RESPONSE;
  }
  params.address = param_data[1];
  params.address <<= 8;
  params.address |= param_data[2];

  switch (param_data[3] >> 6) {
    case 3:
    case 0: params.uart_partity = UART_PARITY_DISABLE; break;
    case 1: params.uart_partity = UART_PARITY_ODD; break;
    case 2: params.uart_partity = UART_PARITY_EVEN; break;
  }

  switch ((param_data[3] >> 3) & 0b111) {
    case 0: params.uart_baud = 1200; break;
    case 1: params.uart_baud = 2400; break;
    case 2: params.uart_baud = 4800; break;
    case 3: params.uart_baud = 9600; break;
    case 4: params.uart_baud = 19200; break;
    case 5: params.uart_baud = 38400; break;
    case 6: params.uart_baud = 57600; break;
    case 7: params.uart_baud = 115200; break;
  }

  switch (param_data[3] & 0b111) {
    case 0: params.air_data_rate = 300; break;
    case 1: params.air_data_rate = 1200; break;
    case 2: params.air_data_rate = 2400; break;
    case 3: params.air_data_rate = 4800; break;
    case 4: params.air_data_rate = 9600; break;
    case 5:
    case 6:
    case 7: params.air_data_rate = 19200; break;
  }

  params.comm_channel = param_data[4];

  params.tx_mode      = (TxMode)(param_data[5] & (1 << 7));
  params.io_mode      = (IoMode)(param_data[5] & (1 << 6));
  params.wake_up_time = 250 * (((param_data[5] >> 3) & 0b111) + 1);
  params.fec_enabled  = (param_data[5] & (1 << 2)) != 0;

  // assume it is a 30dbm module
  switch (param_data[5] & 0b11) {
    case 0: params.tx_power = 30; break;
    case 1: params.tx_power = 27; break;
    case 2: params.tx_power = 24; break;
    case 3: params.tx_power = 21; break;
  }

  params_ = params;

  return ESP_OK;
}
esp_err_t E32_Driver::WriteParams(const Params& params) {
  esp_err_t ret;
  uint8_t   param_data[PARAMS_LEN];

  param_data[0] =
      params.save_params ? CMD_WRITE_PARAMS_SAVE : CMD_WRITE_PARAMS_NO_SAVE;

  param_data[1] = params.address >> 8;
  param_data[2] = params.address & 0xFF;

  param_data[3] = 0;
  switch (params.uart_partity) {
    case UART_PARITY_DISABLE: break;
    case UART_PARITY_ODD: param_data[3] |= 1 << 6; break;
    case UART_PARITY_EVEN: param_data[3] |= 2 << 6; break;
  }

  if (params.uart_baud <= 1200) {
    param_data[3] |= 0 << 3;
  } else if (params.uart_baud <= 2400) {
    param_data[3] |= 1 << 3;
  } else if (params.uart_baud <= 4800) {
    param_data[3] |= 2 << 3;
  } else if (params.uart_baud <= 9600) {
    param_data[3] |= 3 << 3;
  } else if (params.uart_baud <= 19200) {
    param_data[3] |= 4 << 3;
  } else if (params.uart_baud <= 38400) {
    param_data[3] |= 5 << 3;
  } else if (params.uart_baud <= 57600) {
    param_data[3] |= 6 << 3;
  } else {
    param_data[3] |= 7 << 3;
  }

  if (params.air_data_rate <= 300) {
    param_data[3] |= 0;
  } else if (params.air_data_rate <= 1200) {
    param_data[3] |= 1;
  } else if (params.air_data_rate <= 2400) {
    param_data[3] |= 2;
  } else if (params.air_data_rate <= 4800) {
    param_data[3] |= 3;
  } else if (params.air_data_rate <= 9600) {
    param_data[3] |= 4;
  } else /* if (params.air_data_rate <= 19200) */ {
    param_data[3] |= 5;
  }

  param_data[4] = params.comm_channel;

  param_data[5] = 0;
  param_data[5] |= ((uint8_t)params.tx_mode) << 7;
  param_data[5] |= ((uint8_t)params.io_mode) << 6;
  param_data[5] |= (((params.wake_up_time / 250) - 1) & 0b111) << 3;
  param_data[5] |= ((uint8_t)params.fec_enabled) << 2;

  // assume it is a 30dbm module
  if (params.tx_power >= 30) {
    param_data[5] |= 0;
  } else if (params.tx_power >= 27) {
    param_data[5] |= 1;
  } else if (params.tx_power >= 24) {
    param_data[5] |= 2;
  } else /* if (params.tx_power >= 21) */ {
    param_data[5] |= 3;
  }

  RawWrite(param_data, PARAMS_LEN);
  ret = WaitWriteDone();
  if (ret != ESP_OK) {
    return ret;
  }

  params_ = params;

  return ESP_OK;
}

int E32_Driver::Write(Address address, Channel channel, const uint8_t* data,
                      size_t data_size) {
  int written;
  if (params_.tx_mode == TxFixed) {
    uint8_t header[3];
    header[0] = address >> 8;
    header[1] = address & 0xFF;
    header[2] = channel;

    written =
        uart_write_bytes(config_.uart_port, (char*)header, sizeof(header));
    if (written < 0) {
      return written;
    }
  }
  written = uart_write_bytes(config_.uart_port, (char*)data, data_size);
  return written;
}
int E32_Driver::Write(const uint8_t* data, size_t data_size) {
  return Write(params_.address, params_.comm_channel, data, data_size);
}
int E32_Driver::Write(Address address, Channel channel,
                      const std::string& data) {
  return Write(address, channel, (uint8_t*)data.c_str(), data.size());
}
int E32_Driver::Write(const std::string& data) {
  return Write((uint8_t*)data.c_str(), data.size());
}

esp_err_t E32_Driver::WaitWriteDone(TickType_t ticks_to_wait) {
  return uart_wait_tx_done(config_.uart_port, ticks_to_wait);
}

int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) {
  return uart_read_bytes(config_.uart_port, (uint8_t*)data, max_len,
                         ticks_to_wait);
}

int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) {
  data.clear();
  uint8_t*   buf          = (uint8_t*)malloc(128);
  TickType_t start_tick   = xTaskGetTickCount();
  TickType_t current_tick = start_tick;
  int        read, total_read = 0;
  while (current_tick <= start_tick + ticks_to_wait) {
    read = Read(buf, 128, ticks_to_wait - (current_tick - start_tick));
    if (read < 0) {
      free(buf);
      return read;
    }
    data.append((const char*)buf, read);
    total_read += read;
    current_tick = xTaskGetTickCount();
  }
  free(buf);
  return total_read;
}

void E32_Driver::Flush() {
  uart_flush(config_.uart_port);
}

int E32_Driver::RawWrite(const uint8_t* data, size_t data_size) {
  int written =
      uart_write_bytes(config_.uart_port, (const char*)data, data_size);
  if (written < 0) {
    ESP_LOGE(TAG, "RawWrite error: %d", written);
  }
  return written;
}

}  // namespace e32
}  // namespace ugv