/*
 * Logitech 3D Mouse driver for Java3D
 * Author: Eric Engstrom
 * Copyright (C) 2000 Eric Engstrom.
 * v0.1
 */

//package edu.calpoly.eengstro;

import javax.media.j3d.InputDevice;
import javax.media.j3d.Sensor;
import javax.media.j3d.SensorRead;
import javax.media.j3d.Transform3D;
import javax.vecmath.Vector3d;
import java.io.BufferedInputStream;
import java.io.OutputStream;
import java.util.Enumeration;
import java.util.Date;
import javax.comm.*;
import java.lang.Integer;
import java.lang.reflect.Array;

/**
 * <P>This is a device driver for the Logitech 3D Mouse, for the Java3D API.
 * It implements the InputDevice interface. The driver provides one Sensor
 * for the 3D Mouse on its port. The default port is COM1.
 * <P>This driver requires Java3D, JavaCOMM, and JDK1.2.
 * @author  Eric Engstrom
 * @version v0.2 February 2000
 */

public class Logitech3DMouse implements 
   InputDevice, SerialPortEventListener
{
//--------------------------------------------------------------------------
// D A T A   M E M B E R S
//--------------------------------------------------------------------------

   // mouse & sensor data members
   private Sensor[] maSensorArray;                //array of sensors
   private int mnProcessMode;                     //processing mode   
   private int[] manButtons;                      //3D mouse buttons

   // port & stream data members
   private String mzPortName;                     //where the 3D mouse is
   private SerialPort moSerialPort;               //the serial port 
   private BufferedInputStream moBufInputStream;  //buffered input stream
   private OutputStream moOutputStream;           //the output stream

   private Date moDate;                           //used for timestamping

   // raw Rotation and Translation as read from the 3D Mouse
   private Vector3d moRawRotVec;                  //current rotation
   private Vector3d moRawTransVec;                //current X, Y, Z

   // Nominal Rotation and Translation. Subtracted from the Raw
   // Rotation and Translation.
   private Vector3d moNominalRotVec;
   private Vector3d moNominalTransVec;

   private Transform3D moCurrentTrans3D;          //current Transform3D

   // strings to be sent to the 3D mouse
   private byte[] DEMAND_MODE = {0x2A, 0x44};    //puts 3D mouse in Demand mode
   private byte[] DEMAND_REPORT = {0x2A, 0x64};  //demands one report
   private byte[] STREAM_MODE = {0x2A, 0x53};    //puts 3D mouse in Stream mode
   private byte[] RESET = {0x2A, 0x52};          //resets 3D mouse
   private byte[] DIAGNOSTIC = {0x2A, 0x05};     //run diagnostics
   
   // Constants
   private double mdToRadians = Math.PI / 7200.0;  //conv. data to radians
   private double mdToInches = 0.001;              //conv. data to inches
   private double mdInchesToMeters = 1.0 / 39.25;  // 39.25 inches per meter
   private double mdToMeters = mdToInches * mdInchesToMeters;

//--------------------------------------------------------------------------
// P U B L I C   F I E L D S
//--------------------------------------------------------------------------

   /**
    * The Fringe bit's index into the button state array. If the value
    * at this index is 1, then the receiver has entered the fringe space.
    */
   public static final int FRINGE_BIT_IDX = 0;

   /**
    * The Out bit's index into the button state array. If the value
    * at this index is 1, then the receiver is out of range, and the 
    * report is invalid.
    */
   public static final int OUT_BIT_IDX = 1;

   /**
    * The Mouse stand button's index in the button state array. If the
    * value at this index is 1, then the mouse stand's button is down.
    */
   public static final int STAND_BUTTON_IDX = 2;

   /**
    * The Suspend button's index in the button state array. If the value
    * at this index is 1, then the Suspend button is down.
    */
   public static final int SUSPEND_BUTTON_IDX = 3;

   /**
    * The Left button's index in the button state array. If the value
    * at this index is 1, then the Left button is down.
    */
   public static final int LEFT_BUTTON_IDX = 4;

   /**
    * The Middle button's index in the button state array. If the value
    * at this index is 1, then the Middle button is down.
    */
   public static final int MIDDLE_BUTTON_IDX = 5;

   /**
    * The Right button's index in the button state array. If the value
    * at this index is 1, then the Right button is down.
    */
   public static final int RIGHT_BUTTON_IDX = 6;

   /**
    * The length of the Sensor's button state array.
    */
   public static final int BUTTON_STATE_ARRAY_LENGTH = 7;


//--------------------------------------------------------------------------
// P U B L I C   M E T H O D S
//--------------------------------------------------------------------------

   /**
    * Constructor. 
    */
   public Logitech3DMouse()
   {
      maSensorArray = new Sensor[1];
      maSensorArray[0] = new Sensor(this, Sensor.DEFAULT_SENSOR_READ_COUNT, BUTTON_STATE_ARRAY_LENGTH);
      mzPortName = "COM1";
      mnProcessMode = NON_BLOCKING;
      moDate = new Date();
      manButtons = new int[BUTTON_STATE_ARRAY_LENGTH];

      moRawRotVec = new Vector3d(0.0, 0.0, 0.0);
      moRawTransVec = new Vector3d(0.0, 0.0, 0.0);
      moNominalRotVec = new Vector3d(0.0, 0.0, 0.0);
      moNominalTransVec = new Vector3d(0.0, 0.0, 0.0);
      moCurrentTrans3D = new Transform3D(); //identity matrix
   }


   /**
    * Set the port the 3D Mouse is on. Should be called before
    * initialize().
    * @param zPortName the port name
    */
   public void setPort(String zPortName)
   {
      mzPortName = zPortName;
   }


   /**
    * Initialize the 3D Mouse, and begin taking readings. Returns true
    * on successful initialization. Prints an error message to stderr
    * if the initialization fails, and returns false. 
    * @return bool true if initialization succeeded, false otherwise
    */
   public boolean initialize()
   {
      CommPortIdentifier oPortId = null;
      Enumeration ePortList;
      boolean bFoundPort = false;
      
      ePortList = CommPortIdentifier.getPortIdentifiers();

      //iterate through, looking for the port named in mzPortName
      while ((ePortList.hasMoreElements()) && (bFoundPort == false))
      {
         oPortId = (CommPortIdentifier) ePortList.nextElement();
         if (oPortId.getName().equals(mzPortName))
         {
            //found the port
            bFoundPort = true;
         }
      }

      //if port was found, open it
      if (bFoundPort == true)
      {
         try
         {
            //open serial port
            moSerialPort = (SerialPort) oPortId.open(
               "Logitech3DMouse", 2000);

            //set port parameters
            moSerialPort.setSerialPortParams(19200,
               SerialPort.DATABITS_8,
               SerialPort.STOPBITS_1,
               SerialPort.PARITY_NONE);

            //open the streams
            moBufInputStream = new BufferedInputStream(
               moSerialPort.getInputStream(), 16);
            moOutputStream = moSerialPort.getOutputStream();

            //write init string to 3D mouse
            moOutputStream.write(RESET);
            Thread.sleep(1000); //must wait >1 sec for hardware

            //run diagnostics
            if (bRunDiagnostics() == false) //diagnostics failed
            {
               vPrintErr("initialize()", mzPortName, 
                  "Diagnostics failed.");
               close();
               return false;
            }

            //add event listener
            moSerialPort.addEventListener(this);
            moSerialPort.notifyOnDataAvailable(true);

            //put 3D Mouse into stream reporting mode
            moOutputStream.write(STREAM_MODE); //DEMAND_MODE);
         }
         catch (Exception e)
         {
            vPrintErr("initialize()", mzPortName, e.toString());
            close();
            return false;
         }
      } 
      else //port wasn't ever found.
      {
         vPrintErr("initialize()", mzPortName, 
            "Could not find specified port.");
         return false;
      }
      //OK, if we've made it this far, we're good to go.
      return true; 
   }


   /**
    * Set the 3D Mouse's processing mode, either BLOCKING, NON_BLOCKING,
    * or DEMAND_DRIVEN.
    * @param nMode the mode, either BLOCKING, NON_BLOCKING, or DEMAND_DRIVEN.
    */
   public void setProcessingMode(int nMode)
   {
     System.out.println("setProcessingMode()");
     /*
     switch(nMode) {
     case InputDevice.BLOCKING:
       System.out.println("BLOCKING");
       break;
     case InputDevice.DEMAND_DRIVEN:
       System.out.println("DEMAND_DRIVEN");
       break;
     case InputDevice.NON_BLOCKING:
       System.out.println("NON_BLOCKING");
       break;
     default:
       System.out.println("UNRECOGNIZED");
       break;
     }
     */
      mnProcessMode = nMode;
   }


   /**
    * Return the 3D Mouse's processing mode, either BLOCKING, NON_BLOCKING, 
    * or DEMAND_DRIVEN.
    * @return int the processing mode, either BLOCKING, NON_BLOCKING, or
    *             DEMAND_DRIVEN.
    */
   public int getProcessingMode()
   {
     System.out.println("getProcessingMode()");
      return mnProcessMode;
   }


   /**
    * Return the sensor count.
    * @return int number of Sensors
    */
   public int getSensorCount()
   {
      return Array.getLength(maSensorArray);
   }


   /**
    * Return the indicated Sensor.
    * @param sensorIndex the index of the Sensor to return
    * @return Sensor the indicated Sensor
    */
   public Sensor getSensor(int sensorIndex)
   {
      return maSensorArray[sensorIndex];
   }


   /**
    * Set the 3D Mouse's current location & rotation as the nominal
    * position and orientation.
    */
   public void setNominalPositionAndOrientation()
   {
      moNominalRotVec.set(moRawRotVec);
      moNominalTransVec.set(moRawTransVec);
   }


   /**
    * Poll and process one reading from the 3D Mouse. Called when a new
    * reading is requested by the device's Sensor.
    */
   public void pollAndProcessInput()
   {
       //System.out.println("pollAndProcessInput()");
      //set Sensor's SensorRead
      maSensorArray[0].setNextSensorRead(moDate.getTime(), moCurrentTrans3D, manButtons);
   }


   /**
    * "This method will not be called by the Java 3D implementation and
    * should be implemented as an empty method."
    */
   public void processStreamInput() {}


   /**
    * Close the 3D Mouse, and release its resources.
    */
   public void close()
   {
      try
      {
         moOutputStream.write(DEMAND_MODE);
         Thread.sleep(10); //allow enough time for last report
         moOutputStream.write(RESET);
         Thread.sleep(10);
         moBufInputStream.close();
         moOutputStream.close();
         moSerialPort.close();
      }
      catch (Exception e)
      {
         vPrintErr("close()", mzPortName, e.toString());
      }
   }


   /**
    * Handle an event on the serial port. Read the data, decode it, and 
    * store it.
    */
   public void serialEvent(SerialPortEvent oEvent)
   {
      switch (oEvent.getEventType())
      {
      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:
         break;
      case SerialPortEvent.DATA_AVAILABLE:
         try
         {
            byte[] baReport = new byte[16];
            
            //read 16 bytes
            while (moBufInputStream.available() < 16)
               Thread.sleep(1);
            moBufInputStream.read(baReport, 0, 16);
         
            //decode and store in Sensor
            decodeReport(baReport);
         }
         catch (Exception e)
         {
            vPrintErr("serialEvent()", mzPortName, e.toString());
         }
         break;
      } //switch
   }
   

//--------------------------------------------------------------------------
// P R I V A T E   M E T H O D S
//--------------------------------------------------------------------------

   /**
    * Decode the data stored in the 16-byte array, and write it to the
    * rotation and translation vectors, and the Transform3D member.
    * @param baReport one data report, 16 bytes long
    */
   private void decodeReport(byte[] baReport)
   {
      int[] naData = new int[6]; //array of data decoded from baReport
      int naDataIdx; //index into naData array
      int baReportIdx = 1; //index ito baReport array

      //decode fringe, out, and button bits; set button state array
      if ((baReport[0] & 0x40) == 0x40)
         manButtons[FRINGE_BIT_IDX] = 1;
      else
         manButtons[FRINGE_BIT_IDX] = 0;
      if ((baReport[0] & 0x20) == 0x20)
         manButtons[OUT_BIT_IDX] = 1;
      else
         manButtons[OUT_BIT_IDX] = 0;
      if ((baReport[0] & 0x10) == 0x10)
         manButtons[STAND_BUTTON_IDX] = 1;
      else
         manButtons[STAND_BUTTON_IDX] = 0;
      if ((baReport[0] & 0x08) == 0x08)
         manButtons[SUSPEND_BUTTON_IDX] = 1;
      else
         manButtons[SUSPEND_BUTTON_IDX] = 0;
      if ((baReport[0] & 0x04) == 0x04)
         manButtons[LEFT_BUTTON_IDX] = 1;
      else
         manButtons[LEFT_BUTTON_IDX] = 0;
      if ((baReport[0] & 0x02) == 0x02)
         manButtons[MIDDLE_BUTTON_IDX] = 1;
      else
         manButtons[MIDDLE_BUTTON_IDX] = 0;
      if ((baReport[0] & 0x01) == 0x01)
         manButtons[RIGHT_BUTTON_IDX] = 1;
      else
         manButtons[RIGHT_BUTTON_IDX] = 0; 
      
      //decode translation
      for (naDataIdx = 0; naDataIdx < 3; naDataIdx++)
      {
         naData[naDataIdx] = baReport[baReportIdx++];
         naData[naDataIdx] = naData[naDataIdx] << 7;
         naData[naDataIdx] = naData[naDataIdx] | baReport[baReportIdx++];
         naData[naDataIdx] = naData[naDataIdx] << 7;
         naData[naDataIdx] = naData[naDataIdx] | baReport[baReportIdx++];
         //fix negative
         if ((naData[naDataIdx] & 0x00100000) == 0x00100000)
            naData[naDataIdx] = naData[naDataIdx] | 0xFFF00000;
      }

      //decode rotation
      for (naDataIdx = 3; naDataIdx < 6; naDataIdx++)
      {
         naData[naDataIdx] = baReport[baReportIdx++];
         naData[naDataIdx] = naData[naDataIdx] << 7;
         naData[naDataIdx] = naData[naDataIdx] | baReport[baReportIdx++];
      }

      //convert XYZ to inches, rotations to radians
      double x = ((double) naData[0]) * mdToMeters;
      double y = ((double) naData[1]) * mdToMeters;
      double z = ((double) naData[2]) * mdToMeters;
      double rx = ((double) naData[3]) * mdToRadians;
      double ry = ((double) naData[4]) * mdToRadians;
      double rz = ((double) naData[5]) * mdToRadians;

      //set rotation (must do this first)
      Vector3d oNewRot = new Vector3d(moRawRotVec);
      oNewRot.sub(moNominalRotVec);
      moRawRotVec.set(rx, ry, rz);
      moCurrentTrans3D.setEuler(oNewRot);

      //set translation
      Vector3d oNewTrans = new Vector3d(moRawTransVec);
      oNewTrans.sub(moNominalTransVec);
      moRawTransVec.set(x, y, z);
      moCurrentTrans3D.setTranslation(oNewTrans);
   }


   /**
    * Run diagnostics on the 3D Mouse. Assumes the unit has been reset,
    * and is not broadcasting data.
    * @return boolean true if tests were passed, false if tests were failed
    */
   private boolean bRunDiagnostics()
   {
      byte[] abTemp = new byte[2];

      try
      {
         moBufInputStream.skip(moBufInputStream.available()); //flush stream

         moOutputStream.write(DIAGNOSTIC);
         Thread.sleep(500);
         if (moBufInputStream.available() < 2)
         {
            vPrintErr("bRunDiagnostics()", mzPortName, 
               "3D Mouse not responding.");
            return false; //3D Mouse didn't respond
         }
         
         moBufInputStream.read(abTemp, 0, 2);

         //return of 0xBF, 0x3F means diagnostics were passed.
         //any other return means a test was failed.
         if ((abTemp[0] != (byte) 0xBF) || (abTemp[1] != (byte) 0x3F))
            return false; //tests failed

         moBufInputStream.skip(moBufInputStream.available()); //flush stream
      }
      catch (Exception e) 
      {
         vPrintErr("bRunDiagnostics()", mzPortName, e.toString());
         return false;
      }

      return true;
   }


   /**
    * Print an error message to stderr.
    * @param zFunctionName the name of the calling function
    * @param zPortName the port where the error occurred
    * @param zMsg the message to output
    */
   private void vPrintErr(String zFunctionName, String zPortName, String zMsg)
   {
      System.err.println("*** Logitech3DMouse." + zFunctionName);
      System.err.println(zPortName);
      System.err.println(zMsg);
   }
}

// E N D   F I L E   ------------------------------------------------------