#pragma config(Sensor, S1,ard,sensorI2CCustom)
#include "common.h" // I2C lib
#include "settings.h"
ubyte 				I2CReply[17]  ;  //technically we should use tByteArray
ubyte			  	I2CRequest[17];
int xdata;
int ydata;
int wdata;
int datas;
// take two bytes and make an integer
int makeInt(ubyte highbyte,ubyte lowbyte){
	return (highbyte<<8) | lowbyte;
}
int makeInt(ubyte highbyte,ubyte lowbyte){
	return (highbyte<<8) | lowbyte;
}
//start at memoryAddress and return back replysize in bytes
int I2CArduino(tSensors link, ubyte memoryAddress,ubyte replysize) {
	memset(I2CRequest, 0, 17);
	memset(I2CReply, 0, 17);

	I2CRequest[0] = 2;    // Message size
	I2CRequest[1] = ARDUINO_I2C_ADDR; // I2C Address
	I2CRequest[2] = memoryAddress; // memory address
	if (!writeI2C(link,I2CRequest,I2CReply,replysize)){
		return -1;
	}
	return 0;
}

//===========================================================
task main () {

	nI2CRetries = 3;
	clearDebugStream();
	writeDebugStreamLine("Starting...");
	wait1Msec(500);
	while (true){
		if (I2CArduino(ard,ARD_DATA,8)){
			writeDebugStreamLine("ERROROR!!");
		}
		datas=makeInt(I2CReply[0],I2CReply[1]); // take two bytes for Determine the negative or positive value of X,Y,W. 
	        xdata=makeInt(I2CReply[2],I2CReply[3]); // take two bytes for X axis
		ydata=makeInt(I2CReply[4],I2CReply[5]); // take two bytes for Y axis
		wdata=makeInt(I2CReply[6],I2CReply[7]); // take two bytes for W axis
		if(datas&0x01) xdata=-xdata;// if (datas&0x01==1) X<0
		if(datas&0x02) ydata=-ydata;// if (datas&0x02==1) Y<0
		if(datas&0x04) wdata=-wdata;// if (datas&0x04==1) W<0
		nxtDisplayTextLine(1,"(%4d,%4d)",xdata,ydata);
		nxtDisplayTextLine(3,"%4d",wdata);
	}
}