#include "pca9685.h"


Pca9685::Pca9685(i2c_ch1_pImpL* i2c, ErrorHandler* err)
{
	i2c_pca9685 = i2c; 
	errorHandling = err; 
	m_isExternalClock = 0; 	
	m_oscillatorFreq = PCA9685_FREQUENCY_OSCILLATOR;
	setPwmFreq(1526); 
	m_currentLED = PCA9685_LED_PWM_REG_START; 

	errorHandling->addNewError(-1,__FILE__,"Selected duty cycle is too big",KILL);
	errorHandling->addNewError(-2,__FILE__,"Selected duty cycle is too small",KILL);
	errorHandling->addNewError(-3,__FILE__,"Selected PWM frequency is too fast",SPARE);
	errorHandling->addNewError(-4,__FILE__,"Selected PWM frequency is too slow",SPARE);
	errorHandling->addNewError(-5,__FILE__,"Oscillator frequency can't be 0 or inferior to 0",KILL);
	errorHandling->addNewError(-6,__FILE__,"Oscillator frequency cant't be higher than 50 MHz",KILL);
	errorHandling->addNewError(-7,__FILE__,"On Duty Cyle if greater than alowed maximum of 4095",SPARE);
	errorHandling->addNewError(-8,__FILE__,"Off Duty Cyle if greater than alowed maximum of 4095",SPARE);
	errorHandling->addNewError(-9,__FILE__,"Cumulation of the On & Off Duty Cyle if greater than alowed maximum of 4095",SPARE);
	errorHandling->addNewError(-10,__FILE__,"On & Off Duty Cyle can't be equal to each other",SPARE);
}

/*Refere to datasheet page 25 
 *
 *
 *						osc_clock 
 * prescale vlaue = _______________________ - 1
 *						4096 * update_rate
 *
 */

void Pca9685::setPwmFreq(float frequency)
{
	if(frequency > 1526)
	{
		frequency = 1526; 
		errorHandling->handleError(-3,__FILE__); 
	}
	else if (frequency == 1526) // Rounding correction
	{
		frequency -= 1;
	}
	
	if(frequency < 24)
	{
		frequency = 24; 
		errorHandling->handleError(-4,__FILE__); 
	}

	m_currentPrescale = round((m_oscillatorFreq / ( 4096 * frequency)) - 1);
	std::cout << unsigned(m_currentPrescale) << std::endl;
	sleep(); 
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, PCA9685_REG_PRESCALE, m_currentPrescale);
	wakeup(); 
}

float Pca9685::getPwmFreq()
{
	m_currentPrescale = i2c_pca9685->readByte(PCA9685_I2C_ADDRESS,PCA9685_REG_PRESCALE); 
	m_currentPwmFreq = (m_oscillatorFreq / (4096 * ((float)m_currentPrescale + 1)));
	std::cout << m_currentPwmFreq << std::endl;
	return m_currentPwmFreq;
}

void Pca9685::setDutyPercent(uint8_t ledNo, uint8_t on, uint8_t off)
{
	
}
void Pca9685::setDutyRaw(uint8_t ledNo, uint16_t on, uint16_t off)
{
	m_currentLED = PCA9685_LED_PWM_REG_START + (ledNo * PCA9685_LED_NEXT_OFFSET); 
	
	if(on > 4095)
	{
		on = 4095; 
		off = 0; 
		errorHandling->handleError(-7,__FILE__); 
	}
	
	if(off > 4095)
	{
		off = 4095; 
		on = 0; 
		errorHandling->handleError(-8,__FILE__); 
	}

	if(on + off > 4095)
	{
		off = 4095; 
		on = 0; 
		errorHandling->handleError(-9,__FILE__); 
	}
	
	if(on == off)
	{
		off = 4095; 
		on = 0; 
		errorHandling->handleError(-10,__FILE__); 
	}

	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, m_currentLED, on & 0xFF);
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, m_currentLED + PCA9685_PWM_ON_H_OFFSET, on >> 8);
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, m_currentLED + PCA9685_PWM_OFF_L_OFFSET, off & 0xFF);
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, m_currentLED + PCA9685_PWM_OFF_H_OFFSET, off >> 8);
}

void Pca9685::setOnOff(uint8_t ledNo, bool onOff, bool invert)
{
	
}
void Pca9685::confClk(bool internExtern, uint32_t freq)
{
	if(internExtern) 
	{
		m_isExternalClock = 0; 	
	}
	else 
	{
		m_isExternalClock = 1; 
		
		if(freq > 0)
		{
			if(freq <= 50000000)
			{
				m_oscillatorFreq = freq;
			}
			else
			{
				errorHandling->handleError(-6,__FILE__); 
			}
		}
		else
		{
			errorHandling->handleError(-5,__FILE__); 
		}
	}
}

void Pca9685::setOutputMode(bool mode)
{
	
}

void Pca9685::reset()
{
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, PCA9685_REG_MODE1, PCA9685_MODE1_RESTART);
}
void Pca9685::sleep()
{
	m_curMode = i2c_pca9685->readByte(PCA9685_I2C_ADDRESS,PCA9685_REG_MODE1);
	m_curMode = m_curMode | PCA9685_MODE1_SLEEP; 	
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, PCA9685_REG_MODE1, m_curMode);
}

void Pca9685::wakeup()
{
	m_curMode = i2c_pca9685->readByte(PCA9685_I2C_ADDRESS,PCA9685_REG_MODE1);
	m_curMode = m_curMode & ~PCA9685_MODE1_SLEEP; 	
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, PCA9685_REG_MODE1, m_curMode);
	usleep(PCA9685_OSC_STAB_TIME_US); // refer to datasheet page 14
}