/*
 * WheelCommander Test
 *
 * Copyright 2005 by Noetic Design, Inc.
 * www.nubotics.com
 *
 * Noetic Design grants you the right to use, modify, make derivative works and
 * redistribute this source file provided you do not remove this copyright notice.
 */

import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.io.Display;
import com.ridgesoft.io.I2CMaster;

/**
 * This class provides an example of the Nubotics WheelCommander
 * differential drive controller attached to the IntelliBrain.
 */
public class WCtest {
	I2CMaster i2c;
	int slave_address;
	Display display;
	final static float DVolt = 14.0;
	final static float SVolt = 14.0;
	final static float VDivTol = 0.2;
	final static float ADVolt0 = 1.25;
	final static float ADVolt1 = 2.5;
	final static float ADVolt2 = 3.75;
	final static float ADVolt3 = 5.0;
	final static float ADVoltTol = 0.25;
	int num_failures = 0;

/*
 * WheelCommander constructor
 */

	WCtest()
	{
        display = IntelliBrain.getLcdDisplay();	// Get the LCD display
        display.print(0, "WheelCommander Test");		// Clear first line
        display.print(1, "");							// Clear second line
  	    Log("Connecting to WheelCommander at address 0x20");
		slave_address = (byte)0x20;
        i2c = IntelliBrain.getI2CMaster();
		try
		{
			i2c.setFrequency(400000);
			Log("I2C Frequency set to 400KHz");
		}
		catch (Exception e)
		{
			Log("Error setting I2C frequency; defaulting to 100KHz");
		}
	}

	void Log(String string)
	{
		System.out.println(string);
        display.print(1, string);
    }

/*
 * Support Methods for Packing and Unpacking buffers
 */
	byte[] buildCmd(char cmd)
	{
		byte[] ret = new byte[1];
		ret[0] = (byte)cmd;
		return ret;
	}

	byte[] buildByteCmd(char cmd, byte param)
	{
		byte[] ret = new byte[2];
		ret[0] = (byte)cmd;
		ret[1] = param;
		return ret;
	}

	byte[] buildWordCmd(char cmd, short param)
	{
		byte[] ret = new byte[3];
        ret[0] = (byte)cmd;
		ret[1] = (byte)(param & 0x0ff);
		ret[2] = (byte)((param >> 8) & 0x0ff);
		return ret;
	}

	String dumpbytes(byte[] bytes)
	{
		String ret = "";
		for (int i = 0; i < bytes.length; i++)
			ret += Integer.toHexString(bytes[i] & 0x0ff) + "h ";
		return ret;
	}

	byte[] buildDWordCmd(char cmd, int param)
	{
		byte[] ret = new byte[5];
        ret[0] = (byte)cmd;
		ret[1] = (byte)(param & 0x0ff);
		ret[2] = (byte)((param >> 8) & 0x0ff);
		ret[3] = (byte)((param >> 16) & 0x0ff);
		ret[4] = (byte)((param >> 24) & 0x0ff);
		//Log("dword cmd: " + dumpbytes(ret));
		return ret;
	}		

	byte extractByteParam(char cmd, byte[] buf) throws Exception
	{
		if ((byte)cmd == buf[0])
			return buf[1];
        else
			throw new Exception("bad command return");
	}

	short extractWordParam(char cmd, byte[] buf) throws Exception
	{
		if ((byte)cmd == buf[0])
		{
			short ret = (short)(((int)buf[1] & 0x0ff) + (((int)buf[2] << 8) & 0x00ff00));
			//Log("buf[1] = " + buf[1] + "; buf[2] = " + buf[2] + "; short = " + ret);
			return ret;
		}
		else
			throw new Exception("bad command return");
	}		

	short extractWordXParam(char cmd, byte val, byte[] buf) throws Exception
	{
		if (((byte)cmd == buf[0]) && (val == buf[1]))
		{
			return (short)(((int)buf[2] & 0x0ff) + (((int)buf[3] << 8) & 0x00ff00));
		}
		else
		{
			Log("extractWordXParam: missmatch; cmd = " + cmd + ", buf[0] = " + (char)buf[0] + "; val = " + val + ", buf[1] = " + buf[1] );
			throw new Exception("bad command return");
		}
	}		

	int extractDWordParam(char cmd, byte[] buf) throws Exception
	{
		if ((byte)cmd == buf[0])
		{
			return ((int)buf[1] & 0x0ff) + (((int)buf[2] << 8) & 0x00ff00) + (((int)buf[3] << 16) & 0x0ff0000) + (((int)buf[4] << 24) & 0xff000000);
		}
		else
			throw new Exception("bad command return");
	}		

/*
 * Support Methods for Executing Commands
 */
	boolean doCmd(char cmdChar, char expected)
	{
		byte[] cmd = buildCmd(cmdChar);
		byte[] ret = new byte[1];
		String error = "";
		try
		{
			i2c.transfer(slave_address, cmd, null);	// write the command; read the status seperately, as there could be a long delay while the unit does not respond
		}
		catch (Exception e)
		{
			Log("error sending command " + cmdChar);
			return false;
		}

		for (int i = 1; i < 10; i++)
		{
			try
			{
				if (i > 1)
					Thread.sleep(10);
				i2c.transfer(slave_address, null, ret);
				if (ret[0] == (byte)expected)
				{
					//if (i > 1)
					//	Log("doCmd took " + i + " tries to get result of cmd " + cmdChar + " = " + (char)ret[0]);
					return true;
				}
				else
				{
					Log("bad doCmd return; expected " + expected + ", got " + (char)ret[0]);
					throw new Exception("bad cmd return = " + ret[0]);
				}
			}
			catch (Exception e)
			{
				error = "doCmd Exception: " + e.toString();
			}
		}
		return false;
	}

	boolean doSetByteByteCmd(char cmdChar, byte val1, byte val2)
	{
		byte[] cmd = new byte[3];
		cmd[0] = (byte)cmd;
		cmd[1] = val1;
		cmd[2] = val2;
		byte[] ret = new byte[1];
		try
		{
			i2c.transfer(slave_address, cmd, ret);
			if (ret[0] == 'a')
				return true;
			else
			{
				Log("bad doSetByteByteCmd return; expected 'a', got '" + (char)ret[0] + "'");
				throw new Exception("bad cmd return = " + ret[0]);
			}
		}
		catch (Exception e)
		{
			Log("doSetByteByteCmd Exception: " + e.toString());
			return false;
		}
	}

	boolean doSetWordCmd(char cmdChar, short value)
	{
		byte[] cmd = buildWordCmd(cmdChar, value);
		byte[] ret = new byte[1];
		try
		{
			i2c.transfer(slave_address, cmd, ret);
			if (ret[0] == 'a')
				return true;
			else
			{
				Log("bad doSetWordCmd return; expected 'a', got '" + (char)ret[0] + "'");
				throw new Exception("bad cmd return = " + ret[0]);
			}
		}
		catch (Exception e)
		{
			Log("doSetWordCmd Exception: " + e.toString());
			return false;
		}
	}

	boolean doSetDWordCmd(char cmdChar, int value)
	{
		byte[] cmd = buildDWordCmd(cmdChar, value);
		byte[] ret = new byte[1];
		try
		{
            ret[0] = 0;
			i2c.transfer(slave_address, cmd, ret);
			if (ret[0] == 'a')
				return true;
			else
			{
				Log("bad doSetDWordCmd return; expected 'a', got '" + (char)ret[0] + "'");
				throw new Exception("bad cmd return = " + ret[0]);
			}
		}
		catch (Exception e)
		{
			Log("doSetDWordCmd Exception: " + e.toString());
			return false;
		}
	}

	short doGetWordCmd(char cmdChar)
	{
		byte[] cmd = buildCmd(cmdChar);
		byte[] ret = new byte[3];
		try
		{
			i2c.transfer(slave_address, cmd, ret);
			return extractWordParam(cmdChar, ret);
		}
		catch (Exception e)
		{
			Log("doGetWordCmd Exception: " + e.toString());
			return 0;
		}
	}

	int doGetDWordCmd(char cmdChar)
	{
		byte[] cmd = buildCmd(cmdChar);
		byte[] ret = new byte[5];
		try
		{
			i2c.transfer(slave_address, cmd, ret);
			return extractDWordParam(cmdChar, ret);
		}
		catch (Exception e)
		{
			Log("doGetDWordCmd Exception: " + e.toString());
			return 0;
		}
	}

	byte doGetByteByteCmd(char cmdChar, byte val)
	{
		byte[] cmd = buildByteCmd(cmdChar, val);
		byte[] ret = new byte[3];
		try
		{
			ret[0] = 'X';
			i2c.transfer(slave_address, cmd, null);
			i2c.transfer(slave_address, null, ret);
			if (ret[0] == cmdChar)
			{
				if (ret[1] == val)
					return ret[2];
			}
			throw new Exception("bad cmd return = " + ret[0] + "; val = " + ret[1]);
			return 0;
		}
		catch (Exception e)
		{
			Log("doGetByteByteCmd Exception: " + e.toString() + "; first ret byte: " + Integer.toHexString(ret[1]) + "; second ret byte: " + Integer.toHexString(ret[2]));
			return 0;
		}
	}

	short doGetWordByteCmd(char cmdChar, byte val)
	{
		byte[] cmd = buildByteCmd(cmdChar, val);
		byte[] ret = new byte[3];
		try
		{
			ret[0] = 'X';
			i2c.transfer(slave_address, cmd, null);
			i2c.transfer(slave_address, null, ret);
			//i2c.transfer(slave_address, cmd, ret);
			return extractWordParam(cmdChar, ret);
		}
		catch (Exception e)
		{
			Log("doGetWordByteCmd Exception: " + e.toString() + "; first ret byte: " + (char)ret[0]);
			return 0;
		}
	}

	short doGetWordByteXCmd(char cmdChar, byte val)
	{
		byte[] cmd = buildByteCmd(cmdChar, val);
		byte[] ret = new byte[4];
		try
		{
			ret[0] = 'X';
			ret[1] = 'Y';
			//Log("doing get word cmd with byte param: " + cmdChar + ", param = " + val);
			i2c.transfer(slave_address, cmd, null);
			i2c.transfer(slave_address, null, ret);
			return extractWordXParam(cmdChar, val, ret);
		}
		catch (Exception e)
		{
			Log("doGetWordByteXCmd Exception: " + e.toString() + "; first ret byte: " + (char)ret[0]);
			return 0;
		}
	}

/*
 * WheelCommander Commands
 */
	boolean doSync()
	{
		return doCmd('.', '.');
	}

	boolean doEcho(byte value)
	{
		byte[] cmd = buildByteCmd('E', value);
		byte[] ret = new byte[2];
		try
		{
			i2c.transfer(slave_address, cmd, ret);
			byte swapped = (byte)(((value >> 4) & 0x0f) | ((value << 4) & 0xf0));
			if (extractByteParam('E', ret) == swapped)
				return true;
			else
				return false;
		}
		catch (Exception e)
		{
			Log("doEcho Exception: " + e.toString());
			return false;
		}
	}

	String doName()
	{
		byte[] cmd = buildCmd('N');
		byte[] ret = new byte[5];
		try
		{
			i2c.transfer(slave_address, cmd, ret);
			return new String(ret, 0, 5);
		}
		catch (Exception e)
		{
			Log("doName Exception: " + e.toString());
			return null;
		}
	}

	boolean doSetConstant(byte address, byte value)
	{
		short both = (short)(address + (value << 8));
		return doSetWordCmd('F', both);
	}

	byte doGetConstant(byte address) throws Exception
	{
		short both = doGetWordByteCmd('F', address);
		if ((byte)(both & 0x0ff) == address)
		{
			return (byte)((both >> 8) & 0x0ff);
		}
		else
			throw new Exception("doGetConstant: bad address returned");
	}

	boolean doVelocity(short velocity)
	{
		return doSetWordCmd('V', velocity);
	}

	short getVelocity()
	{
		return doGetWordCmd('V');
	}

	boolean doAcceleration(short accel)
	{
		return doSetWordCmd('A', accel);
	}

	short getAcceleration()
	{
		return doGetWordCmd('A');
	}

	boolean doRotationRate(short rate)
	{
		return doSetWordCmd('Y', rate);
	}

	short getRotationRate()
	{
		return doGetWordCmd('Y');
	}

	boolean doAngle(short angle)
	{
		return doSetWordCmd('W', angle);
	}

	short getAngle()
	{
		return doGetWordCmd('W');
	}

	boolean doDistance(int distance)
	{
		return doSetDWordCmd('X', distance);
	}

	int getDistance()
	{
		return doGetDWordCmd('D');
	}

	boolean doGo()
	{
		return doCmd('G', 'a');
	}

	boolean doCoast()
	{
		return doCmd('C', 'a');
	}

	boolean doBrake()
	{
		return doCmd('B', 'a');
	}

	boolean doReset()
	{
		return doCmd('R', 'a');
	}

	float getADVoltage(byte channel)
	{
		short ADval = doGetWordByteXCmd('H', channel);
 	    return (float)(ADval * 5.0 / 1024.0);
	}

	float getDigitalVoltage()
	{
		float ADval = getADVoltage((byte)6); 
 	    return (float)(ADval * 40.01 / 10);
	}

	float getServoVoltage()
	{
		float ADval = getADVoltage((byte)7); 
 	    return (float)(ADval * 40.01 / 10);
	}

	boolean getDigitalInput(int bit)
	{
		return doGetByteByteCmd('J', bit);
	}

	boolean setDigitalOutput(int bit, boolean level)
	{
		return doSetByteByteCmd('J', bit, level);
	}

	boolean WaitDone() throws Exception
	{
		short status;
		short prev_status = 0;

		for (int i = 0; i < 150; i++)
		{
			status = doGetWordCmd('S');
			status &= 0x0fff;
			if (status != prev_status)
			{
				Log("status = " + Integer.toHexString(status & 0x0ffff));
				prev_status = status;
			}
			if ((status & 0x10) != 0) // dst done is true now
				return true;
			Thread.sleep(200);
		}
		return false; // timeout
	}


//********************************************
// Main Entry Point
//
//********************************************
    public static void main(String args[]) {

        try {
			WCtest wc = new WCtest();
			int i;

// connect to the WheelCommander
			wc.Log("Trying sync:");
			if (wc.doSync())
			{
				wc.Log("Got sync; trying echo:");

// confirm communications
				if (wc.doEcho((byte)0x5b))
				{
					wc.Log("Echo worked.  Connected!");

// check device type
					String name = wc.doName();
					wc.Log("Device name and version: " + name);

// clear operating conditions
					wc.Log("Resetting motion.");
					wc.doReset();

// display useful info
					byte mode = wc.doGetConstant((byte)2);
					wc.Log("Current mode = " + Integer.toHexString(mode & 0x0ff));
					float voltage = wc.getDigitalVoltage();
					wc.Log("Digital Supply Voltage = " + voltage);
					if ((voltage < (DVolt - VDivTol)) || (voltage > (DVolt + VDivTol)))
					{
						failures++;
						wc.Log("***** DVolt test failed.");
					}
					voltage = wc.getServoVoltage();
					if ((voltage < (SVolt - VDivTol)) || (voltage > (SVolt + VDivTol)))
					{
						failures++;
						wc.Log("***** SVolt test failed.");
					}
					wc.Log("Servo Supply Voltage = " + voltage);

// test digital I/O
					if (!setDigitalOutput(0, 0))	// connects to DIO2
						failures++;
					if (!setDigitalOutput(1, 0))	// connects to DIO3
						failures++;
					if (getDigitalInput(2, 0)) // should be zero
						failures++;
					if (getDigitalInput(3, 0)) // should be zero
						failures++;

					if (!setDigitalOutput(0, 1))	// connects to DIO2
						failures++;
					if (!getDigitalInput(2, 0)) // should be one
						failures++;
					if (getDigitalInput(3, 0)) // should be zero
						failures++;
					
					if (!setDigitalOutput(0, 0))	// connects to DIO2
						failures++;
					if (!setDigitalOutput(1, 1))	// connects to DIO3
						failures++;
					if (getDigitalInput(2, 0)) // should be zero
						failures++;
					if (!getDigitalInput(3, 0)) // should be one
						failures++;

// test analog I/O
					voltage = getADVoltage(0);
					if ((voltage < (ADVolt0 -ADVoltTol)) || (voltage > (ADVolt0 + ADVoltTol)))
						failures++;
					voltage = getADVoltage(1);
					if ((voltage < (ADVolt1 -ADVoltTol)) || (voltage > (ADVolt1 + ADVoltTol)))
						failures++;
					voltage = getADVoltage(2);
					if ((voltage < (ADVolt2 -ADVoltTol)) || (voltage > (ADVolt2 + ADVoltTol)))
						failures++;
					voltage = getADVoltage(3);
					if ((voltage < (ADVolt3 -ADVoltTol)) || (voltage > (ADVolt3 + ADVoltTol)))
						failures++;

// demonstrate velocity control
					for (short vel = 20; vel < 70; vel += 5)
					{
						wc.Log("Setting velocity to " + vel);
						wc.doVelocity(vel);
						wc.doGo();
						//Thread.sleep(1);
						wc.Log("vel = " + wc.getVelocity() + ", dist = " + wc.getDistance());
					}
					wc.Log("Coasting.");
					wc.doCoast();

// demonstrate position and angle control
					wc.doAcceleration((short)50);
					wc.doVelocity((short)50);
					for (int i = 0; i < 10; i++)
					{
						wc.Log("Turning 90 degrees");
						wc.doAngle((short)90);
						wc.Log("Set angle; setting distance");
						wc.doDistance(0);
						wc.Log("Set distance; starting go");
						wc.doGo();
						wc.Log("Going...");
						if (!wc.WaitDone())
							wc.Log("Timeout waiting for completion");
						wc.doCoast();
						wc.Log("dist = " + wc.getDistance() + ", ang = " + wc.getAngle());
						wc.Log("Going 12 inches");
						wc.doDistance(120);
						wc.doAngle((short)0);
						wc.doGo();
						if (!wc.WaitDone())
							wc.Log("Timeout waiting for completion");
						wc.doCoast();
						wc.Log("dist = " + wc.getDistance() + ", ang = " + wc.getAngle());
					}
					wc.doCoast();
				}
			}
			else
				num_failures++;
        } catch (Throwable t) {
            t.printStackTrace();							// Display a stack trace for any error
			num_failures++;
        }
		if (num_failures)
			wc.Log("FAILED");
		else
			wc.Log("PASSED");
    }
}