#include "ArcsecDatalinkLayer.h"

extern "C" {
#include <wire/common/SLIP.h>
#include <wire/common/misc.h>
}

ArcsecDatalinkLayer::ArcsecDatalinkLayer() : decodeRingBuf(BUFFER_LENGTHS, true) {}

ArcsecDatalinkLayer::~ArcsecDatalinkLayer() {}

ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedFrame,
                                                        size_t& frameLen) {
  size_t currentLen = decodeRingBuf.getAvailableReadData();
  if (currentLen == 0) {
    return DEC_IN_PROGRESS;
  }
  decodeRingBuf.readData(rxAnalysisBuffer, currentLen);

  bool startFound = false;
  size_t startIdx = 0;
  for (size_t idx = 0; idx < currentLen; idx++) {
    if (rxAnalysisBuffer[idx] != SLIP_START_AND_END) {
      continue;
    }
    if (not startFound) {
      startFound = true;
      startIdx = idx;
      continue;
    }
    // Now we can try decoding the whole frame.
    size_t encodedDataSize = 0;
    slip_error_t slipError =
        slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx,
                          idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID, 0);
    decodeRingBuf.deleteData(idx + 1);
    switch (slipError) {
      case (SLIP_OK): {
        if (decodedFrame != nullptr) {
          *decodedFrame = decodedRxFrame;
        }
        frameLen = rxFrameSize;
        return returnvalue::OK;
      }
      case (SLIP_BAD_CRC): {
        return CRC_FAILURE;
      }
      case (SLIP_OVERFLOW): {
        return SLIP_OVERFLOW_RETVAL;
      }
      // Should not happen, we searched for start and end marker..
      case (SLIP_NO_END): {
        return returnvalue::FAILED;
      }
      case (SLIP_ID_MISMATCH): {
        return SLIP_ID_MISSMATCH_RETVAL;
      }
      default: {
        return returnvalue::FAILED;
      }
    }
  }
  return DEC_IN_PROGRESS;
}

ReturnValue_t ArcsecDatalinkLayer::feedData(const uint8_t* rawData, size_t rawDataLen) {
  if (rawDataLen > 4096) {
    sif::error << "ArcsecDatalinklayer: Can not write more than 4096 bytes to ring buffer"
               << std::endl;
    return returnvalue::FAILED;
  }
  return decodeRingBuf.writeData(rawData, rawDataLen);
}

void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); }

void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame,
                                      size_t& size) {
  slip_encode_frame(data, length, txEncoded,sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID);
  if (txFrame != nullptr) {
    *txFrame = txEncoded;
  }
}