#include <bsp_q7s/gpio/LinuxLibgpioIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>

#include <utility>
#include <unistd.h>
#include <gpiod.h>

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

LinuxLibgpioIF::~LinuxLibgpioIF() {
}

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

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

	mapToAdd = gpioCookie->getGpioMap();

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

	result = configureGpios(mapToAdd);
	if (result != RETURN_OK) {
	    return RETURN_FAILED;
	}

	/* Register new GPIOs in gpioMap*/
	gpioMap.insert(mapToAdd.begin(), mapToAdd.end());

	return RETURN_OK;
}

ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap mapToAdd) {
    GpioMapIter mapToAddIter;
    std::string chipname;
    unsigned int lineNum;
    struct gpiod_chip *chip;
    Gpio::Direction direction;
    std::string consumer;
    struct gpiod_line *lineHandle;
    int result;

    for (mapToAddIter = mapToAdd.begin(); mapToAddIter != mapToAdd.end(); mapToAddIter++) {

        chipname = gpioMapIter->second.chipname;
        chip = gpiod_chip_open_by_name(chipname.c_str());
        if (!chip) {
            sif::error << "LinuxLibgpioIF::configureGpios: Failed to open chip "
                    << chipname << ". Gpio ID: " << gpioMapIter->first << std::endl;
            return RETURN_FAILED;
        }

        lineNum = gpioMapIter->second.lineNum;
        lineHandle = gpiod_chip_get_line(chip, lineNum);
        if (!lineHandle) {
            sif::error << "LinuxLibgpioIF::configureGpios: Failed to open line" << std::endl;
            gpiod_chip_close(chip);
            return RETURN_FAILED;
        }

        direction = gpioMapIter->second.direction;
        consumer = gpioMapIter->second.consumer;
        /* Configure direction and add a description to the GPIO */
        switch (direction) {
        case Gpio::OUT:
            result = gpiod_line_request_output(lineHandle, consumer.c_str(),
                    gpioMapIter->second.initValue);
            if (result < 0) {
                sif::error << "LinuxLibgpioIF::configureGpios: Failed to request line "
                        << lineNum << " from GPIO instance with ID: " << gpioMapIter->first
                        << std::endl;
                gpiod_line_release(lineHandle);
                return RETURN_FAILED;
            }
            break;
        case Gpio::IN:
            result = gpiod_line_request_input(lineHandle, consumer.c_str());
            if (result < 0) {
                sif::error << "LinuxLibgpioIF::configureGpios: Failed to request line "
                        << lineNum << " from GPIO instance with ID: " << gpioMapIter->first
                        << std::endl;
                gpiod_line_release(lineHandle);
                return RETURN_FAILED;
            }
            break;
        default:
            sif::error << "LinuxLibgpioIF::configureGpios: Invalid direction specified"
                    << std::endl;
            return RETURN_FAILED;
        }
        /**
         * Write line handle to GPIO configuration instance so it can later be used to set or
         * read states of GPIOs.
         */
        gpioMapIter->second.lineHandle = lineHandle;
    }
    return 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) {
    int result;
	struct gpiod_line *lineHandle;

	GpioMapIter gpioMapIter = gpioMap.find(gpioId);
    if (gpioMapIter == gpioMap.end()){
        sif::debug << "LinuxLibgpioIF::driveGpio: Unknown gpio id " << gpioId << std::endl;
        return RETURN_FAILED;
    }

    lineHandle = gpioMapIter->second.lineHandle;
	result = gpiod_line_set_value(lineHandle, logiclevel);
	if (result < 0) {
		sif::error << "LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID "
				<< gpioId << "to logic level" << logiclevel << std::endl;
		return DRIVE_GPIO_FAILURE;
	}

	return RETURN_OK;
}

ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) {
    struct gpiod_line *lineHandle;

    GpioMapIter gpioMapIter = gpioMap.find(gpioId);
    if (gpioMapIter == gpioMap.end()){
        sif::debug << "LinuxLibgpioIF::readGpio: Unknown gpio id " << gpioId << std::endl;
        return RETURN_FAILED;
    }

    lineHandle = gpioMapIter->second.lineHandle;
    *gpioState = gpiod_line_get_value(lineHandle);

    return RETURN_OK;
}

ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap mapToAdd){
	gpioId_t gpioId;
	GpioMapIter mapToAddIter = mapToAdd.begin();
	for(; mapToAddIter != mapToAdd.end(); mapToAddIter++){
		gpioId = mapToAddIter->first;
		gpioMapIter = gpioMap.find(gpioId);
		if(gpioMapIter != 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 RETURN_OK;
			}
			/* Remove element from map to add because a entry for this GPIO
			 * already exists */
			mapToAdd.erase(mapToAddIter);
		}
	}
	return RETURN_OK;
}