/*
 * Copyright (C) EdgeTX
 *
 * Based on code named
 *   opentx - https://github.com/opentx/opentx
 *   th9x - http://code.google.com/p/th9x
 *   er9x - http://code.google.com/p/er9x
 *   gruvin9x - http://code.google.com/p/gruvin9x
 *
 * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include "afhds3_transport.h"
#include "opentx_helpers.h"
#include "debug.h"

#include "board.h"
#include "dataconstants.h"
#include "mixer_scheduler.h"

// timer is 2 MHz
#if defined(AFHDS3_SLOW)
  // 1000000/57600 = 17,36 us
  #define BITLEN_AFHDS (35)
#else
  // 1000000/115200 = 8,68 us
  #define BITLEN_AFHDS (17)
#endif

#define MAX_RETRIES_AFHDS3 5

namespace afhds3
{

enum AfhdsSpecialChars {
  END = 0xC0,  // Frame end
  START = END,
  ESC_END = 0xDC,  // Escaped frame end - in case END occurs in fame then ESC
                   // ESC_END must be used
  ESC = 0xDB,      // Escaping character
  ESC_ESC = 0xDD,  // Escaping character in case ESC occurs in fame then ESC
                   // ESC_ESC  must be used
};

void FrameTransport::init(void* buffer, uint8_t fAddr)
{
  frameAddress = fAddr;
  trsp_buffer = (uint8_t*)buffer;
  clear();
}

void FrameTransport::clear()
{
  // reset send buffer
  data_ptr = trsp_buffer;

  // reset parser
  esc_state = 0;
}

void FrameTransport::putByte(uint8_t b)
{
  *(data_ptr++) = b;
}

void FrameTransport::putBytes(uint8_t* data, int length)
{
  for (int i = 0; i < length; i++) {
    uint8_t byte = data[i];
    crc += byte;
    if (END == byte) {
      putByte(ESC);
      putByte(ESC_END);
    }
    else if (ESC == byte) {
      putByte(ESC);
      putByte(ESC_ESC);
    }
    else {
      putByte(byte);
    }
  }  
}

void FrameTransport::putFrame(COMMAND command, FRAME_TYPE frameType,
                              uint8_t* data, uint8_t dataLength,
                              uint8_t frameIndex)
{
  // header
  data_ptr = trsp_buffer;

  crc = 0;
  putByte(START);

  uint8_t buffer[] = {frameAddress, frameIndex, frameType, command};
  putBytes(buffer, 4);

  // payload
  if (dataLength > 0) {
    putBytes(data, dataLength);
  }

  // footer
  uint8_t crcValue = crc ^ 0xff;
  putBytes(&crcValue, 1);
  putByte(END);
}

uint32_t FrameTransport::getFrameSize()
{
  return data_ptr - trsp_buffer;
}

static bool _checkCRC(const uint8_t* data, uint8_t size)
{
  uint8_t crc = 0;
  //skip start byte
  for (uint8_t i = 1; i < size; i++) {
    crc += data[i];
  }
  return (crc ^ 0xff) == data[size];
}

bool FrameTransport::processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
                                          uint8_t& rxBufferCount,
                                          uint8_t maxSize)
{
  if (rxBufferCount == 0 && byte != START) {
//     TRACE("AFHDS3 [SKIP] %02X", byte);
    this->esc_state = 0;
    return false;
  }

  if (byte == ESC) {
    this->esc_state = rxBufferCount;
    return false;
  }

  if (rxBufferCount > 1 && byte == END) {
    rxBuffer[rxBufferCount++] = byte;

    if (!_checkCRC(rxBuffer, rxBufferCount - 2)) {
      TRACE("AFHDS3 [INVALID CRC]");
      rxBufferCount = 0;
      return false;
    }

    return true;
  }

  if (this->esc_state && byte == ESC_END) {
    byte = END;
  }
  else if (esc_state && byte == ESC_ESC) {
    byte = ESC;
  }
  //reset esc index
  this->esc_state = 0;

  if (rxBufferCount >= maxSize) {
    TRACE("AFHDS3 [BUFFER OVERFLOW]");
    rxBufferCount = 0;
  }
  rxBuffer[rxBufferCount++] = byte;
  return false;
}

void CommandFifo::clearCommandFifo()
{
  memclear(commandFifo, sizeof(commandFifo));
  setIndex = getIndex = 0;
}

Frame* CommandFifo::getCommand()
{
  if (isEmpty()) return nullptr;
  return &commandFifo[getIndex];
}

void CommandFifo::enqueueACK(COMMAND command, uint8_t frameNumber)
{
  uint32_t next = nextIndex(setIndex);
  if (next != getIndex) {
    commandFifo[setIndex].command = command;
    commandFifo[setIndex].frameType = FRAME_TYPE::RESPONSE_ACK;
    commandFifo[setIndex].payload = 0;
    commandFifo[setIndex].payloadSize = 0;
    commandFifo[setIndex].frameNumber = frameNumber;
    commandFifo[setIndex].useFrameNumber = true;
    setIndex = next;
  }
}

void CommandFifo::enqueue(COMMAND command, FRAME_TYPE frameType, bool useData,
                          uint8_t byteContent)
{
  uint32_t next = nextIndex(setIndex);
  if (next != getIndex) {
    commandFifo[setIndex].command = command;
    commandFifo[setIndex].frameType = frameType;
    commandFifo[setIndex].payload = byteContent;
    commandFifo[setIndex].payloadSize = useData ? 1 : 0;
    commandFifo[setIndex].frameNumber = 0;
    commandFifo[setIndex].useFrameNumber = false;
    setIndex = next;
  }
}

void Transport::init(void* buffer, etx_module_state_t* mod_st, uint8_t fAddr)
{
  trsp.init(buffer, fAddr);
  this->mod_st = mod_st;
}

void Transport::clear()
{
  // reset frame
  trsp.clear();
  
  // reset command layer
  fifo.clearCommandFifo();

  frameIndex = 1;
  repeatCount = 0;

  // reset internal state
  operationState = State::UNKNOWN;
}

void Transport::putFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data,
                          uint8_t dataLength)
{
  operationState = State::SENDING_COMMAND;
  repeatCount = 0;

  trsp.putFrame(command, frameType, data, dataLength, frameIndex);
  frameIndex++;

  switch (frameType) {
    case FRAME_TYPE::REQUEST_GET_DATA:
    case FRAME_TYPE::REQUEST_SET_EXPECT_ACK:
    case FRAME_TYPE::REQUEST_SET_EXPECT_DATA:
      operationState = State::AWAITING_RESPONSE;
      break;
    default:
      operationState = State::IDLE;
  }
}

void Transport::enqueue(COMMAND command, FRAME_TYPE frameType, bool useData,
                        uint8_t byteContent)
{
  fifo.enqueue(command, frameType, useData, byteContent);
}

void Transport::sendBuffer()
{
#if !defined(SIMU)
  auto drv = modulePortGetSerialDrv(mod_st->tx);
  auto ctx = modulePortGetCtx(mod_st->tx);
  drv->sendBuffer(ctx, (uint8_t*)trsp.trsp_buffer, trsp.getFrameSize());
#endif
}

bool Transport::processQueue()
{
  // check waiting commands
  auto f = fifo.getCommand();
  if (!f) return false;

  trsp.putFrame(f->command, f->frameType, &f->payload, f->payloadSize,
                f->useFrameNumber ? f->frameNumber : frameIndex);

//   TRACE(
//       "AFHDS3 [CMD QUEUE] cmd: 0x%02x frameType 0x%02x, useFrameNumber %d "
//       "frame Number %d size %d",
//       f->command, f->frameType, f->useFrameNumber, f->frameNumber,
//       f->payloadSize);

  if (!f->useFrameNumber) frameIndex++;
  fifo.skip();

  return true;
}

bool Transport::handleRetransmissions(bool& error)
{
  if (operationState == State::AWAITING_RESPONSE) {
    if (repeatCount++ < MAX_RETRIES_AFHDS3) {
      error = false;
      return true; // re-send
    }

//     TRACE("AFHDS3 [NO RESP]");
    error = true;
    return false;
  }

  if (operationState == State::UNKNOWN) {
    error = true;
    return false;
  }

  error = false;
  repeatCount = 0;

  return false;
}

bool Transport::handleReply(uint8_t* buffer, uint8_t len)
{
  // TODO: check len...

  AfhdsFrame* responseFrame = reinterpret_cast<AfhdsFrame*>(buffer);
  if (responseFrame->frameType == FRAME_TYPE::REQUEST_SET_EXPECT_ACK) {

    // check if such request is not queued
    auto f = fifo.getCommand();
    if (f && f->frameType == FRAME_TYPE::RESPONSE_ACK &&
        f->frameNumber == responseFrame->frameNumber) {

      // absorb retransmission
      TRACE("ACK for frame %02X already queued", responseFrame->frameNumber);
      return true;
    }

//     TRACE("AFHDS3 [SEND ACK] cmd %02X type %02X number %02X",
//           responseFrame->command, responseFrame->frameType,
//           responseFrame->frameNumber);

    auto command = (enum COMMAND)responseFrame->command;
    trsp.putFrame(command, FRAME_TYPE::RESPONSE_ACK, nullptr, 0,
                  responseFrame->frameNumber);
    sendBuffer();

  } else if (responseFrame->frameType == FRAME_TYPE::RESPONSE_DATA ||
             responseFrame->frameType == FRAME_TYPE::RESPONSE_ACK) {

    if (operationState == State::AWAITING_RESPONSE) {
      operationState = State::IDLE;
    }
  }

  return false;
}

bool Transport::processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
                                     uint8_t& rxBufferCount, uint8_t maxSize)
{
  bool has_frame =
      trsp.processTelemetryData(byte, rxBuffer, rxBufferCount, maxSize);
  if (has_frame && handleReply(rxBuffer, rxBufferCount)) {
    rxBufferCount = 0;
    return false;
  }

  return has_frame;
}
};  // namespace afhds3