#include <bsp_q7s/comIF/I2cComIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include <errno.h>
#include <string.h>


I2cComIF::I2cComIF(object_id_t objectId): SystemObject(objectId){
}

I2cComIF::~I2cComIF() {}

ReturnValue_t I2cComIF::initializeInterface(CookieIF * cookie) {
	if(cookie == nullptr) {
		return NULLPOINTER;
	}
	I2cCookie* i2cCookie = dynamic_cast<I2cCookie*>(cookie);
	if(i2cCookie == nullptr) {
	    sif::error << "I2cComIF: Invalid I2C Cookie!"
	            << std::endl;
	    return NULLPOINTER;
	}

	address_t i2cAddress = i2cCookie->getAddress();

	int fd;
	std::string deviceFile = i2cCookie->getDeviceFile();
	fd = open(deviceFile.c_str(), O_RDWR);
	if (fd < 0) {
		sif::error << "I2cComIF: Opening i2c device failed with error code "
				<< errno << ". Error description: " << strerror(errno)
				<< std::endl;
	}

	if (ioctl(fd, I2C_SLAVE, i2cAddress) < 0) {
		sif::error << "I2cComIF: Specifying target device failed with error "
				<< "code " << errno << ". Error description "
				<< strerror(errno) << std::endl;
	}

	i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress);
	if(i2cDeviceMapIter == i2cDeviceMap.end()) {
		size_t maxReplyLen = i2cCookie->getMaxReplyLen();
		I2cInstance_t i2cInstance = {fd, std::vector<uint8_t>(maxReplyLen), 0};
		i2cDeviceMap.emplace(i2cAddress, i2cInstance);
	}
	else {
		sif::error << "I2cComIF: Device with address " << i2cAddress
				<< "already in use" << std::endl;
	}

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t I2cComIF::sendMessage(CookieIF *cookie,
        const uint8_t *sendData, size_t sendLen) {

	if(sendData == nullptr) {
	    sif::error << "I2cComIF::sendMessage: Send Data is nullptr"
	            << std::endl;
	    return HasReturnvaluesIF::RETURN_FAILED;
	}

	if(sendLen == 0) {
		return HasReturnvaluesIF::RETURN_OK;
	}

	I2cCookie* i2cCookie = dynamic_cast<I2cCookie*>(cookie);
	if(i2cCookie == nullptr) {
		sif::error << "I2cComIF::sendMessasge: Invalid I2C Cookie!"
				<< std::endl;
		return NULLPOINTER;
	}

	address_t i2cAddress = i2cCookie->getAddress();
	i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress);
	if (i2cDeviceMapIter == i2cDeviceMap.end()) {
		sif::error << "I2cComIF::sendMessage: i2cAddress of Cookie not "
				<< "registered in i2cDeviceMap" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	int fd = i2cDeviceMapIter->second.fileDescriptor;

	if (write(fd, sendData, sendLen) != (int)sendLen) {
		sif::error << "I2cComIF::sendMessage: Failed to send data to I2C "
				"device with error code " << errno << ". Error description: "
				<< strerror(errno) << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t I2cComIF::getSendSuccess(CookieIF *cookie) {
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF *cookie,
		size_t requestLen) {

	if (requestLen == 0) {
		return HasReturnvaluesIF::RETURN_OK;
	}

	I2cCookie* i2cCookie = dynamic_cast<I2cCookie*>(cookie);
	if(i2cCookie == nullptr) {
		sif::error << "I2cComIF::sendMessage: Invalid I2C Cookie!"
				<< std::endl;
		return NULLPOINTER;
	}

	address_t i2cAddress = i2cCookie->getAddress();
	i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress);
	if (i2cDeviceMapIter == i2cDeviceMap.end()) {
		sif::error << "I2cComIF::requestReceiveMessage: i2cAddress of Cookie not "
				<< "registered in i2cDeviceMap" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	int fd = i2cDeviceMapIter->second.fileDescriptor;
	uint8_t* replyBuffer = i2cDeviceMapIter->second.replyBuffer.data();

	if (read(fd, replyBuffer, requestLen) != (int)requestLen) {
		sif::error << "I2cComIF::requestReceiveMessage: Reading from I2C "
				<< "device failed with error code " << errno <<". Description"
				<< " of error: " << strerror(errno) << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	i2cDeviceMapIter->second.replyLen = requestLen;

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t I2cComIF::readReceivedMessage(CookieIF *cookie,
		uint8_t **buffer, size_t* size) {
	I2cCookie* i2cCookie = dynamic_cast<I2cCookie*>(cookie);
	if(i2cCookie == nullptr) {
		sif::error << "I2cComIF::readReceivedMessage: Invalid I2C Cookie!"
				<< std::endl;
		return NULLPOINTER;
	}

	address_t i2cAddress = i2cCookie->getAddress();
	i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress);
	if (i2cDeviceMapIter == i2cDeviceMap.end()) {
		sif::error << "I2cComIF::getReplyBuffer: i2cAddress of Cookie not "
				<< "registered in i2cDeviceMap" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	*buffer = i2cDeviceMapIter->second.replyBuffer.data();
	*size = i2cDeviceMapIter->second.replyLen;

	return HasReturnvaluesIF::RETURN_OK;
}