/**
 * @file example.cpp
 * @brief I3C Basic API Example with Binho Supernova (Blocking C++ API)
 *
 * This example demonstrates a complete I3C Basic API workflow using:
 * - Binho Supernova
 * - Binho I3C Target Board
 * - STMicroelectronics STEVAL-MKI239A
 * - Bosch Sensortec BMI323 Shuttle Board 3.0
 * - Adafruit I2C FRAM breakout (legacy I2C target on the I3C bus)
 *
 * Workflow:
 * 1. Connect to the adapter and print host device information.
 * 2. Set I3C VTARG to 3.3 V.
 * 3. Initialize the I3C controller and reconfigure bus parameters.
 * 4. Read and print the current target device table.
 * 5. Initialize the bus without a preloaded target device table to perform discovery and print the updated table.
 * 6. Reset the bus and initialize it with a static target table.
 * 7. Update one target configuration and print the final table state.
 * 8. Perform SDR and legacy I2C transfers to demonstrate communication with the targets.
 * 9. Disconnect from the adapter.
 */

#include <iomanip>
#include <iostream>
#include <vector>

#include "CosmicSDK.hpp"

void printTargetDeviceTable(std::vector<I3cTargetInfo> targetTable){
  for (uint8_t i = 0; i < targetTable.size(); i++) {
    std::cout << "\tStatic address:" << (int)targetTable[i].staticAddress << std::dec << std::endl;
    std::cout << "\tDynamic address: " << (int)targetTable[i].dynamicAddress << std::endl;

    std::cout << "\tPID: ";
    for (uint8_t j = 0; j < 6; ++j) {
        std::cout << (int)targetTable[i].pid[j] << " ";
    }
    std::cout << std::dec << std::endl;

    std::cout << "\tBCR: " << (int)targetTable[i].bcr.byte << std::endl;
    std::cout << "\tDCR: " << (int)targetTable[i].dcr << std::endl;
    std::cout << "\tMWL: " << (int)targetTable[i].mwl << std::endl;
    std::cout << "\tMRL: " << (int)targetTable[i].mrl << std::endl;
    std::cout << "\tMax IBI payload length: " << (int)targetTable[i].maxIbiPayloadLength << std::endl;

    std::cout << "\tTarget configuration:" << std::endl;
    std::cout << "\t\tTarget type: " << (targetTable[i].configuration.bits.targetType ? "I2C" : "I3C") << std::endl;
    std::cout << "\t\tInterrupt request: " << (targetTable[i].configuration.bits.IBIRequest ? "ACCEPT IBI" : "REJECT IBI") << std::endl;
    std::cout << "\t\tController Role request: " << (targetTable[i].configuration.bits.CRRequest ? "ACCEPT CCR" : "REJECT CCR") << std::endl;
    std::cout << "\t\tSETDASSA: " << (targetTable[i].configuration.bits.daaUseSETDASA ? "USE SETDASA" : "DO NOT USE SETDASA") << std::endl;
    std::cout << "\t\tSETDAASA: " << (targetTable[i].configuration.bits.daaUseSETAASA ? "USE SETAASA" : "DO NOT USE SETAASA") << std::endl;
    std::cout << "\t\tENTDAA: " << (targetTable[i].configuration.bits.daaUseENTDAA ? "USE ENTDAA" : "DO NOT USE ENTDAA") << std::endl;
    std::cout << "\t\tIBI Timestamp: " << (targetTable[i].configuration.bits.ibiTimestampEnable ? "ENABLE" : "DISABLE") << std::endl;
    std::cout << "\t\tPending Read capability: " << (targetTable[i].configuration.bits.pendingReadCapability ? "ENABLE" : "DISABLE") << std::endl;
    std::cout << "\t-----------------------------------------------" << std::endl;
  }
}

int main() {
  std::cout << "Starting main code" << std::endl;

  // Initialize the CosmicSDK device
  std::unique_ptr<CosmicSDK> device = std::make_unique<CosmicSDK>(2000);

  std::cout << "------------------------------------------------------------------------" << std::endl;
  std::cout << "------------------ Test connection and initialization ------------------" << std::endl;
  std::cout << "---------------------- Retrieve device information ---------------------" << std::endl;
  std::cout << "------------------------------------------------------------------------" << std::endl;

  // Connect to the device
  if (!device->connect()) {
    std::cerr << "Failed to connect to a Binho Device!" << std::endl;
    exit(1);
  }

  // Variable to hold the retrieved device information
  CosmicSDK::DeviceInfo deviceInformation;
  // Read device information
  if (device->getDeviceInfo(deviceInformation) != SUCCESS) {
    std::cerr << "Failed to read device information!" << std::endl;
    exit(1);
  }

  std::cerr << "Manufacturer: " << deviceInformation.manufacturer << std::endl;
  std::cerr << "Product name: " << deviceInformation.productName << std::endl;
  std::cerr << "Serial number: " << deviceInformation.serialNumber << std::endl;
  std::cerr << "FW version: " << deviceInformation.fwVersion << std::endl;
  std::cerr << "HW version: " << deviceInformation.hwVersion << std::endl;
  std::cout << "\n" << std::endl;

  // Set I3C bus voltage
  std::cout << "Setting voltage to 3.3v: ";
  uint16_t result_code = device->i3cSetBusVoltage(3300);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to set I3C bus voltage! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;

  // Init I3C peripheral
  std::cout << "Initializing I3C controller: ";
  result_code = device->i3cControllerInit(I3cPushPullRate::PUSH_PULL_5_MHz_DC_50, I3cOpenDrainTransferRate::OPEN_DRAIN_400_KHz, I2cTransferRate::FAST_MODE, I3cDriveStrength::FAST);
  if (result_code != SUCCESS && result_code != FW_INTERFACE_ALREADY_INITIALIZED) {
    std::cerr << "Failed to init the I3C peripheral! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;

  // Set I3C parameters
  std::cerr << "Setting I3C parameters: ";
  result_code = device->i3cSetParameters(I3cPushPullRate::PUSH_PULL_2_5_MHz_DC_25, I3cOpenDrainTransferRate::OPEN_DRAIN_100_KHz, I2cTransferRate::STANDARD_MODE, I3cDriveStrength::STANDARD);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to set the I3C parameters! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cerr << "OK" << std::endl;

  std::cout << "-------------------------------------------------------------------------" << std::endl;
  std::cout << "------------------------ Initialize the I3C bus -------------------------" << std::endl;
  std::cout << "--- Retrieve target device table before and after the initialization ----" << std::endl;
  std::cout << "-------------------------------------------------------------------------" << std::endl;

  // Get I3C target devices table
  static std::vector<I3cTargetInfo> targetTable;
  std::cout << "Getting I3C target devices table: ";
  result_code = device->i3cGetDeviceTable(targetTable);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to get the I3C target devices table! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  printTargetDeviceTable(targetTable);
  
  // Init I3C bus
  std::cout << "Initializing I3C bus: ";
  result_code = device->i3cInitBus();
  if (result_code != SUCCESS) {
    std::cerr << "Failed to init the I3C bus! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;

  // Get I3C target devices table
  std::cout << "Getting I3C target devices table: ";
  result_code = device->i3cGetDeviceTable(targetTable);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to get the I3C target devices table! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  printTargetDeviceTable(targetTable);

  std::cout << "-------------------------------------------------------------------------" << std::endl;
  std::cout << "------ Reset the bus to initialize it with a static device table --------" << std::endl;
  std::cout << "--------- Retrieve target device table after the initialization ---------" << std::endl;
  std::cout << "-------------------------------------------------------------------------" << std::endl;

  // Reset I3C bus
  std::cout << "Resetting I3C bus: ";
  result_code = device->i3cResetBus();
  if (result_code != SUCCESS) {
    std::cerr << "Failed to reset the I3C bus! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;

  // Get I3C target devices table
  std::cout << "Getting I3C target devices table: ";
  result_code = device->i3cGetDeviceTable(targetTable);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to get the I3C target devices table! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  printTargetDeviceTable(targetTable);

  I3cBCR bcrI2c;
  bcrI2c.byte = 0x00;

  I3cTargetConfiguration confI2c;
  confI2c.bits.targetType = 1;
  confI2c.bits.IBIRequest = false;
  confI2c.bits.CRRequest = false;
  confI2c.bits.daaUseSETDASA = false;
  confI2c.bits.daaUseSETAASA = false;
  confI2c.bits.daaUseENTDAA = false;
  confI2c.bits.ibiTimestampEnable = false;
  confI2c.bits.pendingReadCapability = false;

  I3cBCR bcrBmi323;
  bcrBmi323.byte = 0x00;

  I3cTargetConfiguration confBmi323;
  confBmi323.bits.targetType = 0;
  confBmi323.bits.IBIRequest = true;
  confBmi323.bits.CRRequest = true;
  confBmi323.bits.daaUseSETDASA = false;
  confBmi323.bits.daaUseSETAASA = false;
  confBmi323.bits.daaUseENTDAA = true;
  confBmi323.bits.ibiTimestampEnable = false;
  confBmi323.bits.pendingReadCapability = false;

  I3cBCR bcrLsmd6v;
  bcrLsmd6v.byte = 0x07;

  I3cTargetConfiguration confLsmd6v;
  confLsmd6v.bits.targetType = 0;
  confLsmd6v.bits.IBIRequest = true;
  confLsmd6v.bits.CRRequest = false;
  confLsmd6v.bits.daaUseSETDASA = true;
  confLsmd6v.bits.daaUseSETAASA = false;
  confLsmd6v.bits.daaUseENTDAA = false;
  confLsmd6v.bits.ibiTimestampEnable = false;
  confLsmd6v.bits.pendingReadCapability = false;

  std::vector<I3cTargetInfo> targets = {
      I3cTargetInfo{
          0x50,                                       // staticAddress
          0x00,                                       // dynamicAddress
          {0x00, 0x00, 0x00, 0x00, 0x00, 0x00},       // pid
          bcrI2c,                                     // bcr
          0x00,                                       // dcr
          0x00,                                       // mwl
          0x00,                                       // mrl
          0x00,                                       // maxIbiPayloadLength
          confI2c                                     // configuration
      },

      I3cTargetInfo{
          0x6B,                                       // staticAddress
          0x0A,                                       // dynamicAddress
          {0x02, 0x08, 0x00, 0x70, 0x92, 0x0B},       // pid
          bcrLsmd6v,                                  // bcr
          0x44,                                       // dcr
          0x00,                                       // mwl
          0x00,                                       // mrl
          0x00,                                       // maxIbiPayloadLength
          confLsmd6v                                  // configuration
      },

      I3cTargetInfo{
          0x69,                                       // staticAddress
          0x0B,                                       // dynamicAddress
          {0x07, 0x70, 0x10, 0x43, 0x10, 0x00},       // pid
          bcrBmi323,                                  // bcr
          0xEF,                                       // dcr
          0x00,                                       // mwl
          0x00,                                       // mrl
          0x00,                                       // maxIbiPayloadLength
          confBmi323                                  // configuration
      },
  };

  // Init I3C bus
  std::cout << "Initializing I3C bus: ";
  result_code = device->i3cInitBus(targets);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to init the I3C bus! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;

  // Get I3C target devices table
  std::cout << "Getting I3C target devices table: ";
  result_code = device->i3cGetDeviceTable(targetTable);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to get the I3C target devices table! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  printTargetDeviceTable(targetTable);

  std::cout << "-------------------------------------------------------------------------" << std::endl;
  std::cout << "------------------- Modify the target device table ----------------------" << std::endl;
  std::cout << "---------- Retrieve target device table after the modification ----------" << std::endl;
  std::cout << "-------------------------------------------------------------------------" << std::endl;

  // Change target configuration
  std::cout << "Changing target device configuration: ";
  I3cTargetConfiguration configuration;
  configuration.bits.CRRequest = false;
  configuration.bits.daaUseENTDAA = false;
  configuration.bits.daaUseSETAASA = false;
  configuration.bits.daaUseSETDASA = true;
  configuration.bits.IBIRequest = true;
  configuration.bits.ibiTimestampEnable = true;
  configuration.bits.pendingReadCapability = false;
  configuration.bits.targetType = 0;
  result_code = device->i3cSetTargetDeviceConfiguration(0x08, configuration);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to set the I3C target device configuration! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;

  // Get I3C target devices table
  std::cout << "Getting I3C target devices table: ";
  result_code = device->i3cGetDeviceTable(targetTable);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to get the I3C target devices table! ERROR CODE:" << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  printTargetDeviceTable(targetTable);

  std::cout << "-------------------------------------------------------------------------" << std::endl;
  std::cout << "--------------------- SDR and Legacy I2C transfers ----------------------" << std::endl;
  std::cout << "-------------------------------------------------------------------------" << std::endl;

  // Write data
  std::cout << "Writing data: ";
  static std::vector<uint8_t> subaddress = {0x00, 0x05};
  static std::vector<uint8_t> data = {1, 2, 3, 4, 5, 6, 7, 9, 10, 11, 12};
  result_code = device->i3cLegacyI2cWrite(0x50, subaddress, data, I3cTransferFlags::I3C_XFER_NONE);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to write data! ERROR CODE: " << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;

  // Address device
  std::cout << "Addressing device: ";
  result_code = device->i3cLegacyI2cWrite(0x50, subaddress, {}, I3cTransferFlags::I3C_XFER_NONE);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to address device! ERROR CODE: " << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  
  // Read data
  std::cout << "Reading data: ";
  static std::vector<uint8_t> outData = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  result_code = device->i3cLegacyI2cRead(0x50, subaddress, 11, outData, I3cTransferFlags::I3C_XFER_NONE);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to read data! ERROR CODE: " << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  std::cout << "\tData: ";
  for (uint8_t i = 0; i < outData.size(); ++i) {
      std::cout << (int)outData[i] << " ";
  }
  std::cout << std::dec << std::endl;

  // Write data
  std::cout << "Writing data: ";
  subaddress = {0x05};
  data = {1, 2, 3, 4, 5, 6, 7, 9, 10, 11, 12};
  result_code = device->i3cSdrWrite(0x08, subaddress, data, I3cTransferFlags::I3C_XFER_NONE);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to write data! ERROR CODE: " << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  
  // Read data
  std::cout << "Reading data: ";
  result_code = device->i3cSdrRead(0x08, subaddress, 11, outData, I3cTransferFlags::I3C_XFER_NONE);
  if (result_code != SUCCESS) {
    std::cerr << "Failed to read data! ERROR CODE: " << result_code << std::endl;
    exit(1);
  }
  std::cout << "OK" << std::endl;
  std::cout << "\tData: ";
  for (uint8_t i = 0; i < outData.size(); ++i) {
      std::cout << (int)outData[i] << " ";
  }
  std::cout << std::dec << std::endl;

  // Disconnect the device
  device->disconnect();

  std::cout << "Finished example!" << std::endl;
}
