#include "LinuxLibgpioIF.h"
#include "GpioCookie.h"

#include <fsfw/serviceinterface/ServiceInterface.h>

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

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

LinuxLibgpioIF::~LinuxLibgpioIF() {
}

ReturnValue_t LinuxLibgpioIF::initialize(GpioCookie* gpioCookie){
    ReturnValue_t result;
    if(gpioCookie == nullptr) {
        sif::error << "LinuxLibgpioIF::initialize: Invalid cookie" << std::endl;
        return RETURN_FAILED;
    }

    GpioMap 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;

    //mapToAddIter = mapToAdd->begin();
    for(auto& gpioConfig: mapToAdd) {
    //for (; mapToAddIter != mapToAdd->end(); mapToAddIter++) {
        chipname = gpioConfig.second.chipname;
        chip = gpiod_chip_open_by_name(chipname.c_str());
        if (!chip) {
            sif::error << "LinuxLibgpioIF::configureGpios: Failed to open chip "
                    << chipname << ". Gpio ID: " << gpioConfig.first << std::endl;
            return RETURN_FAILED;
        }

        lineNum = gpioConfig.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 = gpioConfig.second.direction;
        consumer = gpioConfig.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(),
                    gpioConfig.second.initValue);
            if (result < 0) {
                sif::error << "LinuxLibgpioIF::configureGpios: Failed to request line "
                        << lineNum << " from GPIO instance with ID: " << gpioConfig.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: " << gpioConfig.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.
         */
        gpioConfig.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;

    auto 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;

    auto 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;
    auto gpioMapIter = gpioMap.begin();
    for(auto& gpioConfig: mapToAdd) {
        gpioId = gpioConfig.first;
        /* Cross check with private map */
        gpioMapIter = gpioMap.find(gpioId);
        if(gpioMapIter != mapToAdd.end()) {
            /* An entry for this GPIO already exists. Check if configuration
             * of direction is equivalent */
            if (gpioConfig.second.direction != gpioMapIter->second.direction){
                sif::error << "LinuxLibgpioIF::checkForConflicts: Detected conflict for GPIO " <<
                        gpioConfig.first << std::endl;
                return RETURN_FAILED;
            }
            /* Remove element from map to add because a entry for this GPIO
             * already exists */
            mapToAdd.erase(gpioConfig.first);
        }
    }
    return RETURN_OK;
}