/***************************************************************************************
Filename:lego.cpp, version v-1.0
Authored By: Sudhanshu Gaur (sgaur@vt.edu)
Acknowledgement: This work is based on Daniel Berger's <daniel.berger@tuebingen.mpg.de>
VC++ RCX interface and Kekoa's <kekoa@graphics.stanford.edu> RCX Opcode reference page
****************************************************************************************/

#include <iostream>
using namespace std;
#include <windows.h>
#include <stdio.h>
#include "lego.h"


lego::lego()
{
	// initialize private variables
	switchbit = 0;
	UsbPort = 0;
	dbg_lvl = 0;
}

int lego::open()
{
	int val;

	// open the USB port for comm with RCX
	if (!OpenUsbPort()) {
		cerr<<"Cannot initialize USB port on this computer"<<endl;
		return(0);
	}
	if (dbg_lvl > 0) {
		cout<<"Opened the USB port"<<endl;
	}

	for( ;; )
	{
		if( Alive(val) == 0)
	        Sleep(5000);
		else
			break;
	}

	SetTransmitRange(SHORT_RANGE);
	StopMotor(A);
	StopMotor(B);
	StopMotor(C);
	SetMotorSpeed(A,7);
	SetMotorSpeed(B,7);
	SetMotorSpeed(C,7);
	SetMotorDirection(A,FORWARD);
	SetMotorDirection(B,FORWARD);
	SetMotorDirection(C,FORWARD);
	return(1);
}

int lego::close()
{
	// should turn off all of the motors
	StopMotor(A);
	StopMotor(B);
	StopMotor(C);

	CloseUsbPort();
	if (dbg_lvl > 0) {
		cout<<"Turned off all of the motors and closed the USB port"<<endl;
	}
	return(1);
}

lego::~lego()
{
}

// not a member of the class, but a useful utility
void print_data(unsigned char *arr,int len)
{
	int t;
	cout<<"LEN("<<len<<"):";
	for (int i=0;i<len;i++) {
		t = arr[i];
		cout<<t<<",";
	}
	cout<<endl;
}

void lego::DbgOn()
{
	dbg_lvl = 1;
}

void lego::DbgOff()
{
	dbg_lvl = 0;
}

unsigned char lego::toggle(unsigned char inval)
{
	unsigned char outval;

	outval = inval | switchbit;
	switchbit ^= 0x08;
	return(outval);
}

int lego::SetTone(int freq,int dur)
{
  if ((freq < 1) || (freq > 20000)) {
	  cerr<<"Frequency out of range; must be 1-20000"<<endl;
	  return(0);
  }
  if ((dur < 1) || (dur > 255)) {
	  cerr<<"Duration out of range; must be 1-255"<<endl;
	  return(0);
  }
	  msg[0]=toggle(0x23);
      msg[1]=freq % 256;
	  msg[2]=freq / 256;
	  msg[3]=dur;
      RCX_send(msg,4);
      if(RCX_receive(recbuf,16) == 0){
		  cerr<<"Cannot set tone"<<endl;
		  return(0);
	  }
	  if (dbg_lvl > 0) {
		  cout<<"Set tone"<<endl;
	  }
	return(1);
}

int lego::SetSound(LEGO_SOUND sval)
{
	  msg[0]=toggle(0x51);
      msg[1]=sval;
      RCX_send(msg,2);
      if(RCX_receive(recbuf,16) == 0){
		  cerr<<"Cannot set sound"<<endl;
		  return(0);
	  }
	  if (dbg_lvl > 0) {
		  cout<<"Set sound"<<endl;
	  }
	return(1);
}

int lego::SetSensorMode(SENSOR_NAME name, SENSOR_MODE mode)
{
	int num_recv;

	 /* Set the Type of sensor */
	  msg[0] = toggle(0x42);
      msg[1]=name;
	  msg[2]= mode;
      RCX_send(msg,3);
	  num_recv = RCX_receive(recbuf,16);
	  if (num_recv != 7) {
		  cerr<<"Cannot set sensor mode"<<endl;
		  return(0);
	  }
	  if (dbg_lvl > 0) {
		  cout<<"Set sensor mode"<<endl;
	  }
	  return(1);
}

int lego::SetSensor(SENSOR_NAME name, SENSOR_TYPE type)
{
	int num_recv;

	 /* Set the Type of sensor */
	  msg[0] = toggle(0x32);
      msg[1]=name;
	  msg[2]= type;
      RCX_send(msg,3);
	  num_recv = RCX_receive(recbuf,16);
	  if (num_recv != 7) {
		  cerr<<"Cannot set sensor type"<<endl;
		  return(0);
	  }
	  if (dbg_lvl > 0) {
		  cout<<"Set sensor type"<<endl;
	  }

	  // given the type, let's set the mode
	  switch(type)
	  {
	  case TOUCH_SENSOR:
		  if (!SetSensorMode(name, BOOLEAN_MODE)) {
			  return(0);
		  }
		  break;
	  case LIGHT_SENSOR:
		  if (!SetSensorMode(name, PERCENTAGE_MODE)) {
			  return(0);
		  }
		  break;
	  case ROTATION_SENSOR:
		  if (!SetSensorMode(name, ANGLE_MODE)) {
			  return(0);
		  }
		  break;
	  default:
		  printf("\nError:: sensor type has to be TOUCH_SENSOR, LIGHT_SENSOR or ROTATION_SENSOR\n");
		  break;
	  }
	  return(1);
}

int lego::SetTransmitRange(TRANSMITTER_RANGE dist)
{
	int num_recv;

  msg[0]=toggle(0x31);
  msg[1]=dist;
  RCX_send(msg,2);
  num_recv = RCX_receive(recbuf,16);
  if (num_recv == 0) {
	  cout << "\nFirmware not found"<<endl;
	  DownloadFirmware();
	  SetTransmitRange(dist);
     //cerr<<"Cannot set transmit range"<<endl;
	 return(0);
  }
  if (dbg_lvl > 0) {
	cout<<"Set IR transmit range"<<endl;
  }
  return(1);
}

int lego::Alive(int &value)
{
	  int len;
	  int ret_val;

      msg[0]=toggle(0x10); //get value
      RCX_send(msg,1);
      len=RCX_receive(recbuf,22);
	  if (len == 0) {
		  cerr<<"Please Turn on the RCX"<<endl;
		  return(0);
	  }
      ret_val = RCX_decode_sensor(recbuf,len,value);
	  if (ret_val != len) {
		  cerr<<"Cannot decode alive value"<<endl;
		  return(0);
	  }
	return(1);
}

int lego::ReadSensor(SENSOR_NAME name,int &value)
{
	  int len;
	  int ret_val;

      msg[0]=toggle(0x12); //get value
      msg[1]=9; //sensor value
      msg[2]=name; //sensor name
      RCX_send(msg,3);
      len=RCX_receive(recbuf,22);
	  if (len == 0) {
		  cerr<<"Cannot read sensor value"<<endl;
		  return(0);
	  }
      ret_val = RCX_decode_sensor(recbuf,len,value);
	  if (ret_val != len) {
		  cerr<<"Cannot decode sensor value"<<endl;
		  return(0);
	  }
	return(1);
}

int lego::StartMotor(MOTOR motor)
{
	int recv_num;

  msg[0]=toggle(0x21);
  msg[1]=0x80;
  msg[1] += motor;
  RCX_send(msg,2);
  recv_num = RCX_receive(recbuf,16);
  if (recv_num == 0) {
	  cerr<<"Cannot start the motor"<<endl;
	  return(0);
  }
  if (dbg_lvl > 0) {
	  cout<<"Turned on motor"<<endl;
  }
  return(1);
}

int lego::SetMotorDirection(MOTOR motor, DIRECTION direction)
{
  int recv_num;
  msg[0]=toggle(0xe1);
  msg[1] = direction;
  msg[1] += motor;
  RCX_send(msg,2);
  recv_num = RCX_receive(recbuf,24);
  if (recv_num == 0) {
	  cerr<<"Cannot set motor direction"<<endl;
	  return(0);
  }
  if (dbg_lvl > 0) {
	  cout<<"Set motor direction"<<endl;
  }
  return(1);
}

int lego::SetMotorSpeed(MOTOR motor, int speed)
{
  int recv_num;

  if ((speed < 0) || (speed > 7)) {
	  cerr<<"Motor speed out of range; must be 0-7"<<endl;
	  return(0);
  }
  msg[0]=toggle(0x13);
  msg[1]=motor;
  msg[2]=2;
  msg[3]=speed;

  RCX_send(msg,4);
  recv_num = RCX_receive(recbuf,24);
  if (recv_num == 0) {
	  cerr<<"Cannot set motor speed"<<endl;
	  return(0);
  }
  if (dbg_lvl > 0) {
	  cout<<"Set motor speed"<<endl;
  }
  return(1);
}

int lego::StopMotor(MOTOR motor)
{
	int recv_num;

  msg[0]=toggle(0x21);
  msg[1]=0x40;
  msg[1] += motor;
  RCX_send(msg,2);
  recv_num = RCX_receive(recbuf,16);
  if (recv_num == 0) {
	  cerr<<"Cannot stop the motor"<<endl;
	  return(0);
  }
  if (dbg_lvl > 0) {
	  cout<<"Turned off motor"<<endl;
  }
  return(1);
}

int lego::OpenUsbPort()
{

  COMMTIMEOUTS timeout;

  UsbPort = CreateFile(USB_TOWER, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0);

  if (UsbPort == INVALID_HANDLE_VALUE)
  {
	  return(0);
  }

  GetCommTimeouts(UsbPort,&timeout);
  timeout.ReadIntervalTimeout=100;
  timeout.ReadTotalTimeoutConstant=100;
  timeout.ReadTotalTimeoutMultiplier=0;
  timeout.WriteTotalTimeoutConstant=0;
  timeout.WriteTotalTimeoutMultiplier=0;
  SetCommTimeouts(UsbPort,&timeout);
  return(1);
}

int lego::CloseUsbPort()
{
	int result;
	result = CloseHandle(UsbPort);

    return result;
}


int lego::RCX_send(unsigned char *msgage, int length)
{
  //returns 1 if write succeeded, 0 if failed
  //maximal length of msgage is 125 bytes
  int i;
  unsigned long ptr;
  int s=0;
  DWORD res;

  //create msgage to send
  sendbuf[0]=0x55;
  sendbuf[1]=0xff;
  sendbuf[2]=0x00;
  ptr=3;
  for (i=0; i<length; i++){
    sendbuf[ptr++]=msgage[i];
    sendbuf[ptr++]=msgage[i]^0xff;
    s+=msgage[i];
  }
  sendbuf[ptr++]=(unsigned char)s;
  sendbuf[ptr++]=((unsigned char)s)^0xff;

  res=0;
  if(!WriteFile(UsbPort,sendbuf,ptr,&res,0))
  {
	  cerr<<"Unable to write to USB port..."<<endl;
      return(0);
  }

  if (res!=ptr) return(0);
  return(1);
}


void lego::DownloadFirmware()
{
	int recv_num=0;
	char filename[] = "firmware.txt";
	FILE *fp= NULL;
    unsigned long firmware_checksum = 0;
	int byte =0;
	int dataExhaust = false;
	int blk_checksum =0;
	unsigned long byteNum = 0, SeqNum =0;
	unsigned long  DATA_SIZE = 1000;

    cout<<"\nPlease wait while firmware is downloaded\n..";

	// Delete the Firmware
    msg[0] = toggle(0x65);
	msg[1] = 0x01;
	msg[2] = 0x03;
	msg[3] = 0x05;
	msg[4] = 0x07;
	msg[5] = 0x0b;

    RCX_send(msg,6);
    recv_num = RCX_receive(recbuf,24);

	// Start Firmware Download

	if((fp = fopen(filename, "r")) == NULL)
	{
		cout<<"\nThe file \"firmware.txt\" is not found in the current project directory"<<endl;
		Sleep(1000);
		exit (1);
	}

    // Find firmware checksum
	for (; (fscanf(fp, "%2x", &byte)) != EOF ;)
	{
	   firmware_checksum = (firmware_checksum + byte)%65536;   // 65536 = 2^16
	}

    msg[0] = toggle(0x75);
	// Firmware Address
	msg[1] = 0x00;
	msg[2] = 0x80;
	// checksum lsb
	msg[3] = (firmware_checksum)&(0x00FF);
	// checksum msb
	msg[4] = (firmware_checksum>>8)&(0x00FF);

	msg[5] = 0x00;


    RCX_send(msg,6);
    recv_num = RCX_receive(recbuf,24);

	// Transfer Firmware

	fclose(fp);
	fp = fopen(filename, "r");
	while( dataExhaust == false)
	{
		++SeqNum;
		msg[0] = toggle(0x45);

		msg[1] = (SeqNum) & 0x00FF;
		msg[2] =  (SeqNum >> 8) & 0x00FF;
		msg[3] = ( DATA_SIZE) & 0x00FF;
		msg[4] = (DATA_SIZE >> 8) & 0x00FF;

		byteNum = 4;
		blk_checksum = 0;
		for( ; (fscanf(fp, "%2x", &byte) != EOF) ; )
		{
			msg[++byteNum] = byte;
			blk_checksum = (blk_checksum + byte)% 256 ; // 256 = 2^8
			if(byteNum == DATA_SIZE + 4)
				break;
		}

		if(byteNum != DATA_SIZE + 4)
		{
			dataExhaust = true;
			msg[1] = 0x00; // Last Pkt
			msg[2] = 0x00;

			msg[3] = ( byteNum -4) & 0x00FF;
		    msg[4] = ((byteNum -4)>> 8) & 0x00FF;
		}

		msg[++byteNum] = blk_checksum;

		RCX_send(msg, byteNum +1);

		recv_num = RCX_receive(recbuf, 24);
		cout << "..";

	}

	// Unlock Firmware
	msg[0] = toggle(0xa5);
	msg[1] = 0x4c;
	msg[2] = 0x45;
	msg[3] = 0x47;
	msg[4] = 0x4f;
	msg[5] = 0xae;

	RCX_send(msg, 6);
	recv_num = RCX_receive(recbuf, 24);

	if(recv_num == 0)
	{
		cout << "\nThere was problem with Firmware download"<<endl;
		cout << "\nPlease contact your TA Or Try Again" <<endl;
	}
	else
		cout << "\nFirmware download complete."<<endl;

}


int lego::RCX_receive(unsigned char *rcbuf, unsigned long length)
{
  DWORD res=0;

  if (!ReadFile(UsbPort,rcbuf,length,&res,0))
  {
	  cerr<<" Error:: Couldn't read from USB port...Exiting"<<endl;
	  return(0);
  }
  if (dbg_lvl > 0) {
	  cout<<"Received "<<res<<" bytes from RCX"<<endl;
  }

  return(res);
}

int lego::RCX_decode_sensor(unsigned char *recbuf, int len,int &value)
{
//tries to decode bytestream in recbuf up to length len.
//returns nr of decoded bytes
  int pos=0;
  int tmp;

  if (len<3)
  {
	  printf("Error:: Rcv Msg less than 3 Bytes...\n");
	  return(3);
  }
  if (recbuf[pos]!=0x55)
  {
	  printf("Error:: Rcv Header byte 0x55 missing...\n");
	  return(pos);
  }
  if (recbuf[pos+1]!=0xff)
  {
	  printf("Error:: Rcv Header byte 0xff missing...\n");
	  return(pos);
  }
  if(recbuf[pos+2]!=0x00)
  {
	  printf("Error:: Rcv Header byte 0x00 missing...\n");
	  return(pos);
  }

  value = 0;
    switch(recbuf[pos+4]){
  case 0x10:
  case 0x18: //The RCX is alive, return a value of 1
    value=1;
    break;
  case 0x12: //get value
  case 0x1a:
 //   if(recbuf[7]>2) return(7);

    tmp=(((int)recbuf[pos+7])<<8)+recbuf[pos+5];
	if(tmp > 1023)
	{

		tmp ^=0xffff;
		tmp = tmp +1;
		tmp = -tmp;
	}

    value = tmp;

    break;
  case 0x30:
  case 0x38: //get battery power
    value=(((int)recbuf[pos+7])<<8)+recbuf[pos+5];
    break;
  }
  return(len);
}

// end of lego class files