/*
 * 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 WheelCommander {
    I2CMaster i2c;
    int slave_address;
    Display display;

/*
 * WheelCommander constructor
 */

    WheelCommander()
    {
        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 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;
        }
    }

    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 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 {
            WheelCommander wc = new WheelCommander();

// 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("Digtal Supply Voltage = " + voltage);
                    voltage = wc.getServoVoltage();
                    wc.Log("Servo Supply Voltage = " + voltage);

// 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();
                }
            }
        } catch (Throwable t) {
            t.printStackTrace();                            // Display a stack trace for any error
        }
    }
}