#include <sta/devices/raspi/bus/i2c.hpp>

#ifdef STA_PLATFORM_RASPI

#include <sta/assert.hpp>

#include <cstdlib>
#include <string.h>
#include <iostream>

#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h>


namespace sta 
{
    RaspiI2c::RaspiI2c(I2cNode node, Mutex * mutex, bool persistent_open)
        : I2c{mutex}, persistent_open_{persistent_open}
    {
        // Safer version of malloc + strcpy
        i2cdev_ = strdup(node == I2cNode::DEV_1 ? "/dev/i2c-1" : "/dev/i2c-2");

        STA_ASSERT(i2cdev_ != nullptr);
    }

    RaspiI2c::~RaspiI2c()
    {
        if (i2cdev_ != NULL ) {
            free(i2cdev_);
	        i2cdev_ = NULL;
        }
        
        if (open_) {
            close(i2cfd_);
        }
    }

    void RaspiI2c::transfer(uint8_t value)
    {
        STA_ASSERT(open_);

        write(i2cfd_, &value, 1);
    }

    void RaspiI2c::transfer16(uint16_t value)
    {
        STA_ASSERT(open_);

        write(i2cfd_, &value, 2);
    }

    void RaspiI2c::transfer(const uint8_t * buffer, size_t size)
    {
        STA_ASSERT(open_);

        write(i2cfd_, buffer, size);
    }

    void RaspiI2c::transfer(const uint8_t * txBuffer, uint8_t * rxBuffer, size_t size)
    {
        STA_ASSERT(open_);

        // TODO: Is this even possible in i2c?
    }

    void RaspiI2c::receive(uint8_t * buffer, size_t size)
    {
        STA_ASSERT(open_);

        if (read(i2cfd_, buffer, size) <= 0)
        {
            printf("Error while reading i2c bus.");
        }
    }

    void RaspiI2c::fill(uint8_t value, size_t count) 
    {
        STA_ASSERT(open_);

        // Initialize a buffer of size count and fill it with the value.
        uint8_t *buffer = new uint8_t[count];
        memset(buffer, value, count);

        write(i2cfd_, buffer, count);

        delete [] buffer; 
    }

    void RaspiI2c::selectAddress(uint16_t address)
    {
        if (ioctl(i2cfd_, I2C_SLAVE, address) < 0)
        {
            printf("Failed to send the slave address.");
        }
    }

    void RaspiI2c::acquire()
    {
        I2c::acquire();

        if (open_) {
            return;
        }

        i2cfd_ = open(i2cdev_, O_RDWR);
        open_ = true;

        STA_ASSERT(i2cfd_ >= 0);
    }

    void RaspiI2c::release()
    {
        if (!persistent_open_ && open_) {
            close(i2cfd_);
        }

        I2c::release();
    }

    RaspiI2cDevice::RaspiI2cDevice(I2c * intf, uint16_t address_10bit, Mutex* mutex, bool master, bool blocking) 
        : I2cDevice { intf, address_10bit }
    {
        
    }
} // namespace sta

#endif // STA_PLATFORM_RASPI