#include "StarTrackerHandler.h"
#include "OBSWConfig.h"

#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.h>

extern "C" {
    #include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
    #include "common/misc.h"
}

StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
        CookieIF * comCookie) :
        DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this) {
    if (comCookie == NULL) {
        sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
    }
    slipInit();
}

StarTrackerHandler::~StarTrackerHandler() {
}

void StarTrackerHandler::doStartUp() {

#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
    setMode(MODE_NORMAL);
#else
    setMode(_MODE_TO_ON);
#endif
}

void StarTrackerHandler::doShutDown() {
    setMode(_MODE_POWER_DOWN);
}

ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
    switch (internalState) {
    case InternalState::TEMPERATURE_REQUEST:
        *id = StarTracker::REQ_TEMPERATURE;
        break;
    default:
        sif::debug << "StarTrackerHandler::buildNormalDeviceCommand: Invalid internal step"
                << std::endl;
        break;
    }
    return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
    return NOTHING_TO_SEND;
}

ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
        const uint8_t * commandData, size_t commandDataLen) {

    switch (deviceCommand) {
    case (StarTracker::REQ_TEMPERATURE): {
        prepareTemperatureRequest();
        return RETURN_OK;
    }
    default:
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
    return HasReturnvaluesIF::RETURN_FAILED;
}

void StarTrackerHandler::fillCommandAndReplyMap() {
    /** Reply lengths are unknown because of the slip encoding. Thus always maximum reply size
      * is specified */
	this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 1, &temperatureSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
}

ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t remainingSize,
        DeviceCommandId_t *foundId, size_t *foundLen) {

    uint32_t decodedLength = 0;

    for (size_t idx = 0; idx < remainingSize; idx++) {
        enum arc_dec_result decResult = arc_transport_decode_body(*(start + idx), &slipInfo,
                decodedFrame, &decodedLength);

        switch (decResult) {
        case ARC_DEC_INPROGRESS: {
            continue;
        }
        case ARC_DEC_ASYNC: {
            sif::debug << "StarTrackerHandler::scanForReply: Received asychronous tm" << std::endl;
            /** No asynchronous replies are expected as of now */
            return RETURN_FAILED;
        }
        case ARC_DEC_ERROR_FRAME_SHORT:
            return REPLY_TOO_SHORT;
        case ARC_DEC_ERROR_CHECKSUM:
            return CRC_FAILURE;
        case ARC_DEC_SYNC: {
            /** Reset length of SLIP struct for next frame */
            slipInfo.length = 0;
            break;
        }
        default:
            sif::debug << "StarTrackerHandler::scanForReply: Unknown result code" << std::endl;
            break;
        }
    }

    switch (decodedFrame[1]) {
    case (static_cast<uint8_t>(StarTracker::REQ_TEMPERATURE)): {
        *foundLen = decodedLength;
        *foundId = StarTracker::REQ_TEMPERATURE;
        break;
    }
    default: {
        sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
                << std::endl;
        return RETURN_FAILED;
        break;
    }
    }

    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {

    switch (id) {
    case (StarTracker::REQ_TEMPERATURE): {
        handleTemperatureTm();
        break;
    }
    default: {
        sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
    }

    return RETURN_OK;
}

void StarTrackerHandler::setNormalDatapoolEntriesInvalid() {

}

uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
    return 5000;
}

ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {

    localDataPoolMap.emplace(StarTracker::STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TICKS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMOS_TEMPERATURE, new PoolEntry<float>( { 0 }));

    return RETURN_OK;
}

size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId){
    return StarTracker::MAX_FRAME_SIZE;
}

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

void StarTrackerHandler::prepareTemperatureRequest() {
    uint32_t length = 0;
    arc_tm_pack_temperature_req(commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::handleTemperatureTm() {
    PoolReadGuard rg(&temperatureSet);
    uint32_t offset = 1;
    temperatureSet.status = *(decodedFrame + offset);
    offset += 1;
    if(temperatureSet.status.value != 0) {
        sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
                << static_cast<unsigned int>(temperatureSet.status.value) << std::endl;
        triggerEvent(TM_REPLY_ERROR, temperatureSet.status.value);
    }
    temperatureSet.ticks = *(decodedFrame + offset) << 24
            | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
            | *(decodedFrame + offset + 3);
    offset += 4;
    temperatureSet.time = static_cast<uint64_t>(*(decodedFrame + offset)) << 56
            | static_cast<uint64_t>(*(decodedFrame + offset + 1)) << 48
            | static_cast<uint64_t>(*(decodedFrame + offset + 2)) << 40
            | static_cast<uint64_t>(*(decodedFrame + offset + 3)) << 32
            | *(decodedFrame + offset + 4) << 24 | *(decodedFrame + offset + 5) << 16
            | *(decodedFrame + offset + 6) << 8 | *(decodedFrame + offset + 7);
    offset += 8;
    temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24
            | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
            | *(decodedFrame + offset + 3);
    offset += 4;
    temperatureSet.cmosTemperature = *(decodedFrame + offset) << 24
            | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
            | *(decodedFrame + offset + 3);
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    sif::info << "StarTrackerHandler::handleTemperatureTm: MCU Temperature: "
            << temperatureSet.mcuTemperature << " °C" << std::endl;
    sif::info << "StarTrackerHandler::handleTemperatureTm: CMOS Temperature: "
            << temperatureSet.mcuTemperature << " °C" << std::endl;
#endif
}