#include "UartTestClass.h"

#include <errno.h>  // Error integer and strerror() function
#include <fcntl.h>  // Contains file controls like O_RDWR
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw_hal/linux/uart/UartCookie.h>
#include <linux/devices/ScexDleParser.h>
#include <linux/devices/ScexHelper.h>
#include <linux/devices/ScexUartReader.h>
#include <unistd.h>  // write(), read(), close()

#include "OBSWConfig.h"
#include "fsfw/globalfunctions/CRC.h"
#include "fsfw/globalfunctions/DleEncoder.h"
#include "fsfw/globalfunctions/arrayprinter.h"
#include "fsfw/serviceinterface.h"

#define GPS_REPLY_WIRETAPPING 0

#ifndef RPI_TEST_GPS_HANDLER
#define RPI_TEST_GPS_HANDLER 0
#endif

UartTestClass::UartTestClass(object_id_t objectId, ScexUartReader* reader)
    : TestTask(objectId), reader(reader) {
  mode = TestModes::SCEX;
  scexMode = ScexModes::READER_TASK;
  currCmd = scex::ScexCmds::FRAM;
  if (scexMode == ScexModes::SIMPLE) {
    auto encodingBuf = new std::array<uint8_t, 4096>;
    DleParser::BufPair encodingBufPair{encodingBuf->data(), encodingBuf->size()};
    auto decodedBuf = new std::array<uint8_t, 4096>;
    DleParser::BufPair decodingBufPair{decodedBuf->data(), decodedBuf->size()};
    dleParser = new ScexDleParser(*(new SimpleRingBuffer(4096, true)), dleEncoder, encodingBufPair,
                                  decodingBufPair, &foundDlePacketHandler, this);
  }
}

ReturnValue_t UartTestClass::initialize() {
  if (mode == TestModes::GPS) {
    gpsInit();
  } else if (mode == TestModes::SCEX) {
    scexInit();
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t UartTestClass::performOneShotAction() { return HasReturnvaluesIF::RETURN_OK; }

ReturnValue_t UartTestClass::performPeriodicAction() {
  if (mode == TestModes::GPS) {
    gpsPeriodic();
  } else if (mode == TestModes::SCEX) {
    scexPeriodic();
  }
  return HasReturnvaluesIF::RETURN_OK;
}

void UartTestClass::gpsInit() {
#if RPI_TEST_GPS_HANDLER == 1
  int result = lwgps_init(&gpsData);
  if (result == 0) {
    sif::warning << "lwgps_init error: " << result << std::endl;
  }

  /* Get file descriptor */
  serialPort = open("/dev/serial0", O_RDWR);
  if (serialPort < 0) {
    sif::warning << "open call failed with error [" << errno << ", " << strerror(errno)
                 << std::endl;
  }
  /* Setting up UART parameters */
  tty.c_cflag &= ~PARENB;         // Clear parity bit
  tty.c_cflag &= ~CSTOPB;         // Clear stop field, only one stop bit used in communication
  tty.c_cflag &= ~CSIZE;          // Clear all the size bits
  tty.c_cflag |= CS8;             // 8 bits per byte
  tty.c_cflag &= ~CRTSCTS;        // Disable RTS/CTS hardware flow control
  tty.c_cflag |= CREAD | CLOCAL;  // Turn on READ & ignore ctrl lines (CLOCAL = 1)
  // Use canonical mode for GPS device
  tty.c_lflag |= ICANON;
  tty.c_lflag &= ~ECHO;                    // Disable echo
  tty.c_lflag &= ~ECHOE;                   // Disable erasure
  tty.c_lflag &= ~ECHONL;                  // Disable new-line echo
  tty.c_lflag &= ~ISIG;                    // Disable interpretation of INTR, QUIT and SUSP
  tty.c_iflag &= ~(IXON | IXOFF | IXANY);  // Turn off s/w flow ctrl
  tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR |
                   ICRNL);  // Disable any special handling of received bytes
  tty.c_oflag &= ~OPOST;    // Prevent special interpretation of output bytes (e.g. newline chars)
  tty.c_oflag &= ~ONLCR;    // Prevent conversion of newline to carriage return/line feed

  // Non-blocking mode
  tty.c_cc[VTIME] = 0;
  tty.c_cc[VMIN] = 0;

  cfsetispeed(&tty, B9600);
  cfsetospeed(&tty, B9600);
  if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
    sif::warning << "tcsetattr call failed with error [" << errno << ", " << strerror(errno)
                 << std::endl;
    ;
  }
  // Flush received and unread data. Those are old NMEA strings which are not relevant anymore
  tcflush(serialPort, TCIFLUSH);
#endif
}

void UartTestClass::gpsPeriodic() {
#if RPI_TEST_GPS_HANDLER == 1
  int bytesRead = 0;
  do {
    bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
                     static_cast<unsigned int>(recBuf.size()));
    if (bytesRead < 0) {
      sif::warning << "UartTestClass::performPeriodicAction: read call failed with error [" << errno
                   << ", " << strerror(errno) << "]" << std::endl;
      break;
    } else if (bytesRead >= static_cast<int>(recBuf.size())) {
      sif::debug << "UartTestClass::performPeriodicAction: "
                    "recv buffer might not be large enough"
                 << std::endl;
    } else if (bytesRead > 0) {
      // pass data to lwgps for processing
#if GPS_REPLY_WIRETAPPING == 1
      sif::info << recBuf.data() << std::endl;
#endif
      int result = lwgps_process(&gpsData, recBuf.data(), bytesRead);
      if (result == 0) {
        sif::warning << "UartTestClass::performPeriodicAction: lwgps_process error" << std::endl;
      }
      recvCnt++;
      if (recvCnt == 6) {
        recvCnt = 0;
        sif::info << "GPS Data" << std::endl;
        // Print messages
        printf("Valid status: %d\n", gpsData.is_valid);
        printf("Latitude: %f degrees\n", gpsData.latitude);
        printf("Longitude: %f degrees\n", gpsData.longitude);
        printf("Altitude: %f meters\n", gpsData.altitude);
      }
    }
  } while (bytesRead > 0);
#endif
}

void UartTestClass::scexInit() {
  if (reader == nullptr) {
    sif::warning << "UartTestClass::scexInit: Reader invalid" << std::endl;
    return;
  }
  if (scexMode == ScexModes::SIMPLE) {
    scexSimpleInit();
  } else {
#if defined(RASPBERRY_PI)
    std::string devname = "/dev/serial0";
#else
    std::string devname = "/dev/ul-scex";
#endif
    uartCookie = new UartCookie(this->getObjectId(), devname, UartBaudRate::RATE_57600, 4096);
    reader->setDebugMode(false);
    ReturnValue_t result = reader->initializeInterface(uartCookie);
    if (result != HasReturnvaluesIF::RETURN_OK) {
      sif::warning << "UartTestClass::gpsPeriodic: Initializing SCEX reader "
                      "UART IF failed"
                   << std::endl;
    }
  }
}

void UartTestClass::scexPeriodic() {
  using namespace std;
  using namespace scex;
  if (reader == nullptr) {
    return;
  }

  if (scexMode == ScexModes::SIMPLE) {
    scexSimplePeriodic();
  } else {
    if (not cmdSent) {
      size_t len = 0;
      prepareScexCmd(currCmd, false, cmdBuf.data(), &len);
      reader->sendMessage(uartCookie, cmdBuf.data(), len);
      cmdSent = true;
      cmdDone = false;
    }
    if (cmdSent and not cmdDone) {
      uint8_t* decodedPacket = nullptr;
      size_t len = 0;
      do {
        ReturnValue_t result = reader->readReceivedMessage(uartCookie, &decodedPacket, &len);
        if(len == 0){
        	break;
        }
        ScexHelper helper;
        const uint8_t* helperPtr = decodedPacket;
        result = helper.deSerialize(&helperPtr, &len);
        if (result == ScexHelper::INVALID_CRC) {
          sif::warning << "CRC invalid" << std::endl;
        }
        sif::info << helper << endl;

        // ping
        // if ping cmd
        if (helper.getCmd() == PING) {
          ofstream out("/tmp/scex-ping.bin", ofstream::binary);
          if (out.bad()) {
            sif::warning << "bad" << std::endl;
          }
          out << helper;
        }
        // fram
        // packetcounter eins höher, wenn mehr packet verloren -> merkt sich welches packet fehlt
        //was wenn erstes packet fehlt; mit boolean var (firstpacketarrived=false) die immer mit finish false wird?
        // countdown (max 2min), wenn nicht  if (helper.getPacketCounter() ==
        // helper.getTotalPacketCounter()) { nach 2min  reader->finish();
        if (helper.getCmd() == FRAM) {
          if (helper.getPacketCounter() == 1) {
        	  //countdown starten
        	  finishCountdown.resetTimer();
            ofstream out("/tmp/scex-fram.bin",
                         ofstream::binary);  // neues file anlegen wie oben ping
          } else {
            ofstream out("/tmp/scex-fram.bin",
                         ofstream::binary | ofstream::app);  // an bestehendes file hinzufügen
            out << helper;
          }

          if(finishCountdown.hasTimedOut()){
        	  reader->finish();
          }
        }

        if (helper.getPacketCounter() == helper.getTotalPacketCounter()) {
          reader->finish();
          sif::info << "Reader is finished" << endl;
          cmdDone = true;
          if (helper.getCmd() == scex::ScexCmds::PING) {
            cmdSent = false;
          }
        }
      } while (len > 0);
    }
  }
}

void UartTestClass::scexSimpleInit() {
#if defined(RASPBERRY_PI)
  std::string devname = "/dev/serial0";
#else
  std::string devname = "/dev/ul-scex";
#endif
  /* Get file descriptor */
  serialPort = open(devname.c_str(), O_RDWR);
  if (serialPort < 0) {
    sif::warning << "open call failed with error [" << errno << ", " << strerror(errno)
                 << std::endl;
    return;
  }
  // Setting up UART parameters
  tty.c_cflag &= ~PARENB;         // Clear parity bit
  tty.c_cflag &= ~CSTOPB;         // Clear stop field, only one stop bit used in communication
  tty.c_cflag &= ~CSIZE;          // Clear all the size bits
  tty.c_cflag |= CS8;             // 8 bits per byte
  tty.c_cflag &= ~CRTSCTS;        // Disable RTS/CTS hardware flow control
  tty.c_cflag |= CREAD | CLOCAL;  // Turn on READ & ignore ctrl lines (CLOCAL = 1)

  // Use non-canonical mode and clear echo flag
  tty.c_lflag &= ~(ICANON | ECHO);

  // Non-blocking mode, read until either line is 0.1 second idle or maximum of 255 bytes are
  // received in one go
  tty.c_cc[VTIME] = 0;  // In units of 0.1 seconds
  tty.c_cc[VMIN] = 0;   // Read up to 255 bytes

  // Q7S UART Lite has fixed baud rate. For other linux systems, set baud rate here.
#if !defined(XIPHOS_Q7S)
  if (cfsetispeed(&tty, B57600) != 0) {
    sif::warning << "UartTestClass::scexInit: Setting baud rate failed" << std::endl;
  }
#endif

  if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
    sif::warning << "tcsetattr call failed with error [" << errno << ", " << strerror(errno)
                 << std::endl;
  }
  // Flush received and unread data
  tcflush(serialPort, TCIOFLUSH);
}

void UartTestClass::scexSimplePeriodic() {
  using namespace scex;
  ReturnValue_t result = RETURN_OK;
  if (not cmdSent) {
    // Flush received and unread data
    tcflush(serialPort, TCIFLUSH);
    uint8_t tmpCmdBuf[32] = {};
    size_t len = 0;
    sif::info << "UartTestClass::scexSimplePeriodic: Sending command to SCEX" << std::endl;
    prepareScexCmd(currCmd, false, tmpCmdBuf, &len);
    result = dleEncoder.encode(tmpCmdBuf, len, cmdBuf.data(), cmdBuf.size(), &encodedLen, true);
    if (result != HasReturnvaluesIF::RETURN_OK) {
      sif::warning << "UartTestClass::scexInit: Encoding failed" << std::endl;
      return;
    }
    if (result != 0) {
      return;
    };
    size_t bytesWritten = write(serialPort, cmdBuf.data(), encodedLen);
    if (bytesWritten != encodedLen) {
      sif::warning << "Sending command to solar experiment failed" << std::endl;
    }
    cmdSent = true;
    cmdDone = false;
  }
  if (not cmdDone) {
    // Read back reply immediately
    int bytesRead = 0;
    do {
      bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
                       static_cast<unsigned int>(recBuf.size()));
      if (bytesRead == 0) {
        sif::warning << "Reading SCEX: Timeout or no bytes read" << std::endl;
      } else if (bytesRead < 0) {
        sif::warning << "UartTestClass::performPeriodicAction: read call failed with error ["
                     << errno << ", " << strerror(errno) << "]" << std::endl;
        break;
      } else if (bytesRead >= static_cast<int>(recBuf.size())) {
        sif::debug << "UartTestClass::performPeriodicAction: recv buffer might not be large "
                      "enough, bytes read:"
                   << bytesRead << std::endl;
      } else if (bytesRead > 0) {
        dleParser->passData(recBuf.data(), bytesRead);
        if (currCmd == ScexCmds::PING) {
          cmdDone = true;
          cmdSent = false;
        }
      }
    } while (bytesRead > 0);
  }
}

int UartTestClass::prepareScexCmd(scex::ScexCmds cmd, bool tempCheck, uint8_t* cmdBuf,
                                  size_t* len) {
  using namespace scex;
  // Send command
  cmdBuf[0] = scex::createCmdByte(cmd, false);
  // These two fields are the packet counter and the total packet count. Those are 1 and 1 for each
  // telecommand so far
  cmdBuf[1] = 1;
  cmdBuf[2] = 1;
  uint16_t userDataLen = 0;
  cmdBuf[3] = (userDataLen >> 8) & 0xff;
  cmdBuf[4] = userDataLen & 0xff;
  uint16_t crc = CRC::crc16ccitt(cmdBuf, 5);
  cmdBuf[5] = (crc >> 8) & 0xff;
  cmdBuf[6] = crc & 0xff;
  *len = 7;
  return 0;
}

void UartTestClass::foundDlePacketHandler(const DleParser::Context& ctx) {
  UartTestClass* obj = reinterpret_cast<UartTestClass*>(ctx.userArgs);
  if (ctx.getType() == DleParser::ContextType::PACKET_FOUND) {
    obj->handleFoundDlePacket(ctx.decodedPacket.first, ctx.decodedPacket.second);
  } else {
    DleParser::defaultErrorHandler(ctx.error.first, ctx.error.second);
  }
}

void UartTestClass::handleFoundDlePacket(uint8_t* packet, size_t len) {
  sif::info << "Detected DLE encoded packet with decoded size " << len << std::endl;
}