/*
 * 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"

// 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
};

enum DeviceAddress
{
  TRANSMITTER = 0x01,
  // MODULE = 0x03,
  MODULE = 0x05,
};

//Address used in transmitted frames - it constrains of target address and source address
const uint8_t FrameAddress = DeviceAddress::TRANSMITTER | (DeviceAddress::MODULE << 4);

static void _serial_reset(void* buffer)
{
  auto data = (SerialData*)buffer;
  data->ptr = data->pulses;
}

static void _serial_sendByte(void* buffer, uint8_t b)
{
  auto data = (SerialData*)buffer;
  *(data->ptr++) = b;
}

static void _serial_flush(void*) {}

uint32_t _serial_getSize(void* buffer)
{
  auto data = (SerialData*)buffer;
  return data->ptr - data->pulses;
}

static void _pulses_reset(void* buffer)
{
  auto data = (PulsesData*)buffer;
  data->ptr = data->pulses;
}

static inline void _pulses_send_level(PulsesData* data, uint16_t v)
{
  uint32_t size = data->ptr - data->pulses;
  if (size >= AFHDS_MAX_PULSES_TRANSITIONS) return;
  *(data->ptr)++ = v;
}

static void _pulses_sendByte(void* buffer, uint8_t b)
{
  auto data = (PulsesData*)buffer;
  uint32_t size = data->ptr - data->pulses;
  if (size >= AFHDS_MAX_PULSES_TRANSITIONS) return;

  // use 8n1
  // parity: If the parity is enabled, then the MSB bit of the data to be
  // transmitted is changed by the parity bit start is always 0
  bool level = 0;
  uint16_t length = BITLEN_AFHDS;     // start bit
  for (uint8_t i = 0; i <= 8; i++) {  // 8 data bits + Stop=1
    bool next_level = b & 1;
    if (level == next_level) {
      length += BITLEN_AFHDS;
    } else {
      _pulses_send_level(data, length);
      length = BITLEN_AFHDS;
      level = next_level;
    }
    b = (b >> 1) | 0x80;  // shift left to get next bit, fill msb with stop
                          // bit - needed just once
  }
  _pulses_send_level(data, length);  // last bit (stop)
}

static void _pulses_flush(void* buffer)
{
  // TODO: use less, as the new driver can handle it
  auto data = (PulsesData*)buffer;
  *data->ptr = 60000;
}

uint32_t _pulses_getSize(void* buffer)
{
  auto data = (PulsesData*)buffer;
  return data->ptr - data->pulses;
}
  
void ByteTransport::init(Type t)
{
  if (t == Serial) {
    reset = _serial_reset;
    sendByte = _serial_sendByte;
    flush = _serial_flush;
    getSize = _serial_getSize;
  } else if (t == Pulses) {
    reset = _pulses_reset;
    sendByte = _pulses_sendByte;
    flush = _pulses_flush;
    getSize = _pulses_getSize;
  } else {
    reset = nullptr;
    sendByte = nullptr;
    flush = nullptr;
    getSize = nullptr;
  }
}

void FrameTransport::init(ByteTransport::Type t, void* buffer)
{
  trsp.init(t);
  trsp_buffer = buffer;

  clear();
}

void FrameTransport::clear()
{
  // reset send buffer
  trsp.reset(trsp_buffer);

  // reset parser
  esc_state = 0;
}

void FrameTransport::sendByte(uint8_t b)
{
  trsp.sendByte(trsp_buffer, 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) {
      sendByte(ESC);
      sendByte(ESC_END);
    }
    else if (ESC == byte) {
      sendByte(ESC);
      sendByte(ESC_ESC);
    }
    else {
      sendByte(byte);
    }
  }  
}

void FrameTransport::putFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data,
                              uint8_t dataLength, uint8_t frameIndex)
{
  //header
  trsp.reset(trsp_buffer);
  crc = 0;
  sendByte(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);
  sendByte(END);
  trsp.flush(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(ByteTransport::Type t, void* buffer)
{
  trsp.init(t, buffer);
}

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

  frameIndex = 1;
  repeatCount = 0;

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

void Transport::sendFrame(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);
}

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);

    // TODO: send ACK immediately !!!
    // trsp.putFrame(command, FRAME_TYPE::RESPONSE_ACK, nullptr, 0,
    //               responseFrame->frameNumber);
    // return true;

    fifo.enqueueACK((enum COMMAND)responseFrame->command,
                    responseFrame->frameNumber);

  } 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;
}

};