#pragma once
#ifndef PCA9624_H
#define PCA9624_H

#ifdef TEENSYDUINO
#include <i2c_t3.h>
#else
// #include <Wire.h>
#endif

template <typename WireType>
class PCA9624_
{
    const uint8_t ADDR;

    enum class Reg
    {
        MODE1,
        MODE2,
        PWM0,
        PWM1,
        PWM2,
        PWM3,
        PWM4,
        PWM5,
        PWM6,
        PWM7,
        GRPPWM,
        GRPFREQ,
        LEDOUT0,
        LEDOUT1,
        SUBADR1,
        SUBADR2,
        SUBADR3,
        ALLCALLADR
    };


public:

    enum class LEDState
    {
        Disable,           // LEDs are OFF
        FullyOn,           // uncontrollable, fully on
        PWM_EACH,          // controllable from PWMx
        PWM_EACH_AND_GROUP // controllable from both PWMx and GRPPWM
    };

    PCA9624_(uint8_t addr) : ADDR(addr) {}

    void attach(WireType& w) { wire = &w; }

    void setup(WireType& w)
    {
        attach(w);
        sleep(false);
        setDriverOutputState(LEDState::PWM_EACH_AND_GROUP);
    }

    void sleep(bool b)
    {
        uint8_t data;
        if (b) data = 0b10010001;
        else   data = 0b10000001;
        writeByte(Reg::MODE1, data);
    }

    void setDriverOutputState(LEDState state)
    {
        uint8_t data[2];
        data[0] = ((uint8_t)state << 6) | ((uint8_t)state << 4) | ((uint8_t)state << 2) | ((uint8_t)state << 0);
        data[1] = ((uint8_t)state << 6) | ((uint8_t)state << 4) | ((uint8_t)state << 2) | ((uint8_t)state << 0);
        writeBytes(Reg::LEDOUT0, data, sizeof(data));
    }

    void drive(uint8_t ch, uint8_t vol)
    {
        writeByte(ADDR, (uint8_t)Reg::PWM0 + ch, vol);
    }

    void drive(uint8_t vol)
    {
        uint8_t data[8];
        for (uint8_t i = 0; i < 8; ++i) data[i] = vol;
        writeBytes(Reg::PWM0, data, sizeof(data));
    }

    // void drive() // TODO: variable arguments
    // {
    // }

private:

    void writeByte(Reg reg, uint8_t data)
    {
        writeByte(ADDR, (uint8_t)reg, data);
    }

    void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
    {
        wire->beginTransmission(address);  // Initialize the Tx buffer
        wire->write(subAddress);           // Put slave register address in Tx buffer
        wire->write(data);                 // Put data in Tx buffer
        i2c_err_ = wire->endTransmission();           // Send the Tx buffer
        if (i2c_err_) pirntI2CError();
    }

    void writeBytes(Reg reg, uint8_t* data, uint8_t size)
    {
        writeBytes(ADDR, (uint8_t)reg, data, size);
    }

    void writeBytes(uint8_t address, uint8_t subAddress, uint8_t* data, uint8_t size)
    {
        wire->beginTransmission(address);  // Initialize the Tx buffer
        wire->write(subAddress | 0x80);           // auto increment
        for (uint8_t i = 0; i < size; ++i)
            wire->write(*(data + i));                 // Put data in Tx buffer
        i2c_err_ = wire->endTransmission();           // Send the Tx buffer
        if (i2c_err_) pirntI2CError();
    }

    uint8_t readByte(uint8_t address, uint8_t subAddress)
    {
        uint8_t data = 0; // `data` will store the register data
        wire->beginTransmission(address);         // Initialize the Tx buffer
        wire->write(subAddress);	                 // Put slave register address in Tx buffer
        i2c_err_ = wire->endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive
        if (i2c_err_) pirntI2CError();
        wire->requestFrom(address, (size_t)1);  // Read one byte from slave register address
        if (wire->available()) data = wire->read();                      // Fill Rx buffer with result
        return data;                             // Return data read from slave register
    }

    void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
    {
        wire->beginTransmission(address);   // Initialize the Tx buffer
        wire->write(subAddress);            // Put slave register address in Tx buffer
        i2c_err_ = wire->endTransmission(false);  // Send the Tx buffer, but send a restart to keep connection alive
        if (i2c_err_) pirntI2CError();
        uint8_t i = 0;
        wire->requestFrom(address, count);  // Read bytes from slave register address
        while (wire->available())
        {
            dest[i++] = wire->read();
        } // Put read results in the Rx buffer
    }

    void pirntI2CError()
    {
        if (i2c_err_ == 7) return; // to avoid stickbreaker-i2c branch's error code
        Serial.print("I2C ERROR CODE : ");
        Serial.println(i2c_err_);
    }

    std::array<uint8_t, 8> leds;
    WireType* wire;
    uint8_t i2c_err_;
};

#ifdef TEENSYDUINO
using PCA9624 = PCA9624_<i2c_t3>;
#else
using PCA9624 = PCA9624_<TwoWire>;
#endif

#endif // PCA9624_H
