#include <bsp_q7s/gpio/LinuxLibgpioIF.h>
#include <gpio.h>
#include <unistd.h>

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

LinuxLibgpioIF::~LinuxLibgpioIF() {
}

ReturnValue_t LinuxLibgpioIF::initialize(CookieIF * cookie){
	ReturnValue_t result;
	GpioMap mapToAdd;
	GpioMapIter mapToAddIter;

	if(cookie == nullptr) {
		return NULLPOINTER;
	}
	GpioCookie* GpioCookie = dynamic_cast<GpioCookie*>(cookie);
	if(GpioCookie == nullptr) {
		sif::error << "LinuxLibgpioIF: 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*/
	result = initializeAndConfigure(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 << "LinuxLibgpioIF::initialize: Failed to add "
				<< "GPIO with ID " << mapToAddIter.first << " to gpioMap"
				<< std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId){
	return driveGpio(gpioId, 1);
}

ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId){
	return driveGpio(gpioId, 0);
}

ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId,
		unsigned int logiclevel) {
	GpioMapIter gpioMapIter = gpioMap.find(gpioId);
	char *chipname;
	unsigned int lineNum;
	struct gpiod_chip *chip;
	struct gpiod_line *line;
	int result;
	Gpio::Direction direction;

	/* Verify if GPIO has been configured as output */
	direction = gpioMapIter.second.direction;
	if (direction != Gpio::OUT) {
		sif::error << "LinuxLibgpioIF::pullHigh: GPIO with ID " << gpioId
				<< "not configured as output" << std::endl;
		return CONFIGURATION_FAILURE;
	}

	chipname = gpioMapIter.second.chipname;
	chip = gpiod_chip_open_by_name(chipname);
	if (!chip) {
		sif::error << "LinuxLibgpioIF::pullHigh: Failed to open chip "
				<< chipname << ". Gpio ID: " << gpioId << std::endl;
		return OPEN_CHIP_FAILURE;
	}

	lineNum = gpioMapIter.second.lineNum;
	line = gpiod_chip_get_line(chip, lineNum);
	if (!line) {
		sif::error << "LinuxLibgpioIF::pullHigh: Failed to open line. Gpio ID "
				<< gpioId << std::endl;
		gpiod_chip_close(chip);
		return OPEN_LINE_FAILURE;
	}

	result = gpiod_line_request_output(line, CONSUMER, 0);
	if (result < 0) {
		sif::error << "LinuxLibgpioIF::pullHigh: Failed to request line "
				<< line << " from GPIO instance with ID: " << gpioId
				<< std::endl;
		gpiod_line_release(line);
		return REQUEST_LINE_FAILURE;
	}

	result = gpiod_line_set_value(line, logiclevel);
	if (result < 0) {
		sif::error << "LinuxLibgpioIF::pullHigh: Failed to pull GPIO with ID "
				<< gpioId << "to low" << std::endl;
		gpiod_line_release(line);
		return PULLING_HIGH_FAILURE;
	}

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t LinuxLibgpioIF::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 << "LinuxLibgpioIF::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;
}