#include "RPiGPIO.h"

#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fcntl.h>
#include <sys/types.h>
#include <unistd.h>
#include <cstdint>


int RPiGPIO::enablePin(int pin) {
    char buffer[BUFFER_MAX];
    ssize_t bytes_written;
    int fd;

    fd = open("/sys/class/gpio/export", O_WRONLY);
    if (fd == -1) {
        sif::error << "Failed to open export of pin " << pin << " for writing!" << std::endl;
        return -1;
    }

    bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pin);
    write(fd, buffer, bytes_written);
    close(fd);
    return 0;
}

int RPiGPIO::disablePin(int pin) {
    char buffer[BUFFER_MAX];
    ssize_t bytes_written;
    int fd;

    fd = open("/sys/class/gpio/unexport", O_WRONLY);
    if (fd == -1) {
        sif::error << "Failed to open unexport of pin " << pin << " for writing!" << std::endl;
        return -1;
    }

    bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pin);
    write(fd, buffer, bytes_written);
    close(fd);
    return(0);
}

int RPiGPIO::pinDirection(int pin, Directions dir) {

    char path[DIRECTION_MAX];

    snprintf(path, DIRECTION_MAX, "/sys/class/gpio/gpio%d/direction", pin);
    int fd = open(path, O_WRONLY);
    if (fd == -1) {
        sif::error << "Failed to open gpio " << pin << " direction for writing!" << std::endl;
        return -1;
    }

    int result = 0;
    if(dir == Directions::IN) {
        result = write(fd, "in", IN_WRITE_SIZE);
    }
    else {
        result = write(fd, "out", OUT_WRITE_SIZE);
    }

    if (result != 0) {
        sif::error << "Failed to set direction!" << std::endl;
        return -2;
    }

    close(fd);
    return 0;
}

int RPiGPIO::readPin(int pin) {
    char path[VALUE_MAX];
    char value_str[3];

    snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
    int fd = open(path, O_RDONLY);
    if (fd == -1) {
        sif::error << "RPiGPIO::readPin: Failed to open GPIO pin " << pin << "!" << std::endl;
        return -1;
    }

    if (read(fd, value_str, 3) == -1) {
        sif::error << "Failed to read value!" << std::endl;
        return -1;
    }

    close(fd);
    char* endPtr = nullptr;

    return strtol(value_str, &endPtr, 10);
}

int RPiGPIO::writePin(int pin, States state) {

    char path[VALUE_MAX];
    int fd;

    snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
    fd = open(path, O_WRONLY);
    if (fd == -1) {
        sif::error << "RPiGPIO::writePin: Failed to open GPIO pin " << pin << "!" << std::endl;
        return -1;
    }

    int result = 0;
    if(state == States::LOW) {
        result = write(fd, "0", 1);
    }
    else {
        result = write(fd, "1", 1);
    }


    if (result != 0) {
        sif::error << "Failed to write pin " << pin << " value to " << static_cast<int>(state)
                << "!" << std::endl;
        return -1;
    }

    close(fd);
    return 0;
}