package iodevices.polhemus;

import gnu.io.CommPortIdentifier;
import gnu.io.PortInUseException;
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.FileWriter;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.io.PrintWriter;
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 maspack.util.IndentingPrintWriter;
import artisynth.core.util.FunctionTimer;
import artisynth.core.driver.*;
import artisynth.core.driver.Side.SideType;


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

    //static scene variables
	private boolean staticSceneP = false;
    Point3d c_Peye = new Point3d();
    final RigidTransform3d IdentityTransform = 
    	new RigidTransform3d();
    
    // constants
    
	//TODO get cube and head number from CubeeMain - these values should be settable
    public static final int cubeMarkerNumber = 1;
    public static final int headMarkerNumber = 2;

    public static final int singleMarkerLengthPos = 24;
    public static final int singleMarkerLengthQuaternion = 52;
    public static final int singleMarkerLengthBinQuaternion = 31;
    private int eyeRecordLength=Fastrak.singleMarkerLengthPos;
    private int eyeRecordTokens=8;
    private int cubeRecordLength=Fastrak.singleMarkerLengthQuaternion ;
    private int cubeRecordTokens=8;
    
	public static final int  DEFAULT_BAUDRATE= 115200;

	
	public static final String setUnitsCentimeterCommand    = "u";	
	public static final String toBinaryCommand				= "f";
	public static final String toASCIICommand				= "F";
	public static final String toContinuousModeCommand		= "C";
	public static final String toSingleRecordModeCommand	= "c";
	public static final String retrieveStatusCommand		= "S";
	public static final String getRecordCommand				= "P";
	public static final String systemResetCommand		    = "W";
    public static final String CRCommand				    = "\r";

    //String configString = "O1,52,54,1\nC\n"; // for high precision
    //String outputFormatString = ",2,4,16,1\n"; for F3.2 precision w Stylus switch
    //String outputFormatString = ",2,4,1\n"; // for F3.2 precision w/o Stylus switch
    //String outputFormatString = ",2,4,1\n"; // for F3.2 precision w/o Stylus switch

    public static final String formatbinCubeMarkerCommand	= "O1,2,11\r";
    public static final String formatbinEyeMarkerCommand	= "O2,2,11\r";
    public static final String formatCubeMarkerCommand	= "O1,2,11,1\r";
    public static final String formatEyeMarkerCommand	= "O2,2,1\r";
//    public static final String formatCubeMarkerCommand	= "O1,52,61,1\r";
//    public static final String formatEyeMarkerCommand	= "O2,52,1\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";

    // flags
	public static boolean debug = false;
	public static boolean profile = false;
	public static boolean startContinuousP = true;
    private boolean myCalibrationP = false;
	private boolean fastrakReadyP = false;
	private boolean newCubePose = false;
	private boolean newEyePos = false;
	private boolean isBinary = false;

	// mode flags
	private boolean initP = true;
	private boolean isRunning;

    public static double eyeCalDistance = CubeeMain.MODEL_HEIGHT * 2.0;

	public ArrayList<FakeTrackable> callback;
    ArrayList<Point3d> calibrationEyepos;
	public static FunctionTimer timer = new FunctionTimer();
	public static final int NUMTIMEINGS= 10;
	public static double[] timings = new double[NUMTIMEINGS];
	static int timeID=0;
	Point3d b_Peye;
	Point3d w_Peye;
	Quaternion cubeOrientation;
	RigidTransform3d xCubeBase;
	RigidTransform3d xCubeWorld; 
	RigidTransform3d xBaseWorld; // fixed by XBaseCube at init time
	RigidTransform3d xTrackerBase; // given by raw tracker data
	RigidTransform3d init_xTrackerBase; // xTrackerBase at t=0 
	RigidTransform3d xTrackerCube; // fixed taken from calibration data

    SerialPort serialPort;
    Thread readThread;
    InputStream inputStream;
    OutputStream outputStream;
    BufferedReader reader;
    PrintWriter pw;
    long startTime=0;

    public Fastrak()  throws IOException,PortInUseException, 
    TooManyListenersException, UnsupportedCommOperationException
    {
       this(DEFAULT_BAUDRATE, true, false, new RigidTransform3d());
    }
    
    public Fastrak(int baud,boolean binaryMode)  throws IOException,PortInUseException, 
    TooManyListenersException, UnsupportedCommOperationException
    {
       this(baud, true, binaryMode, new RigidTransform3d());
    }
    
    public Fastrak(int baud, boolean doAutoLaunch,boolean binaryMode)  throws IOException,PortInUseException, 
    TooManyListenersException, UnsupportedCommOperationException
    {
       this(baud, doAutoLaunch, binaryMode, new RigidTransform3d());
    }

    public Fastrak(int baud, boolean doAutoLaunch, RigidTransform3d xTrackerCube)  
    throws IOException,PortInUseException, TooManyListenersException, UnsupportedCommOperationException
    {
        this(baud, doAutoLaunch, false, new RigidTransform3d());    	
    }
    
    public Fastrak(int baud, boolean doAutoLaunch, boolean binaryMode, RigidTransform3d xTrackerCube)  
    throws IOException,PortInUseException, TooManyListenersException, UnsupportedCommOperationException
    {
        setFastrakReady(false);
        

        callback = new ArrayList<FakeTrackable>();
 	    calibrationEyepos = new ArrayList<Point3d>();
 	    
 	    isBinary=binaryMode;
 	    
    	b_Peye = new Point3d();
    	w_Peye = new Point3d();
    	cubeOrientation = new Quaternion();
    	xCubeBase = new RigidTransform3d();
    	xCubeWorld = new RigidTransform3d();
    	xBaseWorld = new RigidTransform3d();
    	xTrackerBase = new RigidTransform3d();
    init_xTrackerBase = new RigidTransform3d();
    	this.xTrackerCube = new RigidTransform3d(xTrackerCube);
    	
    	if (true)
    	{ System.out.println("xTrackerCube: \n" + xTrackerCube.toString("%8.2f"));
    	}
    	
       //TODO - test that serial cable to Fastrak is connected
        System.out.println("Starting Fastrak tracker...");

//		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("FastrakReader", 100);
         } 
        catch (PortInUseException e)
         { e.printStackTrace();
         }

        try
        {
        	inputStream = serialPort.getInputStream();
        	outputStream = serialPort.getOutputStream();

        reader = new BufferedReader ( new InputStreamReader ( inputStream ) );
        

        } 
        catch (Exception e)
        { e.printStackTrace();
        }
        
	  
        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
	 	 { 
//		    Thread.sleep(1000);
		    System.out.println("Resetting Fastrak Tracker...");
			sendCommand(systemResetCommand);
			sendCommand(systemResetCommand);
			Thread.sleep(500);
			
			//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 Fastrak 
			// can save up to three commands in Fastrak
			
			sendCommand(setUnitsCentimeterCommand);
			//sendCommand(toHalfRateCommand);
			Thread.sleep(500);
			
			
			if(isBinary)
			{
				sendCommand(toBinaryCommand);
				Thread.sleep(500);
				sendCommand(formatbinCubeMarkerCommand);
				Thread.sleep(500);
				sendCommand(formatbinEyeMarkerCommand);
				Thread.sleep(500);
				
				eyeRecordLength=singleMarkerLengthBinQuaternion;
				cubeRecordLength=singleMarkerLengthBinQuaternion;
				
				
			}else
			{
				sendCommand(formatCubeMarkerCommand);
				sendCommand(formatEyeMarkerCommand);
				sendCommand(toASCIICommand);
			}
			

			inputStream.skip(inputStream.available());	

	        try
	        { serialPort.addEventListener(this);
	        } 
	        catch (TooManyListenersException e)
	        { e.printStackTrace();
	        }

	        serialPort.notifyOnDataAvailable(true);

			readThread = new Thread(this);
			readThread.setPriority(Thread.MAX_PRIORITY);
			readThread.start();

			
			
			if (startContinuousP == true)
			{  
			   System.out.println("Fastrak started successfully in CONTINUOUS mode.");
			   sendCommand(toContinuousModeCommand);
			   outputStream.flush();
			}
			else
			{
			   sendCommand(toSingleRecordModeCommand);
			   System.out.println("Fastrak started successfully in SINGLE record mode.");
			   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(Fastrak.getRecordCommand);
			   }
			}
	        setFastrakReady(true);
	        

	        // write data to file
	  	  if(profile)
	  	  {
	        pw = new PrintWriter (new FileWriter ("trackerdata.txt"));
	        startTime=System.nanoTime();
	  	  }
         } 
		catch (Exception e) 
         { System.err.println("Fastrak(): exception in constructor after serialThread.start()"); 
           e.printStackTrace(System.err); 
         }	
    }

		
    
    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);}
    }

    

    public void addCallback (FakeTrackable cb, SideType type)
    {
       callback.add(cb);
       setCallbackEyepos(type);
       if (true)
       { System.out.println("Fastrak added callback #" + callback.size() +
    		 " ( " + cb.getClass().getName() + " )");
       }
    }
    
    public boolean removeCallback (FakeTrackable cb)
     {
       int callbackIdx = callback.indexOf(cb);
       if (callbackIdx != -1)
    	  	  calibrationEyepos.remove(callbackIdx);
       return callback.remove(cb); 
     }
    
    
    static public boolean isPort(String port)
    {
       
       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() 
    {
//       System.out.println("isRunning flag = " + isRunning);
       while(!isRunning)
       {
       try {
          Thread.sleep(2000000000);
        } catch (InterruptedException e) {}
       }
       
       // cleanup
       
    }
    
    
    

    int recordCount=0;
    String record;
    long stime=0;
    public void serialEvent(SerialPortEvent event) 
    {
        switch(event.getEventType()) {
        case SerialPortEvent.DATA_AVAILABLE:
           try {

//        	  if(inputStream.available() >=recordLength*4)
//        	  {
//            	 record = reader.readLine();
//            	 record = reader.readLine();
//        		 pw.print("_");
//        	  }
        	  
        	  if(inputStream.available() >= eyeRecordLength) {
            	  if(isBinary)
            	  {
            		  int ret = parseBinCubeeRecord();
            		  if (ret!=0)
            		  {System.out.print(" e:"+ret);};

            	  } else{

            		  record = reader.readLine();
            	  
            	 if(record.length()==eyeRecordLength||record.length()==cubeRecordLength)
            	 {
            		parseCubeeRecord(record);
            	 }
            	 else //if(record.length()<eyeRecordLength)
            	 {
            		System.out.println("read " + record.length() + " bytes: " + record);
            	 }
            	  }
              }
              
              
              // profiling 
              if(profile){    
                 recordCount++;
                 if(recordCount>5000&&pw!=null)
                 {
                    pw.flush();
                    pw.close();
                    System.out.println("Done logging");
                    System.exit(0);
                 }
               }

              
              
           } 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(true)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
    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)
     */
    
    int markerNum;
    double[] p = new double[3];
    double[] q = new double[4];
    String str;

    
    private double readFloat() throws IOException
    {
    	int myNum=0;
    	for (int i=0;i<4;i++)
    	{ myNum=((reader.read()& 0x000000ff)<<8*i) | myNum;
    	}
		//System.out.printf("%x ",myNum);
		//System.out.printf("%f ",Float.intBitsToFloat(myNum));
    	
    	return(double)Float.intBitsToFloat((myNum&0xffffffff));
    	
    }
      
    
        
    private int parseBinCubeeRecord() throws IOException
    {

    	
    	int bytesRead=0;
    	int c;
//    	reader.mark(singleMarkerLengthBinQuaternion);

//    	for(int i=0;i<singleMarkerLengthBinQuaternion;i++)
//    		System.out.printf("%x ",reader.read());
//    	reader.reset();
//    	System.out.println();
//    	System.out.println();
//
//    	for(int i=0;i<singleMarkerLengthBinQuaternion;i++)
//    		System.out.printf("%c ",reader.read());
//    	reader.reset();
//    	System.out.println();
//    	System.out.println();

    	
    	do {
    		if(bytesRead==singleMarkerLengthBinQuaternion) // end of data
    			return -1;
    		bytesRead++;
    		c = reader.read();
    	}while(c!='0');
    	
    	if(bytesRead!=1)// not enough bytes left in the buffer
    		return -2;

    	markerNum = (int)reader.read()-0x30;
    	
    	if((markerNum != cubeMarkerNumber) && (markerNum != headMarkerNumber)) // Wrong makrer
    		return -3;
    	
    	int status = reader.read(); // Command error 
    	
//    	if (status !=0 )
    	
    	
        try 
        {
           for(int i = 0; i < p.length; i++)
           { p[i] = readFloat();
           }
           for(int i = 0; i < q.length; i++)
           { q[i] = readFloat();
           }
        }
    	catch (NumberFormatException e)
    	 { System.out.println("parseFastrakRecord error: remaining record not all floats");
    	   return -1;
    	 }
    	
    	distributeFastrackRecord();
    	
    	return 0;
    }
    
    private int parseCubeeRecord(String inLine)
    {
        //TODO - fvogt: records here with 0 length
        
        if (inLine.length()!=eyeRecordLength&&inLine.length()!=cubeRecordLength)
         {
        	if(true)System.out.println("parseFastrakRecord: bad record length " 
        			+ inLine.length());
            return -1;
         }
        
        inLine = inLine.replaceAll("-", " -");
        StringTokenizer stok = new StringTokenizer(inLine);
        
        // Get marker number (##m)
        try 
         { str = stok.nextToken();
//           str.replace('m', ' '); //m added to end of marker number token
           markerNum = Integer.parseInt(str.substring(0,2));
         }
        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);
           }
           if (markerNum == cubeMarkerNumber)
           {
              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;
    	 }

    	distributeFastrackRecord();
    	
//        else // no callback attach - print debug message
//         { 
//            if(debug)
//                 {
//                    System.out.print("Fastrak has no callbacks. 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;
    }

    
    
    private void distributeFastrackRecord()
    
    {
    	
//    	if(markerNum==cubeMarkerNumber)
//		{
//			System.out.printf("%5.3f %5.3f %5.3f ", p[0],p[1],p[2]);
//			System.out.printf("%5.3f %5.3f %5.3f %5.3f \n",q[0],q[1],q[2],q[3]);
//		}
	

          if (markerNum == cubeMarkerNumber) 
           {
//TODO: toss old code:              
//       	  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);

             cubeOrientation.set(q);
             xTrackerBase.R.set(cubeOrientation);
             xTrackerBase.p.set(p[0], p[1], p[2]);
             //	TODO: check this conversion (mm -> model dist units);
             xTrackerBase.p.scale(CubeeMain.CM_TO_MODELUNITS);
             
             if (initP)
             { // Note - at init time World==Cube, therefore:
           	init_xTrackerBase.set(xTrackerBase);
            	xBaseWorld.mulInverseRight(xTrackerCube, init_xTrackerBase);
             	
           	if (debug)
           	{ System.out.println("xBaseWorld = \n" + xBaseWorld.toString("%8.2f"));
           	}
           	initP = false;
             }
             
             xCubeBase.mulInverseRight(xTrackerBase,xTrackerCube);
             xCubeWorld.mul(xBaseWorld,xCubeBase);

       	  newCubePose = true;
       	  if (debug) 
       	   { 
       	     System.out.println("Fastrak: xTrackerCube = \n" + xTrackerCube.toString("%8.2f"));
       	     System.out.println("Fastrak: xTrackerBase = \n" + xTrackerBase.toString("%8.2f"));
       	     System.out.println("Fastrak: xCubeWorld = \n" + xCubeWorld.toString("%8.2f"));
       	   }
       	  if(profile)
       	  {
       		 pw.printf("%d %d %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f \n",(System.nanoTime()-startTime)/1000000,(System.nanoTime()-stime)/1000000,p[0],p[1],p[2],q[0],q[1],q[2],q[3]);
       		 stime=System.nanoTime();
       	  }

           }
          else if (markerNum == headMarkerNumber)
           {
       	  b_Peye.set(p[0], p[1], p[2]);
       	  b_Peye.scale(CubeeMain.CM_TO_MODELUNITS);
       	  
//    	     System.out.println("Fastrak: b_Peye = " + b_Peye.toString("%8.2f"));
       	  
       	  if (initP) // xBaseWorld not set yet
       			System.out.println("Error: xBaseWorld not yet set: w_Peye = b_Peye");
       	  
       	  w_Peye.transform(xBaseWorld,b_Peye);

       	  
       	  newEyePos = true;
       	  if (debug) 
       	   { 
       	     System.out.println("Fastrak: b_Peye = " + b_Peye.toString("%8.2f"));
       	     System.out.println("xBaseWorld = \n" + xBaseWorld.toString("%8.2f"));
       	     System.out.println("Fastrak:  w_Peye = " + w_Peye.toString("%8.2f"));
       	   }
       	  
       	  
             sendRecord();

           }
          else
           { System.out.println("Fastrak.parseCubeeRecord - unknown marker number " + markerNum);
           } 
    	
    }
 
    
    
    public void setXTrackerCube(RigidTransform3d X)
    {
       xTrackerCube.set(X);
       //update xBaseWorld based on new xTrackerCube
       xBaseWorld.mulInverseRight(xTrackerCube, init_xTrackerBase);
       System.out.println("set tracker cube");
    }
    
    
    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();
//           }
//
       
      if (false)
	  {
		 System.out.println("Fastrak sendRecord to " + callback.size()
			   + "callbacks:");
		 System.out.println("xCubeWorld = \n" + xCubeWorld.toString("%8.2f"));
		 System.out.println("w_Peye = " + w_Peye.toString("%8.2f"));
	  }
      
//      if (myCalibrationP)
//	  {
//		 for (int i = 0; i < callback.size(); i++)
//		 {
//			callback.get(i).setFakeData(new RigidTransform3d(xCubeWorld),
//				  calibrationEyepos.get(i));
//			newCubePose = false;
//			newEyePos = false;
//		 }
//	  } 
      
      if (staticSceneP)
      {
    	 c_Peye.inverseTransform(xCubeWorld, w_Peye);
 		 for (int i = 0; i < callback.size(); i++)
		 {
			callback.get(i).setFakeData(
				new RigidTransform3d(IdentityTransform),
				new Point3d(c_Peye));
			newCubePose = false;
			newEyePos = false;
		 }
      }
      else
	  {
		 for (int i = 0; i < callback.size(); i++)
		 {
			callback.get(i).setFakeData(
				new RigidTransform3d(xCubeWorld),
				new Point3d(w_Peye));
			newCubePose = false;
			newEyePos = false;
		 }
	  }
    }
    
    
    
    
    public void close()
    {
       if(pw!=null&&profile)
       {
    	  pw.flush();
    	  pw.close();
       }
       
       isRunning=false;
    }

    
	public static void main(String[] args) 
	{
//		if (args.length < 1) {
//			System.out.print("SimpleRead.class /dev/ttyxx\n");
//			System.exit(-1);
//		}
		
		debug = false;
		profile=false;
		
		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") ||
               			 portId.getName().equals("COM1") ) {
                		//	if (portId.getName().equals(args[0])) {
                    				try { Fastrak reader = new Fastrak(115200, false, true);}
                    				catch (Exception e) {
                    					System.err.println("Fastrak 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 isFastrakReady() {
		return fastrakReadyP;
	}


	public void setFastrakReady(boolean fastrakReady) {
		this.fastrakReadyP = fastrakReady;
	}
   
    
	private void setCallbackEyepos(SideType type)
	{
	   Point3d eyepos = new Point3d();
	   if (type != null)
	    {switch(type)
	      {
      	   case FRONT:
      	   { eyepos.set(0.0,0.0,eyeCalDistance);
      	     break;
      	   }
      	   case RIGHT:
      	   { eyepos.set(eyeCalDistance,0.0,0.0);
      	     break;
      	   }
      	   case BACK:
      	   { eyepos.set(0.0,0.0,-eyeCalDistance);
      	     break;
      	   }
      	   case LEFT:
      	   { eyepos.set(-eyeCalDistance,0.0,0.0);
      	     break;
      	   }
      	   case TOP:
      	   { eyepos.set(0.0,eyeCalDistance,0.0);
      		 break;
      	   }
      	   default:
      	   {
      		  System.err.println("Fastrak.setCallbackEyepos: " +
      		  		"bad side type: "+type.toString());
      	   }
	     }
	   }	   
	   calibrationEyepos.add(eyepos);
	}

   public void setCalibration(boolean calibration)
   {
      myCalibrationP = calibration;
   }
	
	public boolean isSceneStatic() {
		return staticSceneP;
	}

	public void setSceneStatic(boolean staticScene) {
		staticSceneP = staticScene;
	}
}