/************************************************************************/
/*                                                                      */
/* Program Name: NXTServo-lib.nxc                                       */
/* ===========================                                          */
/*                                                                      */
/* Copyright (c) 2008 by mindsensors.com                                */
/* Email: info (<at>) mindsensors (<dot>) com                           */
/*                                                                      */
/* This program is free software. You can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; version 3 of the License.              */
/* Read the license at: http://www.gnu.org/licenses/gpl.txt             */
/*                                                                      */
/************************************************************************/


/*
 * History
 * ------------------------------------------------
 * Author     Date      Comments
 * Deepak     08/27/08  Initial Authoring.
 */

const int NXTServo_VBatt        = 0x41;
const int NXTServo_SPEED        = 0x52;

string NXTServo_format_hex ( byte loop ) {
  int j;
  string s;
  int b = 0xF0;
  int a = 0x0F;

  j = (loop&b)>>4;

  s = "";
  switch ( j ) {
  case 1:
      s += "1";
      break;
  case 2:
      s += "2";
      break;
  case 3:
      s += "3";
      break;
  case 4:
      s += "4";
      break;
  case 5:
      s += "5";
      break;
  case 6:
      s += "6";
      break;
  case 7:
      s += "7";
      break;
  case 8:
      s += "8";
      break;
  case 9:
      s += "9";
      break;
  case 10:
      s += "A";
      break;
  case 11:
      s += "B";
      break;
  case 12:
      s += "C";
      break;
  case 13:
      s += "D";
      break;
  case 14:
      s += "E";
      break;
  case 15:
      s += "F";
      break;
  case 16:
      s += "0";
      break;
  case 0:
      s += "0";
      break;
  }

  j = loop&a;

  switch ( j ) {
  case 1:
      s += "1";
      break;
  case 2:
      s += "2";
      break;
  case 3:
      s += "3";
      break;
  case 4:
      s += "4";
      break;
  case 5:
      s += "5";
      break;
  case 6:
      s += "6";
      break;
  case 7:
      s += "7";
      break;
  case 8:
      s += "8";
      break;
  case 9:
      s += "9";
      break;
  case 10:
      s += "A";
      break;
  case 11:
      s += "B";
      break;
  case 12:
      s += "C";
      break;
  case 13:
      s += "D";
      break;
  case 14:
      s += "E";
      break;
  case 15:
      s += "F";
      break;
  case 16:
      s += "0";
      break;
  case 0:
      s += "0";
      break;
  } 
  return (s);
}


// read data from sensor register(s)
int NXTServo_i2cread(byte prt, byte adr, byte reg, byte cnt)
{
  int result = -1;             // default: error value
  byte outbuf[];               // here we will get data
  byte cmdbuf[];               // register number holder

  ArrayBuild(cmdbuf, adr, reg);
  if(I2CBytes(prt, cmdbuf, cnt, outbuf))
  {
    result = outbuf[0];       // read value

    if(cnt==2)
      result = result + outbuf[1]*256;
// if 2 registers (16 bit), then add the MSB part
  }

  return result;              // returns -1 if I2CBytes failed
}

string i2cReadString(byte port, byte adr, byte reg, byte cnt)
{
  byte outbuf[];
  byte cmdbuf[];
  string temp = "E";
  
  ArrayBuild(cmdbuf, adr, reg);
  byte nByteReady = 0;
  while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING)
  {
  // Wait for I2C bus to be ready
  }
  
  if(I2CBytes(port, cmdbuf, cnt, outbuf))
  {
    temp = ByteArrayToStr(outbuf);
  }
  return temp;
}


/*===================================
**
** Set the speed of a servo to
** given value.
**
===================================*/

void NXTServo_SetSpeed(byte port, byte addr, byte servoNumber, byte speed)
{

  byte cmdbuf[];
  byte buf[];
  byte nByteReady = 0;
	byte loop;
	int  n;

  SetSensorLowspeed(port);
	n = NXTServo_SPEED + servoNumber - 1;
  // Build the I2C message
  ArrayBuild(cmdbuf, addr, n, speed);

  loop = STAT_COMM_PENDING;
  while ( loop == STAT_COMM_PENDING ) {
    loop = I2CStatus(port, nByteReady);
  }

  // when the I2C bus is ready, send the message you built
  n = I2CWrite(port, 1, cmdbuf);
}

/*===================================
**
** Quick setup for servo.
** Valid values are from 50 to 250
** uses pseudo registers from 0x1A to 0x21
**
===================================*/
void NXTServo_Quick_Servo_Setup(byte port, byte addr, int servoNumber, int position)
{
  byte cmdbuf[];
  byte buf[];
  byte nByteReady = 0;
	byte loop;
	int  n;

  SetSensorLowspeed(port);

  // Build the I2C message
	n = 0x59 + servoNumber;

  ArrayBuild(cmdbuf, addr, n, position);

  loop = STAT_COMM_PENDING;
  while ( loop == STAT_COMM_PENDING ) {
    loop = I2CStatus(port, nByteReady);
  }

  // when the I2C bus is ready, send the message you built
  n = I2CWrite(port, 1, cmdbuf);

}

/*===================================
**
** Set the position of a servo to
** given value.
**
===================================*/
void NXTServo_SetPosition(byte port, byte addr, int servoNumber, int position)
{
  NXTServo_Quick_Servo_Setup(port, addr, servoNumber, position/10);
}


/*===================================
**
** Read the battery voltage data from
** NXTServo module (in mili-volts)
**
===================================*/
int  NXTServo_GetBattVoltage(byte port, byte addr)
{
  byte cmdbuf[];
  byte buf[];
  byte nByteReady = 0;
	byte loop, n;

  SetSensorLowspeed(port);
  ArrayBuild(cmdbuf, addr, NXTServo_VBatt);

  loop = STAT_COMM_PENDING;
  while ( loop == STAT_COMM_PENDING ) {
    loop = I2CStatus(port, nByteReady);
  }

  n = I2CWrite(port, 1, cmdbuf);

  while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);
  n = I2CRead(port, 1, buf);

	if ( n == NO_ERR ) {
		return(37*(0x00FF & buf[0]));  // 37 is calculated from
														//supply from NXT =4700 mv /128
  } else {
    return (0);
	}
}

void ShowSensorInfo(byte addr, byte prt)
{
  SetSensorLowspeed(prt);
  ClearScreen();

  TextOut(0, LCD_LINE1, i2cReadString(prt, addr, 0x00, 8), false);
  TextOut(35, LCD_LINE1, i2cReadString(prt, addr, 0x08, 8), false);
  TextOut(0, LCD_LINE2, i2cReadString(prt, addr, 0x10, 8), false);
}

void NXTServo_SendCommand(byte port, byte addr, byte cmd) {
  byte cmdBuf[];
  ArrayBuild(cmdBuf, addr, 0x41, cmd);
  I2CWrite(port, 0, cmdBuf);
  int status = I2CCheckStatus(port);
  while (status > NO_ERR)
    status = I2CCheckStatus(port);
}