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

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

SusHandler::~SusHandler() {
}


void SusHandler::doStartUp(){
#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::WRITE_SETUP) {
        *id = SUS::WRITE_SETUP;
        communicationStep = CommunicationStep::REQUEST_TEMPERATURE;
    }
    else if (communicationStep == CommunicationStep::REQUEST_TEMPERATURE) {
        *id = SUS::REQUEST_TEMPERATURE;
        communicationStep = CommunicationStep::READ_TEMPERATURE;
    }
    else if (communicationStep == CommunicationStep::READ_TEMPERATURE) {
        *id = SUS::READ_TEMPERATURE;
        communicationStep = CommunicationStep::READ_CHANNELS;
    }
    else if (communicationStep == CommunicationStep::READ_CHANNELS) {
        *id = SUS::READ_CHANNELS;
        communicationStep = CommunicationStep::WRITE_SETUP;
    }
    return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t SusHandler::buildTransitionDeviceCommand(
		DeviceCommandId_t * id){
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SusHandler::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
		size_t commandDataLen) {
	switch(deviceCommand) {
	case(SUS::WRITE_SETUP): {
	    /**
	     * The sun sensor ADC is shutdown when CS is pulled high, so each time requesting a
	     * measurement the setup has to be rewritten. There must also be a little delay between
	     * the transmission of the setup byte and the first conversion. Thus the conversion
	     * will be performed in an extra step.
	     */
	    //TODO: Protect spi bus with mutex
	    gpioComIF->pullLow(chipSelectId);
		cmdBuffer[0] = SUS::SETUP_DEFINITION;
		rawPacket = cmdBuffer;
		rawPacketLen = 1;
		return RETURN_OK;
	}
	case(SUS::READ_CHANNELS): {
	    std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
 	    cmdBuffer[0] = SUS::CONVERT_DIFF_CHANNEL_0;
 	    cmdBuffer[1] = SUS::DUMMY_BYTE;
 	    cmdBuffer[2] = SUS::CONVERT_DIFF_CHANNEL_1;
 	    cmdBuffer[3] = SUS::DUMMY_BYTE;
 	    cmdBuffer[4] = SUS::CONVERT_DIFF_CHANNEL_2;
 	    cmdBuffer[5] = SUS::DUMMY_BYTE;
 	    cmdBuffer[6] = SUS::CONVERT_DIFF_CHANNEL_3;
 	    cmdBuffer[7] = SUS::DUMMY_BYTE;
 	    cmdBuffer[8] = SUS::CONVERT_DIFF_CHANNEL_4;
 	    cmdBuffer[9] = SUS::DUMMY_BYTE;
 	    cmdBuffer[10] = SUS::CONVERT_DIFF_CHANNEL_5;
 	    cmdBuffer[11] = SUS::DUMMY_BYTE;
 	    cmdBuffer[12] = SUS::DUMMY_BYTE;
 		rawPacket = cmdBuffer;
		rawPacketLen = SUS::SIZE_READ_CHANNELS;
		return RETURN_OK;
	}
	case(SUS::REQUEST_TEMPERATURE): {
	    std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
	    cmdBuffer[0] = SUS::CONVERT_TEMPERATURE;
	    rawPacket = cmdBuffer;
	    rawPacketLen = 1;
	    return RETURN_OK;
	}
	case(SUS::READ_TEMPERATURE): {
	    std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
	    rawPacket = cmdBuffer;
	    rawPacketLen = 24;
	    return RETURN_OK;
	}
	default:
		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
	}
	return HasReturnvaluesIF::RETURN_FAILED;
}

void SusHandler::fillCommandAndReplyMap() {
    this->insertInCommandMap(SUS::WRITE_SETUP);
    this->insertInCommandMap(SUS::REQUEST_TEMPERATURE);
    this->insertInCommandAndReplyMap(SUS::READ_CHANNELS, 1, &dataset, SUS::SIZE_READ_CHANNELS);
    this->insertInCommandAndReplyMap(SUS::READ_TEMPERATURE, 1, nullptr,
            SUS::SIZE_READ_TEMPERATURE);
}

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::READ_CHANNELS: {
	    PoolReadGuard readSet(&dataset);
	    dataset.ain0 = (*(packet + 1) << 8 | *(packet + 2));
	    dataset.ain1 = (*(packet + 3) << 8 | *(packet + 4));
	    dataset.ain2 = (*(packet + 5) << 8 | *(packet + 6));
	    dataset.ain3 = (*(packet + 7) << 8 | *(packet + 8));
	    dataset.ain4 = (*(packet + 9) << 8 | *(packet + 10));
	    dataset.ain5 = (*(packet + 11) << 8 | *(packet + 12));
#if OBSW_VERBOSE_LEVEL >= 1 && DEBUG_SUS
        sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN0: "
                << std::dec << dataset.ain0 << std::endl;
        sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN1: "
                << std::dec << dataset.ain1 << std::endl;
        sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN2: "
                << std::dec << dataset.ain2 << std::endl;
        sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN3: "
                << std::dec << dataset.ain3 << std::endl;
        sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN4: "
                << std::dec << dataset.ain4 << std::endl;
        sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN5: "
                << std::dec << dataset.ain5 << std::endl;
#endif
        /**
         * Reading the channel conversion results is the last communication step in one SUS
         * sequence. SPI bus can now be released again.
         */
        gpioComIF->pullHigh(chipSelectId);
		break;
	}
	case SUS::READ_TEMPERATURE: {
	    PoolReadGuard readSet(&dataset);
	    dataset.temperatureCelcius = (*(packet + 22) << 8 | *(packet + 23)) * 0.125;
#if OBSW_VERBOSE_LEVEL >= 1 && DEBUG_SUS
	    sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", temperature: "
            << dataset.temperatureCelcius << " °C" << 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 1000;
}

ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {
    localDataPoolMap.emplace(SUS::TEMPERATURE_C, new PoolEntry<float>( { 0.0 }));
    localDataPoolMap.emplace(SUS::AIN0, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SUS::AIN1, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SUS::AIN2, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SUS::AIN3, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SUS::AIN4, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SUS::AIN5, new PoolEntry<uint16_t>( { 0 }));
    return HasReturnvaluesIF::RETURN_OK;
}