#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(200); 
	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);

}

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

void Pca9685::setPwmFreq(uint16_t 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 = (m_oscillatorFreq / ( 4096 * frequency) ) -1;
	
	sleep(); 
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, PCA9685_REG_PRESCALE, m_currentPrescale);
	wakeup(); 
}

void Pca9685::setPwmMicroSeconds(uint16_t microSeconds)
{
	
}
void Pca9685::setDutyPercent(uint8_t ledNo, uint8_t on, uint8_t off)
{
	
}
void Pca9685::setDutyRaw(uint8_t ledNo, uint8_t on, uint8_t off)
{
	m_currentLED = PCA9685_LED_PWM_REG_START + (ledNo * PCA9685_LED_NEXT_OFFSET); 

	std::cout << "Led" <<unsigned(ledNo)<<" ON L : " << unsigned(m_currentLED) << std::endl; 
	std::cout << "Led" <<unsigned(ledNo)<<" ON H : " << unsigned(m_currentLED+PCA9685_PWM_ON_H_OFFSET) << std::endl; 
	std::cout << "Led" <<unsigned(ledNo)<<" OFF L : " << unsigned(m_currentLED+PCA9685_PWM_OFF_L_OFFSET) << std::endl; 
	std::cout << "Led" <<unsigned(ledNo)<<" 0FF H : " << unsigned(m_currentLED+PCA9685_PWM_OFF_H_OFFSET) << std::endl; 
	

}
void Pca9685::setDutyMicroSecond(uint8_t ledNo, uint16_t on, uint16_t off)
{
}
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
}