using namespace std;

// Registers/etc:
// #define MODE1 0x00			//Mode  register  1
// #define MODE2 0x01			//Mode  register  2
constexpr uint8_t MODE1              = 0x00;
constexpr uint8_t MODE2              = 0x01;
constexpr uint8_t SUBADR1            = 0x02;
constexpr uint8_t SUBADR2            = 0x03;
constexpr uint8_t SUBADR3            = 0x04;
constexpr uint8_t PRESCALE           = 0xFE;
constexpr uint8_t PCA9685_CHANNEL_0  = 0x06;
constexpr uint8_t LED0_ON_H          = 0x07;
constexpr uint8_t LED0_OFF_L         = 0x08;
constexpr uint8_t LED0_OFF_H         = 0x09;
constexpr uint8_t ALL_LED_ON_L       = 0xFA;
constexpr uint8_t ALL_LED_ON_H       = 0xFB;
constexpr uint8_t ALL_LED_OFF_L      = 0xFC;
constexpr uint8_t ALL_LED_OFF_H      = 0xFD;
#define LED_MULTIPLYER 4	// For the other 15 channels

// Bits:
constexpr uint8_t RESTART            = 0x80;
constexpr uint8_t SLEEP              = 0x10;
constexpr uint8_t ALLCALL            = 0x01;
constexpr uint8_t INVRT              = 0x10;
constexpr uint8_t OUTDRV             = 0x04;


class pca9685
{
    public:
        pca9685();
		void startupPCA9685();  // run at startup
		void setPCA9685Freq(double freqHz);  // set pca9685 frequency
        void changePCA9685DriverID(short i, short v);  // assign pca9685 driverID
        void changePCA9685Address(short i, short v);  // assign pca9685 address
        void changePCA9685totalEnabled(short v);  // update total pca9685's enabled
		void adjustPinLevel(short pinID, short value);  // adjust pca9685 pin level
		short getPinLevel(short pinID);  // get PWM level of channel
        short getPCA9685totalEnabled();  // return total number of pca9685's enabled
		int getFrequency(bool cache);
        // short getPCA9685Address(short i);  // return PCA9685 i2c address
        // int getPCA9685I2Cfile(short i);
    private:
		int pca9685I2Cfile[6] = {};  // used when reading and writing to I2C bus
		short pca9685Address[6] = {};  // decimal i2c address for pca9685
		short pca9685DriverID[6] = {};  // 
		short pca9685totalEnabled = 0;  // track number of pca9685 drivers
		short pca9685PinID[96] = {};  // track pinID, enough for 6 pca9685's
		short pca9685PinNumber[96] = {};  // track pinID, enough for 6 pca9685's
		short pca9685AddressSaved[6] = {};  // decimal i2c address for pca9685
		short pca9685TotalPins = 0;
		int frequency = 200;
};
pca9685::pca9685(){}

int pca9685::getFrequency(bool cache)
{
//   if (cache) return frequency;
  
	short pinLoc = 0;
	for(short a=0; a < pca9685TotalPins; a++ )  // search all channels for matching pinID
	{
		if (pca9685PinID[a] == 115) {pinLoc = a; break;}  // record array location
	}

	short driverLoc = 0;
	for(short a=0; a < pca9685totalEnabled; a++ )  // search all channels for matching i2c address so we know which driverID is required
	{
		if (pca9685Address[a] == pca9685AddressSaved[pinLoc]) {driverLoc = a; break;}  // record array location
	}
//   uint8_t scaler = readReg(PRESCALE);
//   scaler++;
  
	char buffer[5];
	int length = 1; 
	// uint8_t reg
    // buffer[0] = 0x00; 
	buffer[0] = 0xFE;
	write(pca9685I2Cfile[driverLoc], buffer, length); // initiate write

  
	length = 1; 
	char input[20];
	// buffer[0] = (uint8_t)1;
	// buffer[0] = PRESCALE;
	// buffer[1] = 0;


    // if (read(pca9685I2Cfile[driverLoc], input, 1) != length) {
	// 	return frequency;
	// }
    // else {
		read(pca9685I2Cfile[driverLoc], input, 10);

		  uint8_t scaler = 0;
		// scaler++;
		frequency /= 8;
		if (startupComplete == 0) {
			std::cout << "input             = " << input << std::endl;
			std::cout << "input             = " << input[0] << std::endl;
			std::cout << "input             = " << input[1] << std::endl;
			std::cout << "input             = " << input[2] << std::endl;
			// std::cout << "input             = " << input[3] << std::endl;
			// std::cout << "scaler             = " << scaler << std::endl;
		// frequency = 48828 / scaler;
			std::cout << "frequency               = " << frequency << std::endl;
		}
		return frequency;
	// }

}

short pca9685::getPCA9685totalEnabled() {return pca9685totalEnabled;}  // return total number of pca9685's enabled

// short pca9685::getPCA9685Address(short i) {return pca9685Address[i];}  // return PCA9685 i2c address

// int pca9685::getPCA9685I2Cfile(short i) {return pca9685I2Cfile[i];}

void pca9685::changePCA9685DriverID(short i, short v) {pca9685DriverID[i] = v;}  // assign pca9685 driverID

void pca9685::changePCA9685totalEnabled(short v) {pca9685totalEnabled = v;}  // update total pca9685's enabled

void pca9685::changePCA9685Address(short i, short v) {pca9685Address[i] = v;}  // assign pca9685 address

void pca9685::adjustPinLevel(short pinID, short value) {  // adjust pca9685 pin level
    // std::cout << "adjustPinLevel  - " << pinID << " - " << value << std::endl;
	short pinLoc = 0;
	for(short a=0; a < pca9685TotalPins; a++ )  // search all channels for matching pinID
	{
		if (pca9685PinID[a] == pinID) {pinLoc = a; break;}  // record array location
	}

	short driverLoc = 0;
	for(short a=0; a < pca9685totalEnabled; a++ )  // search all channels for matching i2c address so we know which driverID is required
	{
		if (pca9685Address[a] == pca9685AddressSaved[pinLoc]) {driverLoc = a; break;}  // record array location
	}

	short a = 0;
	char buffer[5];
	int length = 5; 
	buffer[0] = PCA9685_CHANNEL_0 + (pca9685PinNumber[pinLoc] << 2);
	buffer[1] = a & 0xFF;
	buffer[2] = (a >> 8) & 0x1F;
	buffer[3] = value & 0xFF;
	buffer[4] = (value >> 8) & 0x1F;
	write(pca9685I2Cfile[driverLoc], buffer, length); // initiate write
	delay(5);

	// verify GPIO pin was updated
	bool f = 0;  // flag to exit while loop
	while (f == 0)  // check if pca9685 received the signal
	{
		if (getPinLevel(pinID) == value) {f = 1;}  // pca9685 was updated correctly
		else {  // pca9685 wasn't updated, try again
    		// std::cout << "TRY AGAIN                 - " << std::endl;
            logError(3, to_string(pca9685AddressSaved[pinLoc]), "PCA9685 did not update");
			delay(50);
			f = 0;
			buffer[0] = PCA9685_CHANNEL_0 + (pca9685PinNumber[pinLoc] << 2);
			buffer[1] = a & 0xFF;
			buffer[2] = (a >> 8) & 0x1F;
			buffer[3] = value & 0xFF;
			buffer[4] = (value >> 8) & 0x1F;
			write(pca9685I2Cfile[driverLoc], buffer, length); // initiate write
			delay(50);
		}
	}
}

short pca9685::getPinLevel(short pinID) {  // get PWM level of channel
	short pinLoc = 0;
	for(short a=0; a < pca9685TotalPins; a++ )  // search all channels for matching pinID
	{
		if (pca9685PinID[a] == pinID) {pinLoc = a; break;}  // record array location
	}

	short driverLoc = 0;
	for(short a=0; a < pca9685totalEnabled; a++ )  // search all channels for matching i2c address so we know which driverID is required
	{
		if (pca9685Address[a] == pca9685AddressSaved[pinLoc]) {driverLoc = a; break;}  // record array location
	}

	char buffer[1];
	buffer[0] = PCA9685_CHANNEL_0 + (pca9685PinNumber[pinLoc] << 2);  // send channel number request
	write(pca9685I2Cfile[driverLoc], buffer, 1); // initiate write
	
    uint16_t inData[4];
	int length = 4;  // number of bytes to read
	if (read(pca9685I2Cfile[driverLoc], inData, 4) != length) {
		std::cout << "Failed to read from the i2c bus - getPWM." << std::endl;
		return -1;
	}

    // std::cout << "getPinLevel    - " << pinID << " - " << inData[1] << std::endl;
	return inData[1];  // return PWM level
}

void pca9685::setPCA9685Freq(double freqHz) {  // set pca9685 frequency
  frequency = freqHz;
    // std::cout << "freqHz        = " << freqHz << std::endl;
    // std::cout << "frequency     = " << frequency << std::endl;

  auto prescaleval = 25000000.0; //    # 25MHz
  prescaleval /= 4096.0; //       # 12-bit
  prescaleval /= freqHz;
  prescaleval -= 1.0;

    // std::cout << "prescaleval   = " << prescaleval << std::endl;
  auto prescale = static_cast<int>(std::round(prescaleval));
    // std::cout << "prescale      = " << prescale << std::endl;

	int prescaleINT = prescale;
    // std::cout << "prescaleINT   = " << prescaleINT << std::endl;


	char buffer[5];
		buffer[0] = 0x0;    // target register
  auto oldmode = read(pca9685I2Cfile[0], buffer, 1);
    // std::cout << "oldmode     = " << oldmode << std::endl;

  auto newmode = (oldmode & 0x7F) | SLEEP;
    // std::cout << "newmode     = " << newmode << std::endl;

	std::stringstream ss;
	ss<< std::hex << prescale; // int decimal_value
	std::string prescaleHex ( "0x" + ss.str() );

	std::stringstream ss1;
	ss1<< std::hex << oldmode; // int decimal_value
	std::string oldmodeHex ( "0x" + ss1.str() );

	std::stringstream ss2;
	ss2<< std::hex << newmode; // int decimal_value
	std::string newmodeHex ( "0x" + ss2.str() );

	// std::cout << "prescaleHex = " << prescaleHex << std::endl;
	// std::cout << "oldmodeHex  = " << oldmodeHex << std::endl;
	// std::cout << "newmodeHex  = " << newmodeHex << std::endl;

	int length = 2;       // number of bytes, including address

	buffer[0] = MODE1;
	buffer[1] = 0x11;
	write(pca9685I2Cfile[0], buffer, length); // initiate write
	
	buffer[0] = PRESCALE;
	buffer[1] = 0x3;  // PRESCALE  // 24hz = 0xfd  / 1526hz = 0x3
	write(pca9685I2Cfile[0], buffer, length); // initiate write
	
	buffer[0] = MODE1;
	buffer[1] = 0x1;
	write(pca9685I2Cfile[0], buffer, length); // initiate write
	
  usleep(5'000);
  	// delay(500);
	// buffer[0] = MODE1;
	// buffer[1] = 0x1;
	// write(pca9685I2Cfile[0], buffer, length); // initiate write

	buffer[0] = MODE1;    // target register
	buffer[1] = 0x20; // desired value
	write(pca9685I2Cfile[0], buffer, length); // initiate write


}

void pca9685::startupPCA9685() {  // run at startup
    std::cout << " " << std::endl;
    std::cout << "startupPCA9685 --------------------------------" << std::endl;

    MYSQL_RES *res;
    MYSQL_ROW row;

    // get list of all pca9685 pins
    connectMysql();
    std::string query = "SELECT pinsAvailable.pinID, pinsAvailable.gpio, i2cDevices.decAddress\
        FROM pinsAvailable\
        LEFT JOIN i2cDevices\
        ON pinsAvailable.driverID = i2cDevices.i2cDriverID\
        WHERE i2cDevices.i2cDeviceTypeID = 2\
        ORDER BY pinsAvailable.pinID ASC";

    mysql_query(conn, query.c_str());  // run query
    res = mysql_use_result(conn);
    
    short c = 0;
    while ((row = mysql_fetch_row(res)) != NULL)
    {
		// std::cout << atoi(row[0]) << " - " << atoi(row[1]) << " - " << atoi(row[2]) << std::endl;
		pca9685PinID[c] = atoi(row[0]);
		pca9685PinNumber[c] = atoi(row[1]);
		pca9685AddressSaved[c] = atoi(row[2]);
		c++;
    }
    mysql_free_result(res);
    mysql_disconnect(); 

	pca9685TotalPins = c;

    std::cout << "pca9685TotalPins        = " << pca9685TotalPins << std::endl;
    std::cout << "pca9685totalEnabled     = " << pca9685totalEnabled << std::endl;
	char *filename = (char*)"/dev/i2c-1"; // Define the i2c file path
	for(short a=0; a < pca9685totalEnabled; a++ )  // go through each pca9685 and initiate
	{
		pca9685I2Cfile[a] = open(filename, O_RDWR); // open file for read and right
		
		// open connection for pca9685
		if (ioctl(pca9685I2Cfile[a], I2C_SLAVE, pca9685Address[a]) < 0)
		{
			std::cout << "Failed to acquire bus access and/or talk to slave." << std::endl;
			return;
		}
		// setPCA9685Freq(1526);
		char buffer[2];
		buffer[0] = 0x0;    // target register
		buffer[1] = 0x21; // desired value - initialize pca9685
		int length = 2;       // number of bytes
		write(pca9685I2Cfile[a], buffer, length); // write

		std::cout << "pca9685I2Cfile     = " << pca9685I2Cfile[a] << std::endl;
		std::cout << "pca9685Address     = " << pca9685Address[a] << std::endl;
		std::cout << "pca9685DriverID    = " << pca9685DriverID[a] << std::endl;
		std::cout << "" << std::endl;

	

		// // setPCA9685Freq(24);
		// length = 2;       // number of bytes, including address
// 		buffer[0] = 0x0;    // target register
//   auto oldmode = read(pca9685I2Cfile[0], buffer, 1);
//     std::cout << "oldmode     = " << oldmode << std::endl;
	
	}
	if (pca9685totalEnabled != 0) {getFrequency(false);}
  		
		  
    	// std::cout << "Freq               = " << Freq << std::endl;
}


// void pca9685::adjustPinLevel_WORKING(short pinID, short value) {  // adjust pca9685 pin level
//     // std::cout << "lightModePinNumber  - " << lightModePinNumber[mode][c] << std::endl;
// 	char buffer[5];
// 	int length = 2;       // number of bytes, including address
// 	int startAdd = 0x06;
// 	int stopAdd = 0x08;

// 	short pinLoc = 0;
// 	for(short a=0; a < pca9685TotalPins; a++ )  // search all channels for matching pinID
// 	{
// 		if (pca9685PinID[a] == pinID) {pinLoc = a; break;}  // record array location
// 	}

// 	short driverLoc = 0;
// 	for(short a=0; a < pca9685totalEnabled; a++ )  // search all channels for matching i2c address so we know which driverID is required
// 	{
// 		if (pca9685Address[a] == pca9685AddressSaved[pinLoc]) {driverLoc = a; break;}  // record array location
// 	}
	
// 	startAdd = startAdd + (pca9685PinNumber[pinLoc] * 4);  // calculate startAdd for channel being adjusted
// 	stopAdd = stopAdd + (pca9685PinNumber[pinLoc] * 4);  // ccalculate stopAdd for channel being adjusted

// 	buffer[0] = startAdd;  // "start time" reg for channel 15
// 	buffer[1] = 0;     // We want the pulse to start at time t=0
// 	buffer[2] = 0;
// 	length = 3;        // 3 bytes total written
// 	if (write(pca9685I2Cfile[driverLoc], buffer, length) != length)  // initiate the write
// 	{
//         // std::cout << "Failed to write to the i2c bus." << std::endl;
// 	}

// 	// write(pca9685I2Cfile[0], buffer, length); // initiate the write

// 	// buffer[0] = 0x40;   // "stop time" reg for channel 14
// 	buffer[0] = stopAdd;   // "stop time" reg for channel 15
// 	buffer[1] = value & 0xff; // The "low" short comes first...
// 	buffer[2] = (value>>8) & 0xff; // followed by the high short.
// 	if (write(pca9685I2Cfile[driverLoc], buffer, length) != length)  // initiate the write
// 	{
//         // std::cout << "Failed to write to the i2c bus." << std::endl;
// 	}
// }

pca9685 pca9685;