#include "fsfw/globalfunctions/arrayprinter.h"

#include <bitset>
#include <cmath>

#include "fsfw/serviceinterface/ServiceInterface.h"

void arrayprinter::print(const uint8_t *data, size_t size, OutputType type, bool printInfo,
                         size_t maxCharPerLine) {
  if (size == 0) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::info << "Size is zero, nothing to print" << std::endl;
#else
    sif::printInfo("Size is zero, nothing to print\n");
#endif
    return;
  }

#if FSFW_CPP_OSTREAM_ENABLED == 1
  if (printInfo) {
    sif::info << "Printing data with size " << size << ": " << std::endl;
  }
#else
#if FSFW_NO_C99_IO == 1
  sif::printInfo("Printing data with size %lu: \n", static_cast<unsigned long>(size));
#else
  sif::printInfo("Printing data with size %zu: \n", size);
#endif /* FSFW_NO_C99_IO == 1 */
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */

  if (type == OutputType::HEX) {
    arrayprinter::printHex(data, size, maxCharPerLine);
  } else if (type == OutputType::DEC) {
    arrayprinter::printDec(data, size, maxCharPerLine);
  } else if (type == OutputType::BIN) {
    arrayprinter::printBin(data, size);
  }
}

void arrayprinter::printHex(const uint8_t *data, size_t datasize, size_t maxCharPerLine) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
  if (sif::info.crAdditionEnabled()) {
    std::cout << "\r" << std::endl;
  }

  std::cout << "hex [" << std::setfill('0') << std::hex;
  for (size_t i = 0; i < datasize; i++) {
    std::cout << std::setw(2) << static_cast<int>(data[i]);
    if (i < datasize - 1) {
      std::cout << ",";
      if (i > 0 and (i + 1) % maxCharPerLine == 0) {
        std::cout << std::endl;
      }
    }
  }
  std::cout << std::dec << std::setfill(' ');
  std::cout << "]" << std::endl;
#else
  printf("hex [");
  for (size_t i = 0; i < datasize; i++) {
    printf("0x%02x ", data[i]);
  }
  printf("]\n");
#endif
}

void arrayprinter::printDec(const uint8_t *data, size_t size, size_t maxCharPerLine) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
  if (sif::info.crAdditionEnabled()) {
    std::cout << "\r" << std::endl;
  }

  std::cout << "dec [" << std::dec;
  for (size_t i = 0; i < size; i++) {
    std::cout << static_cast<int>(data[i]);
    if (i < size - 1) {
      std::cout << ",";
      if (i > 0 and (i + 1) % maxCharPerLine == 0) {
        std::cout << std::endl;
      }
    }
  }
  std::cout << "]" << std::endl;
#else
  // General format: 32,243,-12 so it is number of chars times 4
  // plus line break plus small safety margin.

  for (size_t i = 0; i < size; i++) {
    // TODO
  }
#endif
}

void arrayprinter::printBin(const uint8_t *data, size_t size) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
  for (size_t i = 0; i < size; i++) {
    sif::info << "Byte " << i + 1 << ": 0b" << std::bitset<8>(data[i]) << std::endl;
  }
#else
  for (size_t i = 0; i < size; i++) {
    sif::printInfo("Byte %d: 0b" BYTE_TO_BINARY_PATTERN "\n", i + 1, BYTE_TO_BINARY(data[i]));
  }
#endif
}