#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/devices/SusHandler.h>
#include <OBSWConfig.h>
#include <sys/time.h>

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

SusHandler::~SusHandler() {
}


void SusHandler::doStartUp(){
    if (internalState == InternalState::CONFIGURED) {
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
        setMode(MODE_NORMAL);
#else
        setMode(_MODE_TO_ON);
#endif
    }
}

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

ReturnValue_t SusHandler::buildNormalDeviceCommand(
		DeviceCommandId_t * id) {
    if (communicationStep == CommunicationStep::PERFORM_CONVERSIONS) {
        *id = SUS::PERFORM_CONVERSIONS;
//        communicationStep = CommunicationStep::READ_TEMP;
        communicationStep = CommunicationStep::PERFORM_CONVERSIONS;
    }
    else if (communicationStep == CommunicationStep::READ_TEMP) {
        *id = SUS::READ_TEMP;
        communicationStep = CommunicationStep::PERFORM_CONVERSIONS;
    }
    return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t SusHandler::buildTransitionDeviceCommand(
		DeviceCommandId_t * id){
    if (internalState == InternalState::SETUP) {
        *id = SUS::WRITE_SETUP;
    }
    else {
        return HasReturnvaluesIF::RETURN_OK;
    }
	return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t SusHandler::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
		size_t commandDataLen) {
	switch(deviceCommand) {
	case(SUS::WRITE_SETUP): {
		cmdBuffer[0] = SUS::SETUP_DEFINITION;
		cmdBuffer[1] = SUS::UNIPOLAR_CONFIG;
		rawPacket = cmdBuffer;
		rawPacketLen = 2;
		internalState = InternalState::CONFIGURED;
		return RETURN_OK;
	}
	case(SUS::PERFORM_CONVERSIONS): {
	    std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
	    /**
	     * The sun sensor ADC is shutdown when CS is pulled high so each time requesting a
	     * measurement the setup has to be rewritten
	     */
	    cmdBuffer[0] = SUS::SETUP_DEFINITION;
        cmdBuffer[1] = SUS::UNIPOLAR_CONFIG;
	    // wirte one dummy byte here
//        cmdBuffer[2] = SUS::CONVERT_TEMPERATURE;
	    cmdBuffer[2] = SUS::CONVERT_TEMPERATURE;
//	    struct timeval startOfDelay;
//        gettimeofday(&startOfDelay, NULL);
//        struct timeval currentTime;
//        gettimeofday(&currentTime, NULL);
//        while (currentTime.tv_usec - startOfDelay.tv_usec < 1000) {
//            gettimeofday(&currentTime, NULL);
//        }

//	    cmdBuffer[27] = SUS::CONVERT_DIFF_CHANNEL_0_1;
//	    cmdBuffer[29] = SUS::CONVERT_DIFF_CHANNEL_2_3;
//	    cmdBuffer[31] = SUS::CONVERT_DIFF_CHANNEL_4_5;

//	    cmdBuffer[0] = SUS::SETUP_DEFINITION;
//        cmdBuffer[1] = SUS::UNIPOLAR_CONFIG;
//	    cmdBuffer[2] = SUS::CONVERT_TEMPERATURE;
//	    cmdBuffer[26] = SUS::CONVERT_DIFF_CHANNEL_0_1;
//	    cmdBuffer[28] = SUS::CONVERT_DIFF_CHANNEL_2_3;
//	    cmdBuffer[30] = SUS::CONVERT_DIFF_CHANNEL_4_5;
 		rawPacket = cmdBuffer;
//		rawPacketLen = SUS::SIZE_PERFORM_CONVERSIONS;
		rawPacketLen = 7;
		return RETURN_OK;
	}
	case(SUS::READ_TEMP): {
	    std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
	    rawPacket = cmdBuffer;
	    rawPacketLen = 26;
	    return RETURN_OK;
	}
	default:
		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
	}
	return HasReturnvaluesIF::RETURN_FAILED;
}

void SusHandler::fillCommandAndReplyMap() {
    this->insertInCommandMap(SUS::WRITE_SETUP);
    this->insertInCommandAndReplyMap(SUS::READ_TEMP, 1, &dataset, SUS::SIZE_PERFORM_CONVERSIONS);
//    this->insertInCommandAndReplyMap(SUS::PERFORM_CONVERSIONS, 1, &dataset,
//            SUS::SIZE_PERFORM_CONVERSIONS);
    this->insertInCommandMap(SUS::PERFORM_CONVERSIONS);
}

ReturnValue_t SusHandler::scanForReply(const uint8_t *start,
        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
    *foundId = this->getPendingCommand();
    *foundLen = remainingSize;
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id,
        const uint8_t *packet) {
	switch (id) {
	case SUS::PERFORM_CONVERSIONS: {
	    PoolReadGuard readSet(&dataset);
	    dataset.temperatureCelcius = (*(packet + 25) << 8 | *(packet + 26)) * 0.125;
	    dataset.diffScanChannel0_1 = (*(packet + 29) << 8 | *(packet + 30));
	    dataset.diffScanChannel2_3 = (*(packet + 31) << 8 | *(packet + 32));
	    dataset.diffScanChannel4_5 = (*(packet + 33) << 8 | *(packet + 34));
#if OBSW_VERBOSE_LEVEL >= 1 && DEBUG_SUS
        sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", temperature: "
                << dataset.temperatureCelcius << " °C" << std::endl;
        sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 0/1: "
                << dataset.diffScanChannel0_1 << std::endl;
        sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 2/3: "
                << dataset.diffScanChannel2_3 << std::endl;
        sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 4/5: "
                << dataset.diffScanChannel4_5 << std::endl;
#endif
		break;
	}
	default: {
	    sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
	}

	}
	return HasReturnvaluesIF::RETURN_OK;
}

void SusHandler::setNormalDatapoolEntriesInvalid(){

}

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

ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {
    localDataPoolMap.emplace(SUS::TEMPERATURE_C, new PoolEntry<float>( { 0.0 }));
    localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_0_1, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_2_3, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_4_5, new PoolEntry<uint16_t>( { 0 }));
    return HasReturnvaluesIF::RETURN_OK;
}