package iodevices.polhemus;

import gnu.io.CommPortIdentifier;
import gnu.io.PortInUseException;
import gnu.io.RXTXCommDriver;
import gnu.io.SerialPort;
import gnu.io.SerialPortEvent;
import gnu.io.SerialPortEventListener;
import gnu.io.UnsupportedCommOperationException;

import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.util.ArrayList;
import java.util.Enumeration;
import java.util.StringTokenizer;
import java.util.Timer;
import java.util.TooManyListenersException;

import maspack.matrix.Point3d;
import maspack.matrix.Quaternion;
import maspack.matrix.RigidTransform3d;
import maspack.matrix.RotationMatrix3d;
import artisynth.core.util.FunctionTimer;


public class Latus implements Runnable, SerialPortEventListener {
    static CommPortIdentifier portId;
    static Enumeration portList;

	//TODO get cube and head number from CubeeMain - these values should be settable
    public static final int cubeMarkerNumber = 4;
    public static final int headMarkerNumber = 8;
    public static final int anchorReceptorNumber = 1;
    
    public static final String toHalfRateCommand				= "^R3\r";	
    public static final String toThroughputModeCommand				= "R3\r";	
    public static final String toMetricCommand				= "U1\r";	
    public static final String toEnglishCommand				= "U0\r";	
    public static final String toBinaryCommand				= "F1\r";
    public static final String toASCIICommand				= "F0\r";
    public static final String toContinuousModeCommand	 	= "C\r";
    public static final String retrieveStatusCommand		= "S\r";
    public static final String toSingleRecordModeCommand	= "P";
    public static final String CRCommand				    = "\r";
    public static final String systemResetCommand		    = "\u0019\r";
    public static final String systemInfoCommand		    = "\u0016\r";
    public static final String autoLaunchOnCommand		    = "@A1\r";
    public static final String autoLaunchOffCommand		    = "@A0\r";
    //Euler angle representation:
    public static final String outputFormatEulerCommand		= "O*,2,4,1\r";
    //Quaternion anlge representation:
    public static final String outputFormatQuaternionCommand = "O*,2,7,1\r";
    public static final String phaseStepCubeMarkerCommand 	= "\u0010" + cubeMarkerNumber + "\r";
    public static final String phaseStepHeadMarkerCommand 	= "\u0010" + headMarkerNumber + "\r";
// NB - launch command uses repector number not marker number:
//    public static final String launchCubeMarkerCommand 		= "L" + cubeMarkerNumber + "\r";
//    public static final String launchHeadMarkerCommand 		= "L" + headMarkerNumber + "\r";
    public static final String launchMarkerCommand 			= "L" + anchorReceptorNumber + "\r";
    public static final String unLaunchCubeMarkerCommand 	= "\u000c" + cubeMarkerNumber + "\r";
    public static final String unLaunchHeadMarkerCommand 	= "\u000c" + headMarkerNumber + "\r";
    public static final String setCubeAsReferenceCommand	= "G0\r";
    public static final String setUnitsCentimeterCommand	= "U1\r";

    // length using Euler angle representation
    public static final int singleMarkerLengthEuler = 57;
    // length using Quaternion angle representation
    public static final int singleMarkerLengthQuaternion = 66;
    public static final int messageLength = 0;
    
    public static final double DEG_TO_RAD = Math.PI/180.0;
    
	public static final int  DEFAULT_BAUDRATE= 115200;
	public ArrayList<Trackable> callback;
	public static boolean debug = true;
	public static boolean profile=true;
	public static FunctionTimer timer = new FunctionTimer();
	public static final int NUMTIMEINGS= 10;
	public static double[] timings = new double[NUMTIMEINGS];
	static int timeID=0;
	Point3d eyePos;
	Quaternion cubeOrientation;
	RigidTransform3d cubePose;
	boolean newCubePose = false;
	boolean newEyePos = false;
	
	protected boolean autoLaunchP;

	boolean aligningMarkersP = false;
	int alignMarkerNumber;
	
	boolean launchingMarkersP = false;
	int launchMarkerNumber;
	
	protected boolean latusReadyP = false;
	protected boolean usingEulerP = false;
	
	boolean isRunning;
    SerialPort serialPort;
    Thread readThread;
    InputStream inputStream;
    OutputStream outputStream;
    int numSensors=4;
    int recordLength=Latus.singleMarkerLengthEuler;
    int recordTokens=8;
    int recordValues=6;
    int positionOffset=3;
    int recordStringWidth=7;
    
    FlushLatusBuffer flusher;
    public static boolean flusherOnP = false;
    MarkerAutoAligner aligner;
    MarkerLauncher launcher;
    
    public Latus()  throws IOException,PortInUseException, 
    TooManyListenersException, UnsupportedCommOperationException
    {
       this(DEFAULT_BAUDRATE, true);
    }
    
    public Latus(int baud)  throws IOException,PortInUseException, 
    TooManyListenersException, UnsupportedCommOperationException
    {
       this(baud, true);
    }
    
    public Latus(int baud, boolean doAutoLaunch)  throws IOException,PortInUseException, 
    TooManyListenersException, UnsupportedCommOperationException
    {
       
       //TODO - test that serial cable to Latus is connected

        setLatusReady(false);
        autoLaunchP = doAutoLaunch;
        
        System.out.println("Starting Latus tracker...");

        callback = new ArrayList <Trackable>();
        eyePos = new Point3d();
        cubePose = new RigidTransform3d();
        cubeOrientation = new Quaternion();

//		RXTXCommDriver TxPort = new RXTXCommDriver();
////		System.out.print("open Ports\n");		
//		serialPort = (SerialPort) TxPort.getCommPort("/dev/ttyS0", CommPortIdentifier.PORT_SERIAL);
//
//		//		System.out.print("Get Streams\n");		

        
		try {
		   serialPort = (SerialPort) portId.open("LatusReader", 100);
		} catch (PortInUseException e) {e.printStackTrace();}
		try {
			inputStream = serialPort.getInputStream();
			outputStream = serialPort.getOutputStream();
			//outputStream.flush();

			//XXX reset causes IOException: mark/reset not supported
			//inputStream.reset();
		} catch (Exception e) {e.printStackTrace();}
		try {
			serialPort.addEventListener(this);
		} catch (TooManyListenersException e) {e.printStackTrace();}
		serialPort.notifyOnDataAvailable(true);
		try {
		   serialPort.setSerialPortParams(baud, SerialPort.DATABITS_8, SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
		   
		   serialPort.setFlowControlMode(SerialPort.FLOWCONTROL_NONE);
		   System.out.println(serialPort.getFlowControlMode() +"\n");
		} catch (UnsupportedCommOperationException e) {e.printStackTrace(); }

		try {
		   // clear buffer
		   sendCommand(CRCommand);
		   Thread.sleep(1000);
		} catch (Exception e) {e.printStackTrace();}
		
		readThread = new Thread(this);
		//readThread.setPriority(Thread.MIN_PRIORITY);
		readThread.start();
		try{
			recordLength = Latus.messageLength;
		    Thread.sleep(1000);
		    System.out.println("Resetting Latus Tracker...");
			sendCommand(systemResetCommand);
			Thread.sleep(8000);
			//TODO - check that we receive the correct message back after reset and that markers are aligned
			
//			sendCommand(systemInfoCommand);
//			Thread.sleep(1000);

			//TODO - save initialization commands in Latus (should be able to save up to three commands)
			sendCommand(setUnitsCentimeterCommand);
			//sendCommand(toHalfRateCommand);
			Thread.sleep(1000);
			
			usingEulerP = false;
			if (usingEulerP)
			{	sendCommand(outputFormatEulerCommand);
				recordLength = Latus.singleMarkerLengthEuler;
				recordTokens = 7; // 1index 3pos 3euler
			}
			else
			{   sendCommand(outputFormatQuaternionCommand);
				recordLength = Latus.singleMarkerLengthQuaternion;
				recordTokens = 8; // 1index 3pos 4quaternion
			}

			if (autoLaunchP == true)
			{
			   	sendCommand(autoLaunchOnCommand);
				doAutoAlign();
			}
			else 
			{
			    sendCommand(autoLaunchOffCommand);
				doManualLaunch();
			}
			
			// TODO - if manual launch check that the two correct markers are launched
			
			if (flusherOnP == true)
			 { flusher = new FlushLatusBuffer();
			   this.addCallback(flusher);
			 }
			
//			sendCommand(Latus.setCubeAsReferenceCommand);
//			System.out.println("Reset reference in 1 sec");
//			Thread.sleep(1000);
//			sendCommand(Latus.setCubeAsReferenceCommand);

			sendCommand(toContinuousModeCommand);
			outputStream.flush();
			
//			sendCommand(Latus.toSingleRecordModeCommand);
//			BufferedReader br;
//		    
//			int c = 0;
//			while(c < 1) // go forever
//			 {
//				
//				br = new BufferedReader(new InputStreamReader(System.in));
//				  
//				try 
//				 { br.readLine(); // wait for enter to be pressed
//				 } 
//				catch (IOException ioe) 
//				 { System.out.println("IO error on keyboard input");
//				      System.exit(1);
//				 }
//
//				sendCommand(Latus.toSingleRecordModeCommand);
//				
//			 }
			
	        setLatusReady(true);

        } catch (Exception e) { e.printStackTrace(System.err); }	
    }


	public void doAutoAlign()
	{
		// configure cubee marker
		System.out.println("Latus.doAutoAlign: place in rest position (-X up)...");
		aligningMarkersP = true; // parse and use orientation for all markers (not just cube)

        aligner = new MarkerAutoAligner(this);

        alignMarkerNumber = cubeMarkerNumber; 
        if (aligner.alignCubeMarker() == false)
         {
        	System.out.println("Latus.doAutoAlign: error - cubee marker unaligned.");
        	return;
         }
		alignMarkerNumber = headMarkerNumber; 
        if (aligner.alignHeadMarker() == false)
         {
        	System.out.println("Latus.doAutoAlign: error - cubee marker unaligned.");
        	return;
         }

		aligningMarkersP = false; // only use orientation for Cube Marker
		System.out.println("Latus.doAutoAlign: Aligning Markers Successful.");
	}
	
	public void doManualLaunch()
	{
	   if (debug) {System.out.println("Latus.doManualLaunch: starting Launcher...");}
		
		launchingMarkersP = true;
		
		launcher = new MarkerLauncher(this);
		
		launchMarkerNumber = cubeMarkerNumber;
		launcher.launchCubeMarker();

		launchMarkerNumber = headMarkerNumber;
		launcher.launchHeadMarker();

		//TODO: check success of marker launch
		System.out.println("Latus.doManualLaunch: Cube and Head markers are launched.");
	}
		
		
    
    public void sendCommand(String cmd) 
    {
    	if(outputStream==null){
        	try {
        		System.err.println("outputstream not ready");
                outputStream = serialPort.getOutputStream();
            } catch (IOException e) {e.printStackTrace(System.err);}
        }
    	try {
    		outputStream.write(cmd.getBytes());
        } catch (IOException e) {e.printStackTrace(System.err);}

        if (debug){ System.out.println("Written: "+cmd); }
//        try {
//        	Thread.sleep(10);
//        } catch (InterruptedException e) {e.printStackTrace(System.err);}
//        try {
//        	outputStream.close();
//        } catch (IOException e) {e.printStackTrace(System.err);}
    }

    
    // Hardware Receive Properties which  could be used
    // for more efficent receive handling. 
    // meassure with debug3
    // Note: Threshold and Framing do not work on winxp properly
    // Note: not imlemented in RXTX java serial library
    public void testProperties() {

        try {
          serialPort.enableReceiveThreshold(10);
          System.out.println("Receive threshold supported");
        }
        catch (UnsupportedCommOperationException e) {
          System.out.println("Receive threshold not supported");
        }

        try {
          serialPort.enableReceiveTimeout(10);
          System.out.println("Receive timeout supported");
        }
        catch (UnsupportedCommOperationException e) {
          System.out.println("Receive timeout not supported");
        }
    
        try {
          serialPort.enableReceiveFraming(10);
          System.out.println("Receive framing supported");
        }
        catch (UnsupportedCommOperationException e) {
          System.out.println("Receive framing not supported");
        }
        serialPort.disableReceiveTimeout();
        serialPort.disableReceiveThreshold();
        serialPort.disableReceiveFraming();
      }

    public void addCallback (Trackable cb)
    {
       callback.add(cb);
    }
    
    public void removeCallback (Trackable cb)
     {
        callback.remove(cb); 
     }
    
    /**
     * Adds a listener to the Fastrak
     *
     * @see FastrakListener
     * @param inListener the listener to add
     */
    public void addFastrakListener(FastrakListener inListener)
    {
//        if (!mOpen)
//            throw new FastrakNotOpenException("Add Fastrak Listener");
//        //System.out.println("addFTlistner");
//        mAsyncThread.addFastrakListener(inListener);
    }
    
    static public boolean isPort(String port)
    {
	   
//		try {
//			inputStream = serialPort.getInputStream();
//			outputStream = serialPort.getOutputStream();
//		} catch (IOException e) {

       
       portList = CommPortIdentifier.getPortIdentifiers();

        while (portList.hasMoreElements()) {
            portId = (CommPortIdentifier) portList.nextElement();
            if (portId.getPortType() == CommPortIdentifier.PORT_SERIAL) {
                 if (portId.getName().equals(port)) {
                 return true ;
                 }
            }
        }
        return false;
    }

    
    
    public void run() 
    {
       while(!isRunning)
       {
       try {
            Thread.sleep(2000000000);
        } catch (InterruptedException e) {}
       }
    }

    String record;
    public void serialEvent(SerialPortEvent event) 
    {
        switch(event.getEventType()) {
        case SerialPortEvent.DATA_AVAILABLE:
           try {

              while (inputStream.available() >= recordLength*2) {
               	record = readLine();
               	parseCubeeRecord(record);
               	record = readLine();
               	parseCubeeRecord(record);
               	 sendRecord();
                    while (inputStream.available() >= recordLength*3) {
                   	inputStream.read(new byte[recordLength]);
                    }
                    
              }
           } catch (IOException e) {}
           break;
        case SerialPortEvent.BI:
        case SerialPortEvent.OE:
        case SerialPortEvent.FE:
        case SerialPortEvent.PE:
        case SerialPortEvent.CD:
        case SerialPortEvent.CTS:
        case SerialPortEvent.DSR:
        case SerialPortEvent.RI:
        case SerialPortEvent.OUTPUT_BUFFER_EMPTY:
        	if(debug)System.out.print("_");
            break;
        }
    }
    
    /**
     * Reads a line of input from the Fastrak and transforms it into
     * a {@link FastrakEvent FastrakEvent} object.
     *
     * @return the event corresponding to the next line of input from
     * the Fastrak device, or null if corrupted.
     * @exception IOException when there is an error reading
     */
 //    byte[] inputByteBuffer=null;
    synchronized String readLine() throws IOException
    {
        // Create a StringBuffer and int to receive input data.
//       if(inputByteBuffer==null)
//    	  inputByteBuffer= new byte[recordLength];
      // inputStream.read(inputByteBuffer);
//       
       StringBuffer inputBuffer = new StringBuffer();
       //      inputBuffer.delete(0,inputBuffer.length()-1);
       
       int newData = 0;
        while (true)
        {
        	newData = inputStream.read();
        	if (newData == -1)
        	 { System.out.println("readLine(): read() returned -1 breaking loop");
        		break;
        	 }
        	inputBuffer.append((char)newData);
        	if ((char)newData == '\n')
        	{
        		break;
        	}
        }
        if (newData != -1)
        {
        	if (debug) {System.out.print("readLine(): <" + inputBuffer + ">");}

            return inputBuffer.toString().trim();
        }
        return null;
    }
    
    /**
     * Parses a line of input from the fastrak into a FastrakEvent.
     * The event is in the following format:
     * <br><br>
     *
     * <pre>01 12.3 35.4 -1.55 178.0 -87.5 34.4 0</pre>
     * Which represents:
     * <pre>
     *	sensor: 01
     *	(x,y,z) = (12.3, 35.4, -1.55)
     *	(azi, elev, roll) = (178.0, -87.5, 34.4)
     *	button is: up (not pressed)
     * </pre>
     *
     * @param inLine the line of input from the Fastrak device
     * @return the event corresponding to this input (or null if invalid)
     */
    private int parseCubeeRecord(String inLine)
    {
        StringTokenizer stok = new StringTokenizer(inLine);
        String str;

        if (inLine.length() != recordLength || stok.countTokens() != recordTokens){
        	if(debug)System.out.println("parseFastrakRecord: not full record: length = " 
        			+ inLine.length() + ", numTokens = " + stok.countTokens());
            return -1;
        }
        int markerNum;
        double[] p = new double[3];
        double[] q = new double[4];
        //boolean buttonDown;
        
        // The marker number is the first token (integer)
        try 
         { str = stok.nextToken();
           markerNum = Integer.parseInt(str);
         }
        catch (NumberFormatException e)
         { System.out.println("parseFastrakRecord error: first token in record not an integer");
           return -1;
         }

        // The position & orientation are the next 7 tokens (floating point)
        try 
        {
           for(int i = 0; i < p.length; i++)
           { str = stok.nextToken();
             p[i] = Float.parseFloat(str);
           }
           for(int i = 0; i < q.length; i++)
           { str = stok.nextToken();
             q[i] = Float.parseFloat(str);
           }
        }
    	catch (NumberFormatException e)
    	 { System.out.println("parseFastrakRecord error: remaining record not all floats");
    	   return -1;
    	 }

    	
    	
    	
        if(callback.size()>0)
         {
           if(eyePos==null) { eyePos = new Point3d(); }
           if(cubePose == null) { cubePose = new RigidTransform3d(); }
           if(cubeOrientation == null) { cubeOrientation = new Quaternion(); }

           if (markerNum == cubeMarkerNumber) 
            {
        	  cubeOrientation.set(q);
//     	  	  quaternionToRotMat(q, cubePose.R);
        	  cubePose.R.set(cubeOrientation);
        	  cubePose.R.transpose();
        	  
        	  cubePose.p.set(p[0], p[1], p[2]);
        	  cubePose.p.negate();
        	  cubePose.p.transform(cubePose.R);
        	  
        	  newCubePose = true;
        	  if (false) 
        	   { System.out.println("Latus: New Cube Pose = \n" + cubePose.toString("%8.2f"));
        	   }
            }
           else if (markerNum == headMarkerNumber)
            {
        	  eyePos.set(p[0], p[1], p[2]);
//        	  System.out.println("set eye position");
        	  newEyePos = true;
        	  if (false) 
        	   { System.out.println("New Eye Pos = " + eyePos.toString("%8.2f"));
        	   }
            }
           else
            { System.out.println("Latus.parseCubeeRecord - unknown marker number " + markerNum);
            } 
         }        else // no callback attach - print debug message
         
            if(debug)
                 {
                    System.out.print("no callback parsed: " + markerNum+"\t");
                    for (int j=0; j < p.length; j++)
                    { System.out.printf("%8.2f",p[j]);
                    }
                    for (int j=0; j < q.length; j++)
                    { System.out.printf("%8.2f",q[j]);
                    }
                    System.out.println("");
                 }
                 return 0;
          
    }
    
    void sendRecord()    
    {            
        
           //TODO -  check if we have stale data and set bad data flag
//         if (newEyePos == true && newCubePose == true)
           
           // profile code 
//           if(profile)
//           { 
//        	  timer.stop();
//              timings[timeID++] = timer.getTimeUsec();
//              if(timeID>=NUMTIMEINGS)
//              {
//            	 for(int i=0;i<NUMTIMEINGS;i++)
//              	{ System.out.print(timings[i]+"\t");
//              	}
//            	 System.out.println("");
//            	 timeID=0;
//            	 
//              }
//              timer.start();
//           }
//           
           for(int i=0;i<callback.size();i++)
            {//XXXcallback.get(i).setPos(pos);
        	  callback.get(i).setTrackerData(
        			new RigidTransform3d(cubePose),new Point3d(eyePos));
        	  newCubePose = false;
        	  newEyePos = false;
            }
         }
    
           
    
    
    /**
     * Parses a line of input from the fastrak into a FastrakEvent.
     * The event is in the following format:
     * <br><br>
     *
     * <pre>01 12.3 35.4 -1.55 178.0 -87.5 34.4 0</pre>
     * Which represents:
     * <pre>
     *	sensor: 01
     *	(x,y,z) = (12.3, 35.4, -1.55)
     *	(azi, elev, roll) = (178.0, -87.5, 34.4)
     *	button is: up (not pressed)
     * </pre>
     *
     * @param inLine the line of input from the Fastrak device
     * @return the event corresponding to this input (or null if invalid)
     */
    private FastrakEvent parseRecord(String inLine)
    {
        StringTokenizer stok = new StringTokenizer(inLine);
        String str;

        if (inLine.length() != recordLength || stok.countTokens() != recordTokens){
        	if(debug)System.out.println("parseFastrakRecord: not full record: length = " 
        			+ inLine.length() + ", numTokens = " + stok.countTokens());
            return null;
        }
        int markerNum;
        double[] values = new double[recordTokens-1];
        double[] p = new double[3];
        double[] q = new double[4];
        //boolean buttonDown;
        
        // The marker number is the first token (integer)
        try 
         { str = stok.nextToken();
           markerNum = Integer.parseInt(str);
         }
        catch (NumberFormatException e)
         { System.out.println("parseFastrakRecord error: first token in record not an integer");
           return null;
         }

        System.out.println("marker number: ");
        
        // The position & orientation are the next 7 tokens (floating point)
        try {
        	
         if (usingEulerP)
         {
           int tokenIndex=0;
           while(stok.hasMoreElements())
            { str = stok.nextToken();
              values[tokenIndex++] = Float.parseFloat(str);
            }
         }
         else
         {
           for(int i = 0; i < p.length; i++)
           { str = stok.nextToken();
             p[i] = Float.parseFloat(str);
           }
           for(int i = 0; i < q.length; i++)
           { str = stok.nextToken();
             q[i] = Float.parseFloat(str);
           }
         }
           
        }
        
    	catch (NumberFormatException e)
    	 { System.out.println("parseFastrakRecord error: remaining record not all floats");
    	   return null;
    	 }
    	
//        System.out.print("Read: " + markerNum+":\t");
//         for (int j=0; j < values.length; j++)
//         { System.out.printf("%8.3f",values[j]);
//         }
//        System.out.println("");


        if(callback.size()>0)
            {
               if(eyePos==null) { eyePos = new Point3d(); }
               if(cubePose == null) { cubePose = new RigidTransform3d(); }
               if(cubeOrientation == null) { cubeOrientation = new Quaternion(); }

               // Note - need orientation info for AlignMarker callback
        	   if (aligningMarkersP && markerNum != alignMarkerNumber)
        	   { 
        		   return null;
        	   }
        	   
        	   if (markerNum == cubeMarkerNumber || aligningMarkersP) 
               {
        		if (usingEulerP)
        		{
               	 cubePose.p.set(values[0], values[1], values[2]);
               	 cubePose.R.setEuler(values[3]*DEG_TO_RAD,
               			 	  values[4]*DEG_TO_RAD,
               			 	  values[5]*DEG_TO_RAD);
        		}
        		else
        		{
              	 cubePose.p.set(p[0], p[1], p[2]);
//              	 quaternionToRotMat(q, cubePose.R);
 	           	 cubeOrientation.set(q);
            	 cubePose.R.set(cubeOrientation);
        		}

          	   	 newCubePose = true;
          	   	 if (false) {
          	   		 System.out.println("New Cube Pose = \n" + cubePose.toString("%8.2f"));
          	   	 }
               }
               else
               {
            	if (usingEulerP)
            	{
              	  eyePos.set(values[0], values[1], values[2]);
            	}
            	else
            	{
             	  eyePos.set(p[0], p[1], p[2]);
//             	  System.out.println("set eye position");
            	}
            	  newEyePos = true;
           	   	  if (false) {
          	   		 System.out.println("New Eye Pos = " + eyePos.toString("%8.2f"));
          	   	 }
               }
               
//               if (newEyePos == true && newCubePose == true)
               {
               for(int i=0;i<callback.size();i++)
                {//XXXcallback.get(i).setPos(pos);
            	   callback.get(i).setTrackerData(cubePose,eyePos);
            	   newCubePose = false;
            	   newEyePos = false;
                }
               }
        	  return null;
            }
                    
            else{
            	
              if (usingEulerP)
              {
             // The button state is last (only if button string in output format)
            //buttonDown = (inLine.charAt(46) == '1');

            	  FastrakEvent event = new FastrakEvent( this,
                    markerNum,
                    (float)values[0],
                    (float)values[1],
                    (float)values[2],
                    (float)values[3],
                    (float)values[4],
                    (float)values[5],
                    false);
            
            	if(debug)
            	{
            		System.out.print("parsed: " + markerNum+"\t");
            		for (int j=0; j < values.length; j++)
            		{ System.out.printf("%8.2f",values[j]);
            		}
            		System.out.println("");
            	}
            	  
            	return event;
              }
              else
              {
                if(debug)
                {
                   System.out.print("parsed: " + markerNum+"\t");
                   for (int j=0; j < p.length; j++)
                   { System.out.printf("%8.2f",p[j]);
                   }
                   for (int j=0; j < q.length; j++)
                   { System.out.printf("%8.2f",q[j]);
                   }
                   System.out.println("");
                }
                return null;
              }

            }
           
    }
    
    public void close()
    {
       isRunning=false;
    }

//TODO - remove old code:    
//    public static void main2(String[] args) {
//
//       String port= "/dev/ttyS0";
//
//       debug = true;
//       
//       if(debug)System.out.println("starting Latus.main open "+port);
//
//    	if(isPort(port)){
//    		try{
//    			@SuppressWarnings("unused") 
//    			Latus reader = new Latus(115200);
//////    			reader.sendCommand(CRCommand);
//////    			reader.sendCommand(CRCommand);
//////    			reader.sendCommand(CRCommand);
//////    			reader.sendCommand(CRCommand);
//////    			Thread.sleep(2000);
////    			reader.sendCommand(systemResetCommand);
////    			Thread.sleep(8000);
////    			reader.sendCommand(autoLaunchOnCommand);
//////    			reader.sendCommand(OutputFormatCommand);
////    			reader.sendCommand(getRecordCommand);
//////    			reader.sendCommand(toContinuousCommand);
//////    			reader.sendCommand(toContinuousCommand);
//            } catch (Exception e) { e.printStackTrace(System.err); }	
//    	} else 
//    	{ System.err.println(" Serial Port not found");
//    	}
//    	
//    }
    
	public static void main(String[] args) 
	{
//		if (args.length < 1) {
//			System.out.print("SimpleRead.class /dev/ttyxx\n");
//			System.exit(-1);
//		}
		
		debug = true;
		flusherOnP = true;
		
		portList = CommPortIdentifier.getPortIdentifiers();

		while (portList.hasMoreElements()) {
            		portId = (CommPortIdentifier) portList.nextElement();
           			if (portId.getPortType() == CommPortIdentifier.PORT_SERIAL) {
                		// if (portId.getName().equals("COM1")) {
               		 if (portId.getName().equals("/dev/ttyS0")) {
                		//	if (portId.getName().equals(args[0])) {
                    				try { Latus reader = new Latus(115200, false);}
                    				catch (Exception e) {
                    					System.err.println("Latus creation failed.");
                    					e.printStackTrace();}
                			}
            		}
		}
	}

	/**
	 * Sets this rotation to one represented by the
	 * quaternion q.
	 *
	 * @param q quaternion representing the rotation
	 */
	public void quaternionToRotMat (double[] q, RotationMatrix3d R)
	 {
		if (R == null)
		{ System.err.println("quaternionToRotMat: RotationMatrix R is null.");
		  return;
		}
	   R.m00 = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]; 
	   R.m10 = 2*(q[3]*q[0] + q[1]*q[2]);
	   R.m20 = 2*(q[1]*q[3] - q[0]*q[2]);

	   R.m01 = 2*(q[1]*q[2] - q[0]*q[3]);
	   R.m11 = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3]; 
	   R.m21 = 2*(q[1]*q[0] + q[3]*q[2]);

	   R.m02 = 2*(q[1]*q[3] + q[0]*q[2]);
	   R.m12 = 2*(q[2]*q[3] - q[0]*q[1]);
	   R.m22 = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; 
	 }


	public boolean isLatusReady() {
		return latusReadyP;
	}


	public void setLatusReady(boolean latusReady) {
		this.latusReadyP = latusReady;
	}
   
    
}


//XXX - finish timer checker & implement timer for tracker status
class CheckTrackerStatus implements ActionListener
{
	protected Timer myTimer;
	
	public CheckTrackerStatus(Timer t)
	 {
		System.out.println("Adding tracker status checker");
		myTimer = t;
 	 }
	
	public void actionPerformed(ActionEvent e)
	 {
		
	 }
	
}

class FlushLatusBuffer implements Trackable
{
   protected boolean trackerIsGood = false;
   
   public FlushLatusBuffer()
    {
	  System.out.println("Setting up callback to flush buffer");
    }
   
   public void setTrackerData(RigidTransform3d X, Point3d pos)
    {
	   System.out.println("Flush: Xcube = \n" + X.toString("%8.2f"));
	   System.out.println("Flush: Peye = " + pos.toString("%8.2f"));
    }
   
   public void setTrackerRunning(boolean tracking)
    {
	   trackerIsGood = tracking;
    }
}

class MarkerAutoAligner implements Trackable
{
   protected Latus myLatusReader;
   protected boolean debug = true;
   protected boolean trackerIsGood = false;
   protected boolean myAlignedP = false;
   protected boolean stepPhaseRequest = false;
   
   protected RigidTransform3d expectedXwm; // expected X world-to-marker
   
   protected Point3d actualAA = new Point3d();
   protected Point3d expectedAA = new Point3d();
   
   protected double theta;
   
   protected int phaseStepCounter = 0;
   protected static final int MAX_PHASE_ATTEMPTS = 20;
   protected static final double AA_ERROR_THRESHOLD = 20.0;
   

   private final int CUBE = Latus.cubeMarkerNumber;
   private final int HEAD = Latus.headMarkerNumber;
   
   private String markerName = "_";
   
   public MarkerAutoAligner(Latus latusReader)
    {
//	  System.out.println("Setting up callback to configure Cubee Marker");
	  
	  expectedXwm = new RigidTransform3d();
	  expectedXwm.R.setAxisAngle(0,0,1,Math.PI);
	  if (debug) {System.out.println("Align(): Expected Xwc = \n" + expectedXwm.toString("%8.2f"));}

	  if (latusReader == null )
	    {
		   System.err.println("Align(): latusReader null - exiting");
		   return;
	    }
	  myLatusReader = latusReader;
    }
   
   public boolean alignCubeMarker()
    {
	   markerName = "Cube";
	   return align(CUBE);
    }

   public boolean alignHeadMarker()
   {
	   markerName = "Head";
	   return align(HEAD);
   }

   private boolean align(int MarkerIndex)
    {
	   if (myLatusReader == null )
	    {
		   System.err.println("Align(): latusReader null - exiting");
		   return false;
	    }
	   myAlignedP = false;
	   stepPhaseRequest = false;
	   phaseStepCounter = 0;
		  
	   myLatusReader.addCallback(this);
		  
	   while(phaseStepCounter < MAX_PHASE_ATTEMPTS)
	    {
		   myLatusReader.sendCommand(Latus.toSingleRecordModeCommand);
		   try {
			   Thread.sleep(200);
		   } catch (Exception e) {e.printStackTrace();}
		   
		   if (this.isAligned())
		    {
			   System.out.println("Align(): " + markerName + "Marker (#" + MarkerIndex + 
					   ") Aligned. AxisAngle discrepancy = " + theta + "degrees.");
			   myLatusReader.removeCallback(this);
			   return true;
		    }
		   else if (stepPhaseRequest == true)
		    {
	   			  // step to next phase
			   if (MarkerIndex == CUBE)
			    { myLatusReader.sendCommand(Latus.phaseStepCubeMarkerCommand);
			    }
			   else
			    { myLatusReader.sendCommand(Latus.phaseStepHeadMarkerCommand);
			    }
				   
			    stepPhaseRequest = false;
			    phaseStepCounter++;
		    }
		   else
		    {
			   System.out.println("Align(): Didn't get marker record: " +
			    		"turn on " + markerName + "Marker #" + MarkerIndex);
		    }
	    }
	   System.out.println("Align(): tried all possible phases for " + markerName + 
			   " Marker #" + MarkerIndex + " alignment failed.");
	   System.out.println("Align(): ensure that " + markerName + 
			   " Marker #" + MarkerIndex + "is in rest position (X- up, Y+ left).");
	   return false;
    }
   
   
   public void setTrackerData(RigidTransform3d actualXwm, Point3d pos)
    {
 	   expectedXwm.R.getAxisAngle(expectedAA);
 	   actualXwm.R.getAxisAngle(actualAA);
 	   
 	   theta = Math.acos(expectedAA.dot(actualAA)/(expectedAA.norm()*actualAA.norm()));
 	   theta *= (180.0/Math.PI); //degrees

	   if (debug) {
	   System.out.println("actual Xwc = \n" + actualXwm.toString("%8.2f"));
 	   System.out.println("expected Xwc = \n" + expectedXwm.toString("%8.2f"));
 	   System.out.println("angle between exAA and acAA) = " + theta);
	   }
 	   
 	   if (theta < AA_ERROR_THRESHOLD)
 	    {
 		  if (debug) {System.out.println(markerName + " Marker orientation is correct - setting configuredP");}
 		   setAligned(true);
 	    }
 	   else // step to new phase 
 	    {
 		  if (debug) {System.out.println(markerName + " Marker: Bad Orientation - Phase #" + phaseStepCounter);}
 		   stepPhaseRequest = true;
 	    }
    }
   
   public void setTrackerRunning(boolean tracking)
    {
	   trackerIsGood = tracking;
    }

   public boolean isAligned() {
	   return myAlignedP;
   }

   private void setAligned(boolean configured) {
	   this.myAlignedP = configured;
   }
}


class MarkerLauncher implements Trackable
{
   protected boolean debug = true;
   protected boolean trackerIsGood = false;
   
   protected Latus myLatusReader;
   
   private final int CUBE = Latus.cubeMarkerNumber;
   private final int HEAD = Latus.headMarkerNumber;
   private String markerName = "_";
   BufferedReader br;
   
   public MarkerLauncher(Latus latusReader)
    {
	  System.out.println("Setting up callback for marker launcher");
	  br = new BufferedReader(new InputStreamReader(System.in));
	  
	  if (latusReader == null )
	    {
		   System.err.println("Align(): latusReader null - exiting");
		   return;
	    }
	  myLatusReader = latusReader;
    }

   private void launch(int markerNumber)
    {
	   System.out.println("Launching " + markerName + " Marker " + markerNumber + 
			 ": orient X+ up under Anchor. Press ENTER to launch.");
	   try 
	    { br.readLine(); // wait for enter to be pressed
	    } 
	   catch (IOException ioe) 
	    { System.out.println("IO error on keyboard input");
	      System.exit(1);
	    }
//	    if (debug) {System.out.println("Sending Launch cmd to Latus...");}
	    if (markerNumber == CUBE)
	     {  if(debug){System.out.println("Sending CubeLaunch cmd to Latus...");}
	    	myLatusReader.sendCommand(Latus.launchMarkerCommand);
	     }
	    else if (markerNumber == HEAD)
	     { if(debug){System.out.println("Sending HeadLaunch cmd to Latus...");}
	       myLatusReader.sendCommand(Latus.launchMarkerCommand);
	     }
	    else 
	     { System.err.println("MarkerLauncher.launch() - unknown marker number " + markerNumber);
	     }
	    if (debug) {System.out.println("Launch cmd successful...");}   
    }
   
   public void launchCubeMarker()
   {
	   markerName = "Cube";
	   launch(CUBE);
   }

   public void launchHeadMarker()
    {
	   markerName = "Head";
	   launch(HEAD);
    }

   public void setTrackerData(RigidTransform3d X, Point3d pos)
    {
	   System.out.println("Launch: Xcube = \n" + X.toString("%8.2f"));
	   System.out.println("Launch: Peye = " + pos.toString("%8.2f"));
    }
   
   public void setTrackerRunning(boolean tracking)
    {
	   trackerIsGood = tracking;
    }
}