/************************************************************************/
/*                                                                      */
/* Program Name: NXTServo-disp-macro.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     11/10/08  Initial Authoring.
 */


#define PORT S1
#include "NXTServo-lib.nxc"
const byte ServoAddr = 0xb0;

//Print sensor information from its register
void MySensorInfo(byte prt, byte Addres )
{

	string msg;
	msg = "Address= ";
	msg += NXTServo_format_hex(Addres);
	if( NXTServo_i2cread(prt,Addres,0x00,1)>1)
	{
		ClearScreen();
		TextOut(0, LCD_LINE1, msg);
		TextOut(0, LCD_LINE2, i2cReadString(prt, Addres, 0x00, 8));
		TextOut(0, LCD_LINE3, i2cReadString(prt, Addres, 0x08, 8));
		TextOut(0, LCD_LINE4, i2cReadString(prt, Addres, 0x10, 8));
		TextOut(0, LCD_LINE5, "Press btn.");
		until(ButtonPressed(BTNCENTER, true));

	}
}

string NXTServo_printable(byte b)
{
  string s;
	byte az[27];
	byte n;

	az[0] = "A";
	//BCDEFGHIJKLMNOPQRSTUVWXYZ";

  if ( b >= 0x30 && b <= 0x39 ) {
	  s = NumToStr(b - 0x30);
		s += "_";
		return s;
	}
	switch (b) {
	case 0x41:
		s = "A_";
		break;
	case  0x42:
		s = "B_";
		break;
	case  0x43:
		s = "C_";
		break;
	case  0x44:
		s = "D_";
		break;
	case  0x45:
		s = "E_";
		break;
	case  0x46:
		s = "F_";
		break;
	case  0x47:
		s = "G_";
		break;
	case  0x48:
		s = "H_";
		break;
	case  0x49:
		s = "I_";
		break;
	case  0x4A:
		s = "J_";
		break;
	case  0x4B:
		s = "K_";
		break;
	case  0x4C:
		s = "L_";
		break;
	case  0x4D:
		s = "M_";
		break;
	case  0x4E:
		s = "N_";
		break;
	case  0x4F:
		s = "O_";
		break;
	case  0x50:
		s = "P_";
		break;
	case  0x51:
		s = "Q_";
		break;
	case  0x52:
		s = "R_";
		break;
	case  0x53:
		s = "S_";
		break;
	case  0x54:
		s = "T_";
		break;
	case  0x55:
		s = "U_";
		break;
	case  0x56:
		s = "V_";
		break;
	case  0x57:
		s = "W_";
		break;
	case  0x58:
		s = "X_";
		break;
	case  0x59:
		s = "Y_";
		break;
	case  0x5A:
		s = "Z_";
		break;
	case  0x7B:
		s = "{_";
		break;
	case  0x7D:
		s = "}_";
		break;
	case  0x3C:
		s = "<_";
		break;
	case  0x3D:
		s = "=_";
		break;
	case  0x3E:
		s = ">_";
		break;
	case  0x3F:
		s = "?_";
		break;
	default:
		return NXTServo_format_hex(b);
	}
	return s;
}

void NXTServo_DisplayMacro (byte sensorPort, byte addr, byte loc, byte lines)
{
	byte i, j;
	int y;
  byte message[20], buf[20];
	int count;
  byte nByteReady = 0;
	string msg, x;
	byte line_loc[10];
	line_loc[0] = LCD_LINE1;
	line_loc[1] = LCD_LINE2;
	line_loc[2] = LCD_LINE3;
	line_loc[3] = LCD_LINE4;
	line_loc[4] = LCD_LINE5;
	line_loc[5] = LCD_LINE6;
	line_loc[6] = LCD_LINE7;
	line_loc[7] = LCD_LINE8;

	for ( j = 0; j < lines; j++) {
		y = (j*8)+loc;
		ArrayBuild(message, addr, y);
		while (I2CStatus(sensorPort, nByteReady) ==  STAT_COMM_PENDING);
		count = 8;
		if(I2CBytes(sensorPort, message, count, buf)) {
			msg = "                    ";
			for ( i=0; i < 8; i++ ) {
				x = NXTServo_printable(buf[i]);
				msg = StrReplace(msg, i*2, x);
				TextOut(0, line_loc[j], msg, false);
			}
		}
	}
}

void NXTServo_ExitMacroEditMode(byte port) {
  byte cmdBuf[];
  ArrayBuild(cmdBuf, 0x40, 0x00, 'Q');
  I2CWrite(port, 0, cmdBuf);
  int status = I2CCheckStatus(port);
  while (status > NO_ERR)
    status = I2CCheckStatus(port);
  Wait(200);
}

task main()           // main task:
{
  byte Address=0x02;
  SetSensorLowspeed(PORT);

	ShowSensorInfo(PORT,0xb0);

	TextOut(0, LCD_LINE5, "changing mode....");
	TextOut(0, LCD_LINE6, "Press Btn.");
	until(ButtonPressed(BTNCENTER, true));
	Wait (200);
  NXTServo_SendCommand(PORT, 0xb0, 'E');
  NXTServo_SendCommand(PORT, 0xb0, 'M');

  NXTServo_DisplayMacro(PORT, 0x40, 0x21, 7);
	Wait (200);
  TextOut(0, LCD_LINE8, "pg-1 - prss btn.", false);
  until(ButtonPressed(BTNCENTER, true));

  NXTServo_DisplayMacro(PORT, 0x40, 0x21+(8*7), 7);
	Wait (200);
  TextOut(0, LCD_LINE8, "pg-2 - prss btn.", false);
  until(ButtonPressed(BTNCENTER, true));

  NXTServo_DisplayMacro(PORT, 0x40, 0x21+(8*14), 7);
	Wait (200);
  TextOut(0, LCD_LINE8, "pg-3 - prss btn.", false);
  until(ButtonPressed(BTNCENTER, true));

  NXTServo_DisplayMacro(PORT, 0x40, 0x21+(8*21), 7);
	Wait (200);
  TextOut(0, LCD_LINE8, "pg-4 - prss btn.", false);
  until(ButtonPressed(BTNCENTER, true));

  while(Address<0xFE)
  {
    MySensorInfo(PORT,Address);
    Address=Address+0x02;
  }

	NXTServo_ExitMacroEditMode(PORT);
  TextOut(0, LCD_LINE8, "prss btn.", false);
  until(ButtonPressed(BTNCENTER, true));

}