#include "ArcsecDatalinkLayer.h"

ArcsecDatalinkLayer::ArcsecDatalinkLayer() : decodeRingBuf(4096, true) { slipInit(); }

ArcsecDatalinkLayer::~ArcsecDatalinkLayer() {}

ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedFrame,
                                                        size_t& frameLen) {
  size_t currentLen = decodeRingBuf.getAvailableReadData();
  decodeRingBuf.readData(decodedRxFrame, currentLen);
  for (size_t idx = 0; idx < currentLen; idx++) {
    enum arc_dec_result decResult =
        arc_transport_decode_body(decodedRxFrame[idx], &slipInfo, decodedRxFrame, &rxFrameSize);
    switch (decResult) {
      case ARC_DEC_INPROGRESS: {
        break;
      }
      case ARC_DEC_ERROR_FRAME_SHORT: {
        decodeRingBuf.deleteData(idx);
        return REPLY_TOO_SHORT;
      }
      case ARC_DEC_ERROR_CHECKSUM:
        decodeRingBuf.deleteData(idx);
        return CRC_FAILURE;
      case ARC_DEC_ASYNC:
      case ARC_DEC_SYNC: {
        // Reset length of SLIP struct for next frame
        slipInfo.length = 0;
        if (decodedFrame != nullptr) {
          *decodedFrame = decodedRxFrame;
        }
        frameLen = rxFrameSize;
        decodeRingBuf.deleteData(idx);
        return returnvalue::OK;
      }
      default:
        sif::debug << "ArcsecDatalinkLayer::decodeFrame: Unknown result code" << std::endl;
        break;
        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::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, decodedRxFrame, &rxFrameSize);
//     *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;
// }

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