/*
 * TODO : output mde selection
 * TODO : Implement chainning function to use it with more than one module 
 * 
 *
 *
 *
 *
 */

#include "pca9685.h"


Pca9685::Pca9685(I2C* i2c)
{
	i2c_pca9685 = i2c; 
	m_isExternalClock = 0; 	
	m_oscillatorFreq = PCA9685_FREQUENCY_OSCILLATOR;
	turnAllOff();
	setPwmFreq(1000); 
	m_currentLED = PCA9685_LED_PWM_REG_START; 
}

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

void Pca9685::setPwmFreq(float frequency)
{	
	if(frequency > 1526)
	{
		frequency = 1526; 
		throwError(freqTooFast); 
	}
	else if (frequency == 1526) // Rounding correction
	{
		frequency -= 1;
	}
	
	if(frequency < 24)
	{
		frequency = 24; 
		throwError(freqTooSlow); 
	}

	m_currentPrescale = round(((m_oscillatorFreq / ( 4096 * frequency))) - 1);

	
	setPwmRaw(m_currentPrescale);
}


void Pca9685::turnAllOff()
{
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS,PCA9685_ALL_OFF_H, 16 );
}
void Pca9685::turnAllOn()
{
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS,PCA9685_ALL_ON_H, 16 );
}
//The impresition leves of thise device is out from this world this is a crude way from me trying to mitigate de awfullness. 
//Tested with other module all to react the same 
//maybe an external Clock could help with that but MCU level precision will never be reached !! Too sad
uint8_t Pca9685::compensatePrescale(uint8_t preScale)
{
	uint8_t compensation = 0;

	if(preScale > 236)
	{
		compensation = 12;
	}
	else if(preScale > 216 )
	{
		compensation = 11;
	}
	else if(preScale > 200)
	{
		compensation = 10;
	}
	else if(preScale > 178)
	{
		compensation = 9;
	}
	else if(preScale > 157)
	{
		compensation = 8;
	}
	else if(preScale > 134)
	{
		compensation = 7;
	}
	else if(preScale > 114)
	{
		compensation = 6;
	}
	else if(preScale > 93)
	{
		compensation = 5;
	}
	else if(preScale > 71)
	{
		compensation = 4;
	}
	else if(preScale > 52)
	{
		compensation = 3;
	}
	else if(preScale > 32)
	{
		compensation = 2;
	}
	else if(preScale > 9)
	{
		compensation = 1;
	}
	return preScale - compensation;
}


// direct register acess and raw value to set PWM
// Datashhet page 25 
// FEh | PRE_SCALE7:0 | PRE_SCALE[7:0] | R/W | 0001 1110 | prescaler to program the PWM output frequency (default is 200 Hz)
void Pca9685::setPwmRaw(uint8_t preScale)
{
	m_currentPrescale = compensatePrescale(preScale);
	if(m_currentPrescale < 3 )
	{
		m_currentPrescale = 3; 
		throwError(freqTooFast); 
	}
	else if(m_currentPrescale > 255)
	{
		m_currentPrescale = 255; 
		throwError(freqTooSlow); 
	}
	//std::cout << unsigned(m_currentPrescale) << " " << std::endl;
	sleep(); 
	i2c_pca9685->writeWord(PCA9685_I2C_ADDRESS, PCA9685_REG_PRESCALE, m_currentPrescale);
	wakeup(); 
}


/*Refere to datasheet page 25 
 *
 *
 *				osc_clock 
 * Freq = _______________________
 *			4096 * (prescaler -1)
 *
 */
float Pca9685::getPwmFreq()
{
	m_currentPrescale = compensatePrescale(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;
}

/*
 *The duty cylce is set in a special fation, rather than being relation between on and off time it is the timing definition of their ocurances. 
 * ON Meas after how many cycle counts from the PWM start will the risign edge occur 
 * OFF Means after heo many cycle counts from the PWM start will the falling edge will occur
 * Than means : ON can't be equal to OFF anf OFF cna't be smaller than off 
 * 4096 as value is not a typo error it is used to turn one led fully on or off. Refere to datasheet page 21
 */
void Pca9685::setDutyRaw(uint8_t ledNo, uint16_t on, uint16_t off)
{
	if(ledNo > 15)
	{
		throwError(portInvalid); 
	}

	if(on > 4095)
	{
		on = 4096; 
		off = 0; 
		throwError(onDutySetTooBig); 
	}
	
	if(off > 4095)
	{
		off = 4096; 
		on = 0; 
		throwError(offDutySetTooBig); 
	}

	if(on + off > 4095)
	{
		off = 4096; 
		on = 0; 
		throwError(onOffDutySetTooBig); 
	}
	
	if(on == off)
	{
		off = 4096; 
		on = 0; 
		throwError(onEqaulOff); 
	}

	if(off == 0)// if there is no falling edge that measn that the signal is allways off 
	{
		off = 4096; // set to fully off
		on = 0; 
	}

	m_currentLED = PCA9685_LED_PWM_REG_START + (ledNo * PCA9685_LED_NEXT_OFFSET); 
	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::setOnDutyPercent(uint8_t ledNo, uint8_t percent)
{
	uint16_t onLength = 0; 
	if(ledNo > 15)
	{
		throwError(portInvalid); 
	}

	if(percent <= 100)
	{
		if(percent == 0)
		{
			setOnOff(ledNo, 0);
		}
		else if(percent == 100)
		{
			setOnOff(ledNo, 1);
		}
		else 
		{
			onLength = (4095 * percent)	/ 100; 
			setDutyRaw(ledNo,0,onLength);
		}
	}
	else 
	{
		throwError(dutyCycle100Prcnt); 
	}
}

//turns the given Port fully on or foo using the special value of 4096
void Pca9685::setOnOff(uint8_t ledNo, bool onOff)
{
	uint16_t on, off = 0; 		
	
	if(ledNo > 15)
	{
		throwError(portInvalid); 
	}

	if(onOff)
	{
		off = 0; 
		on = 4096; 
	}
	else 
	{
		off = 4096; 
		on = 0; 
	}
	
	m_currentLED = PCA9685_LED_PWM_REG_START + (ledNo * PCA9685_LED_NEXT_OFFSET); 
	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);
}


//Sets if internal or External clok would be used. 
//if external clok will be used user must define it's frequency "uint32_t freq" in Hz
//The maximum supported external clok frequency if 50 Mhz
//Defaul intern Clok is 25 Mhz
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
			{
				throwError(freqOver50Mhz); 
			}
		}
		else
		{
			throwError(freqIsNull); 
		}
	}
}
//TODO
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
}

void Pca9685::throwError(Pca9685::errors errNo)
{	
	#ifdef LINUX
	std::cout << "Linux pca9685 Error" << std::endl; 
	exit(1); 
	#endif
}