#include <bsp_q7s/comIF/GpioComIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fcntl.h>
#include <string.h>


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

GpioComIF::~GpioComIF() {}

ReturnValue_t GpioComIF::initializeInterface(CookieIF * cookie) {

	ReturnValue_t result;
	GpioMap mapToAdd;

	if(cookie == nullptr) {
		return NULLPOINTER;
	}
	GpioCookie* GpioCookie = dynamic_cast<GpioCookie*>(cookie);
	if(GpioCookie == nullptr) {
	    sif::error << "GpioComIF: Invalid Gpio Cookie!"
	            << std::endl;
	    return NULLPOINTER;
	}

	mapToAdd = GpioCookie->getGpioMap();

	result = checkForConflicts(mapToAdd);
	if (result != HasReturnvaluesIF::RETURN_OK){
		return result;
	}

	/* Register new GPIOs in gpioMap*/
	std::pair insertionResult = gpioMap.insert(mapToAdd.begin(),
			mapToAdd.end());
	if (insertionResult.second() != true) {
		sif::error << "GpioComIF::initializeAndConfigure: Failed to add "
				<< "GPIO " << gpioStr.c_st() << " to gpioMap" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	return HasReturnvaluesIF::RETURN_OK;
}

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

	ReturnValue_t result;
	int fd;
	std::string deviceFile;

	if(sendData == nullptr) {
	    sif::error << "GpioComIF::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 << "GpioComIF::sendMessasge: Invalid I2C Cookie!"
				<< std::endl;
		return NULLPOINTER;
	}

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

	deviceFile = i2cCookie->getDeviceFile();
	result = openDevice(deviceFile, i2cAddress, &fd);
	if (result != HasReturnvaluesIF::RETURN_OK){
		return result;
	}

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

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

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

	ReturnValue_t result;
	int fd;
	std::string deviceFile;

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

	I2cCookie* i2cCookie = dynamic_cast<I2cCookie*>(cookie);
	if(i2cCookie == nullptr) {
		sif::error << "GpioComIF::requestReceiveMessage: Invalid I2C Cookie!"
				<< std::endl;
		i2cDeviceMapIter->second.replyLen = 0;
		return NULLPOINTER;
	}

	address_t i2cAddress = i2cCookie->getAddress();
	i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress);
	if (i2cDeviceMapIter == i2cDeviceMap.end()) {
		sif::error << "GpioComIF::requestReceiveMessage: i2cAddress of Cookie not "
				<< "registered in i2cDeviceMap" << std::endl;
		i2cDeviceMapIter->second.replyLen = 0;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	deviceFile = i2cCookie->getDeviceFile();
	result = openDevice(deviceFile, i2cAddress, &fd);
	if (result != HasReturnvaluesIF::RETURN_OK){
		i2cDeviceMapIter->second.replyLen = 0;
		return result;
	}

	uint8_t* replyBuffer = i2cDeviceMapIter->second.replyBuffer.data();

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

	i2cDeviceMapIter->second.replyLen = requestLen;

	close(fd);

	return HasReturnvaluesIF::RETURN_OK;
}

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

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

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GpioComIF::openDevice(std::string deviceFile,
		address_t i2cAddress, int* fileDescriptor) {
	*fileDescriptor = open(deviceFile.c_str(), O_RDWR);
	if (*fileDescriptor < 0) {
		sif::error << "GpioComIF: Opening i2c device failed with error code "
				<< errno << ". Error description: " << strerror(errno)
				<< std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	if (ioctl(*fileDescriptor, I2C_SLAVE, i2cAddress) < 0) {
		sif::error << "GpioComIF: Specifying target device failed with error "
				<< "code " << errno << ". Error description "
				<< strerror(errno) << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GpioComIF::checkForConflicts(GpioMap mapToAdd){
	gpioId_t gpioId;
	GpioMapIter mapToAddIter = mapToAdd.begin();
	for(; mapToAddIter != mapToAdd.end(); mapToAddIter++){
		gpio = mapToAddIter.first();
		if(gpioMapIter.find(gpioId) != mapToAdd.end()){
			/* An entry for this GPIO already exists. Check if configuration
			 * of direction is equivalent */
			if (mapToAddIter.second.direction != gpioMapIter.second.direction){
				sif::error << "GpioComIF::checkForConflicts: Detected conflict "
						<< "for GPIO " << mapToAddIter.first() << std::endl;
				return HasReturnvaluesIF::RETURN_OK;
			}
			/* Remove element from map to add because a entry for this GPIO
			 * already exists */
			mapToAdd.erase(mapToAddIter);
		}
	}
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GpioComIF::initializeAndConfigure(GpioMap mapToAdd){

	int exportfd;
	int directionfd;
	std::string gpioStr;
	GpioMapIter mapToAddItr;

	mapToAddItr = mapToAdd.begin();
	/* Perform initialization for all GPIOs in map */
	for(; mapToAddItr != mapToAdd.end(); mapToAddItr++){
		/* The GPIO has to be exported to be able to see it in sysfs */
		exportfd = open("/sys/class/gpio/export", O_WRONLY);
		if (exportfd < 0)
		{
			sif::error << "GpioComIF::initializeAndConfigure: Cannot open GPIO "
					<< mapToAddItr.first() << " to export it" << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		gpioStr = std::to_string(mapToAddItr.first());
		write(exportfd, gpioStr.c_str(), gpioStr.length() + 1);
		close(exportfd);

		/* Set direction of the GPIO */
		std::string directionPath = "/sys/class/gpio/" + gpioStr +
				"/direction";
		directionfd = open(directionPath.c_str(), O_RDWR);
		if (directionfd < 0)
		{
			sif::error << "Cannot open " << directionPath.c_str() << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}

		if (mapToAdd.second() == Gpio::IN) {
			write(directionfd, "in", 3);
		}
		else if (mapToAdd.second() == Gpio::OUT) {
			write(directionfd, "out", 4);
		}
		else {
			sif::error << "GpioComIF::initializeAndConfigure: Invalid direction"
					<< " specified" << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		close(directionfd);

		/* Add GPIO to GpioMap of GpioComIF */
		std::pair insertionResult = gpioMap.insert(mapToAddItr);
		if (insertionResult.second() != true) {
			sif::error << "GpioComIF::initializeAndConfigure: Failed to add "
					<< "GPIO " << gpioStr.c_st() << " to gpioMap" << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
	}
	return HasReturnvaluesIF::RETURN_OK;
}