/*
    RobotC-HTLink-Power Function PWM Mode
    Author: Mike Hatton
    Date: 13/08/08
    Contact: Parax77ATgooglemailDOTCom
    Usage:
        PFComboPWM(Channel(0-3), OutputRed(0-16), OutputBlue(0-16))
        Function populates PFBytes[] Array
        use sendI2CMsg to send PFBytes[] to HTLink Sensor
        See task main() below for example
    Changes: Removed large Bit[] Array, Bits written directly into PFByte Array
*/


//PFByteArray for I2C msg
byte PFBytes[17];


//Exponent Function:
int pow(int X, int Y)
{
	int Result = 1;
	int i = 0;
  for (i = 0; i < Y; i++)
  {
  	Result = Result * X;
  }
  return Result;
}


//PFBytes[] Creation Function
void PFComboPWM(byte Channel, byte OutR, byte OutB)
{
	byte PFBytei = 0;
	byte PFBiti = 0;
	byte Nib[4];
	byte Nibi = 0;
	byte NibBit = 0;
	//Clear PFBytes[] Array
	for (PFBytei = 0; PFBytei < 17; PFBytei++) {
		PFBytes[PFBytei] = 0x00;
	}
	PFBytei = 0;
	//Populate Nibbles
	Nib[0] = 0x4+Channel; //PWM Mode + (0-3)
	Nib[1] = OutB; //(0-15)
	Nib[2] = OutR; //(0-15)
	Nib[3] = 0xF; //XOR CRC
	Nib[3] = Nib[3]^Nib[0];
	Nib[3] = Nib[3]^Nib[1];
	Nib[3] = Nib[3]^Nib[2];
	//Write PFBytes[0-3]
	PFBytes[0] = 16; //PFByte Length
	PFBytes[1] = 0x02; //Sensor Address
	PFBytes[2] = 0x42; //Write to Address
	PFBytes[3] = 0x80; //Start Bit
	//Write PFBytes[4-13] From Nibbles
	//For Nibble 1 to 4
	for (Nibi = 0 ; Nibi < 4 ; Nibi++) {
		//For NibBit = 3 to 0 step -1
		for (NibBit = 3 ; NibBit >= 0 ; NibBit--) {
			//Write Bit to Byte
			PFBytes[4+(PFBiti/8)] = PFBytes[4+(PFBiti/8)] | pow(2,(7-(PFBiti % 8)));
			//Check NibBit & Move PFBiti
			if (Nib[Nibi] & pow(2,NibBit)) {
				//add High = 5
				PFBiti = PFBiti + 5;
			}
			else {
				//add Low = 3
				PFBiti = PFBiti + 3;
			}
		//Next NibBit
		}
	//Next Nibble
	}
	//Add Stop Bit, Trailing Bits are Zero
	PFBytes[4+(PFBiti/8)] = PFBytes[4+(PFBiti/8)] | pow(2,(7-(PFBiti % 8)));
	//Write PFBytes[14-16]
	PFBytes[14] = 0x11; //Length
	PFBytes[15] = 0x02; //PF Mode
	PFBytes[16] = 0x01; //Xmit
}


task main()
{
	//Create PFBytes[]
	PFComboPWM(0, 6, 7);

	//Send PFBytes[] to S1
	nI2CBytesReady[S1] = 0;
	SensorType[S1] = sensorI2CCustom9V;
	while(1) {
		while (nI2CStatus[S1] == STAT_COMM_PENDING) {
		  // Wait until I2C is ready
		}
		sendI2CMsg(S1, PFBytes[0], 0); // Send PFBytes
		wait10Msec(50);
	}
}