#include "ArcsecDatalinkLayer.h"

ArcsecDatalinkLayer::ArcsecDatalinkLayer() { slipInit(); }

ArcsecDatalinkLayer::~ArcsecDatalinkLayer() {}

void ArcsecDatalinkLayer::slipInit() {
  slipInfo.buffer = rxBuffer;
  slipInfo.maxlength = startracker::MAX_FRAME_SIZE;
  slipInfo.length = 0;
  slipInfo.unescape_next = 0;
  slipInfo.prev_state = SLIP_COMPLETE;
}

ReturnValue_t ArcsecDatalinkLayer::decodeFrame(const uint8_t* rawData, size_t rawDataSize,
                                               size_t* bytesLeft) {
  size_t bytePos = 0;
  for (bytePos = 0; bytePos < rawDataSize; bytePos++) {
    enum arc_dec_result decResult =
        arc_transport_decode_body(*(rawData + bytePos), &slipInfo, decodedFrame, &decFrameSize);
    *bytesLeft = rawDataSize - bytePos - 1;
    switch (decResult) {
      case ARC_DEC_INPROGRESS: {
        if (bytePos == rawDataSize - 1) {
          return DEC_IN_PROGRESS;
        }
        continue;
      }
      case ARC_DEC_ERROR_FRAME_SHORT:
        return REPLY_TOO_SHORT;
      case ARC_DEC_ERROR_CHECKSUM:
        return CRC_FAILURE;
      case ARC_DEC_ASYNC:
      case ARC_DEC_SYNC: {
        // Reset length of SLIP struct for next frame
        slipInfo.length = 0;
        return returnvalue::OK;
      }
      default:
        sif::debug << "ArcsecDatalinkLayer::decodeFrame: Unknown result code" << std::endl;
        break;
        return returnvalue::FAILED;
    }
  }
  return returnvalue::FAILED;
}

uint8_t ArcsecDatalinkLayer::getReplyFrameType() { return decodedFrame[0]; }

const uint8_t* ArcsecDatalinkLayer::getReply() { return &decodedFrame[1]; }

void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, uint32_t length) {
  arc_transport_encode_body(data, length, encBuffer, &encFrameSize);
}

uint8_t* ArcsecDatalinkLayer::getEncodedFrame() { return encBuffer; }

uint32_t ArcsecDatalinkLayer::getEncodedLength() { return encFrameSize; }

uint8_t ArcsecDatalinkLayer::getStatusField() { return *(decodedFrame + STATUS_OFFSET); }

uint8_t ArcsecDatalinkLayer::getId() { return *(decodedFrame + ID_OFFSET); }