#include <iomanip>
#include <iostream>

#include "CosmicAsyncSDK.hpp"

#define SEND_DELAY 0

uint16_t txId = 0;

uint16_t getTxId() {
  return ++txId;
}

void printResponse(const std::vector<uint8_t>& response) {
  for (const auto& byte : response) {
    std::cerr << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
  }
}

std::shared_ptr<CosmicAsyncSDK> sdk;

using ResponseCallback = std::function<void(const std::vector<uint8_t>&)>;

int main() {
  sdk = std::make_unique<CosmicAsyncSDK>();

  sdk->onResponse([](const std::vector<uint8_t>& response) {
    std::cerr << "Received response (raw): ";
    printResponse(response);
    std::cerr << std::dec << std::endl;
    std::cerr << "----------------------------------------" << std::endl;
    CommandResponseHeader formattedResponse = *reinterpret_cast<const CommandResponseHeader*>(response.data());
    std::cerr << "Received response (formatted):" << std::endl;
    std::cerr << "  ID: " << std::dec << formattedResponse.id << std::endl;
    std::cerr << "  Code: 0x" << std::hex << formattedResponse.code << std::endl;
    std::cerr << "  Status: " << std::dec << formattedResponse.result << std::endl;
    std::cerr << "  Length: " << std::dec << response.size() << std::endl;
    std::cerr << "----------------------------------------" << std::endl;
    switch (formattedResponse.code) {
      case CMD_SYS_GET_USB_STRING:
        std::cerr << "CMD_GET_USB_STRING" << std::endl;
        std::cerr << std::string(response.begin() + sizeof(CommandResponseHeader) + 1, response.end()) << std::endl;
        break;
      case CMD_SYS_SET_I2C_SPI_UART_GPIO_VOLTAGE:
        std::cerr << "CMD_SET_I2C_SPI_UART_BUS_VOLTAGE" << std::endl;
        break;
      case CMD_I2C_SET_PULLUP_RESISTORS:
        std::cerr << "CMD_SET_PULL_UP_RESISTORS" << std::endl;
        break;
      case CMD_I2C_CONTROLLER_INIT:
        std::cerr << "CMD_I2C_CONTROLLER_INIT" << std::endl;
        break;
      case CMD_I2C_CONTROLLER_SET_PARAMETERS:
        std::cerr << "CMD_I2C_SET_PARAMETERS" << std::endl;
        break;
      case CMD_I2C_CONTROLLER_WRITE:
        std::cerr << "CMD_I2C_WRITE" << std::endl;
        break;
      case CMD_I2C_CONTROLLER_READ: {
        std::cerr << "CMD_I2C_READ" << std::endl;

        if (response.size() < sizeof(I2cTransferResponseHeader) + 1) {
          std::cerr << "  Error: Response too short." << std::endl;
          return;
        }

        I2cTransferResponseHeader i2cReadResponse = *reinterpret_cast<const I2cTransferResponseHeader*>(response.data());

        uint8_t i2cBusId        = i2cReadResponse.header.id;
        uint16_t i2cReadPayload = i2cReadResponse.parameters.payloadLength;

        std::cerr << "  I2C Bus ID: " << std::dec << i2cBusId << std::endl;
        std::cerr << "  Payload Length: " << std::dec << i2cReadPayload << std::endl;
        std::cerr << "  Data Length: " << std::dec << response.size() - sizeof(I2cTransferResponseHeader) << std::endl;

        std::vector<uint8_t> data(response.begin() + sizeof(I2cTransferResponseHeader), response.end());
        std::cerr << "Data: ";
        printResponse(std::vector<uint8_t>(data));
        std::cerr << std::endl;

        break;
      }
      default:
        std::cerr << "  Unknown code: " << std::hex << formattedResponse.code << std::endl;
        break;
    }
    std::cerr << "========================================" << std::endl;
  });

  std::cout << "----------------------------------------" << std::endl;
  std::cout << "CosmicAsyncSDK Example" << std::endl;
  std::cout << "----------------------------------------" << std::endl
            << std::endl;

  std::cout << "Opening device..." << std::endl
            << std::endl;

  bool ok = sdk->connect();
  if (!ok) {
    std::cerr << "Failed to open device!" << std::endl;
    return 1;
  }

  sdk->getUsbString(getTxId(), GET_FW_VERSION);
  std::this_thread::sleep_for(std::chrono::microseconds(SEND_DELAY));

  sdk->i2cSetBusVoltage(getTxId(), 3300);
  std::this_thread::sleep_for(std::chrono::microseconds(SEND_DELAY));

  sdk->i2cControllerInit(getTxId(), I2C_BUS_A, 200000, I2C_PULLUP_330Ohm);
  std::this_thread::sleep_for(std::chrono::microseconds(SEND_DELAY));

  size_t transferLength = 1;

  std::vector<uint8_t> reset_payload(transferLength, 0x00);

  sdk->i2cWrite(getTxId(), I2C_BUS_A, 0x50, {0x00, 0x00}, reset_payload, false);
  std::this_thread::sleep_for(std::chrono::microseconds(SEND_DELAY));

  sdk->i2cRead(getTxId(), I2C_BUS_A, 0x50, {0x00, 0x00}, transferLength, false);
  std::this_thread::sleep_for(std::chrono::microseconds(SEND_DELAY));

  std::vector<uint8_t> payload(transferLength, 0xFF);

  for (int i = 0; i < 10; ++i) {
    sdk->i2cWrite(getTxId(), I2C_BUS_A, 0x50, {0x00, 0x00}, payload, false);
    std::this_thread::sleep_for(std::chrono::microseconds(SEND_DELAY));
  }

  sdk->i2cRead(getTxId(), I2C_BUS_A, 0x50, {0x00, 0x00}, transferLength, false);
  std::this_thread::sleep_for(std::chrono::microseconds(SEND_DELAY));

  // delay for a while to allow the response to be processed
  std::this_thread::sleep_for(std::chrono::seconds(1));

  sdk->disconnect();

  std::cout << "Device disconnected." << std::endl;
  return 0;
}
