/**
 * Title:        P5 Glove LynxMotion Java Test Program
 * Description:  Drive Lynxmotion Robot Arm based upon P5 Glove Position
 * Copyright:    Copyright (c) 2004
 * Company:
 * @author Eric Lundquist 
 * @version 1.0
 *
 * <Applet code = RobotGlove.class width=800 height=600>
 * </Applet>
 */

import com.essentialreality.*;
import java.io.*;
import javax.swing.*;
import javax.swing.event.*;
import java.awt.*;
import java.awt.event.*;
import java.util.Hashtable;
import java.util.Enumeration;
import javax.comm.*;

public class RobotGlove  extends JApplet {

// Declare the sliders
   JSlider xSlider = null;
   JSlider ySlider = null;
   JSlider zSlider = null;
   JSlider rollSlider = null;
   JSlider pitchSlider = null;
   JSlider yawSlider = null;
   JSlider thumbSlider = null;
   JSlider indexSlider = null;
   JSlider middleSlider = null;
   JSlider ringSlider = null;
   JSlider pinkySlider = null;
   
   // Slider offsets
   int xZero = 0;
   int yZero = 0;
   int zZero = 0;
   
   // Other global variables
   Container container = null;
   GridLayout grid = null;
   Timer timer = null;
   
   //  The P5 Glove
   static CP5DLL gloves = null;
   
   // Serial Port Stuff
   static Enumeration portList;
   static CommPortIdentifier portId;
   static String messageString = "Hello, world!\n";
   static SerialPort serialPort;
   static OutputStream outputStream;
  
  // Servo Stuff
  int currentServoPosition[] = {0, 0, 0, 0, 0, 0, 0, 0};
  int desiredServoPosition[] = {127, 127, 127, 127, 127, 127, 127, 127};
  
   public void init() {
     // get a handle on the container
	 container = this.getContentPane();
	 container.setBackground(Color.lightGray);
     grid = new GridLayout(11, 1, 10, 10);
	 container.setLayout(grid);
	 // Create Sliders
	 xSlider = setSlider(JSlider.HORIZONTAL, true, -1024, 1024, 0, 100, 10);
	 ySlider = setSlider(JSlider.HORIZONTAL, true, -1024, 1024, 0, 100, 10);
	 zSlider = setSlider(JSlider.HORIZONTAL, true, -1024, 1024, 0, 100, 10);
	 rollSlider = setSlider(JSlider.HORIZONTAL, true, -90, 90, 0, 10, 5);
	 pitchSlider = setSlider(JSlider.HORIZONTAL, true, -90, 90, 0, 10, 5);
	 yawSlider = setSlider(JSlider.HORIZONTAL, true, -90, 90, 0, 10, 5);
	 thumbSlider = setSlider(JSlider.HORIZONTAL, true, 0, 64, 0, 10, 5);
	 indexSlider = setSlider(JSlider.HORIZONTAL, true, 0, 64, 0, 10, 5);
	 middleSlider = setSlider(JSlider.HORIZONTAL, true, 0, 64, 0, 10, 5);
	 ringSlider = setSlider(JSlider.HORIZONTAL, true, 0, 64, 0, 10, 5);
	 pinkySlider = setSlider(JSlider.HORIZONTAL, true, 0, 64, 0, 10, 5);
	 
	 // Fire off periodic timer to update sliders
	 timer = new Timer(250, new TimerListener());
	 timer.setInitialDelay(250);				 // Every 1/4 second
	 timer.start();

	 // Start up the servo thread
	 System.out.println("Starting moveServo Thread");
   	 moveServos msThread = new moveServos();
	 msThread.start();
	 System.out.println("Thread Started");
  }
  
  class TimerListener implements ActionListener {
      int val = 0;
	  Hashtable labelTable = null;
	  
      public void actionPerformed (ActionEvent e) {
 	    gloves.m_P5Devices[0].update();
		val = (new Float(gloves.m_P5Devices[0].m_fx)).intValue();
		if (gloves.m_P5Devices[0].m_byButtons[2]) {  // Button C Zeros out x,y,z
		   xZero = val;
	    }
		val = val - xZero;
		if (val < -1024) {
		   val = -1024;
		} else if (val > 1024) {
		   val = 1024;
		}
		xSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(-1023), new JLabel("X-Axis"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		xSlider.setLabelTable(labelTable);
		
		val = (new Float(gloves.m_P5Devices[0].m_fy)).intValue();
		if (gloves.m_P5Devices[0].m_byButtons[2]) {  // Button C Zeros out x,y,z
		   yZero = val;
	    }
		val = val - yZero;
		if (val < -1024) {
		   val = -1024;
		} else if (val > 1024) {
		   val = 1024;
		}
		ySlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(-1023), new JLabel("Y-Axis"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		ySlider.setLabelTable(labelTable);
		
		val = (new Float(gloves.m_P5Devices[0].m_fz)).intValue();
		if (gloves.m_P5Devices[0].m_byButtons[2]) {  // Button C Zeros out x,y,z
		   zZero = val;
	    }
		val = val - zZero;
		if (val < -1024) {
		   val = -1024;
		} else if (val > 1024) {
		   val = 1024;
		}
		zSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(-1023), new JLabel("Z-Axis"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		zSlider.setLabelTable(labelTable);
		
		val = (new Float(gloves.m_P5Devices[0].m_froll)).intValue();
		if (val < -90) {
		   val = -90;
		} else if (val > 90) {
		   val = 90;
		}
		rollSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(-89), new JLabel("Roll"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		rollSlider.setLabelTable(labelTable);
		
		val = (new Float(gloves.m_P5Devices[0].m_fpitch)).intValue();
		if (val < -90) {
		   val = -90;
		} else if (val > 90) {
		   val = 90;
		}
		pitchSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(-89), new JLabel("Pitch"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		pitchSlider.setLabelTable(labelTable);
		
		val = (new Float(gloves.m_P5Devices[0].m_fyaw)).intValue();
		if (val < -90) {
		   val = -90;
		} else if (val > 90) {
		   val = 90;
		}
		yawSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(-89), new JLabel("Yaw"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		yawSlider.setLabelTable(labelTable);
		
		val = gloves.m_P5Devices[0].m_byBendSensor_Data[0];
		if (val < 0) {
		   val = 0;
		} else if (val > 64) {
		   val = 64;
		}
		thumbSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(1), new JLabel("Thumb"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		thumbSlider.setLabelTable(labelTable);
		
		val = gloves.m_P5Devices[0].m_byBendSensor_Data[1];
		if (val < 0) {
		   val = 0;
		} else if (val > 64) {
		   val = 64;
		}
		indexSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(1), new JLabel("Index"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		indexSlider.setLabelTable(labelTable);

		val = gloves.m_P5Devices[0].m_byBendSensor_Data[2];
		if (val < 0) {
		   val = 0;
		} else if (val > 64) {
		   val = 64;
		}
		middleSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(1), new JLabel("Middle"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		middleSlider.setLabelTable(labelTable);
		
		val = gloves.m_P5Devices[0].m_byBendSensor_Data[3];
		if (val < 0) {
		   val = 0;
		} else if (val > 64) {
		   val = 64;
		}
		ringSlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(1), new JLabel("Ring"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		ringSlider.setLabelTable(labelTable);
		
		val = gloves.m_P5Devices[0].m_byBendSensor_Data[4];
		if (val < 0) {
		   val = 0;
		} else if (val > 64) {
		   val = 64;
		}
		pinkySlider.setValue(val);
		labelTable = new Hashtable();
		labelTable.put(new Integer(1), new JLabel("Pinky"));
		labelTable.put(new Integer(val), new JLabel(Integer.toString(val)));
		pinkySlider.setLabelTable(labelTable);
		
    }
  }
	
  public JSlider setSlider(int orientation,
  		 boolean paintTicks,
		 int minValue, int maxValue,
		 int initValue,
		 int majorTickSpacing,
		 int minorTickSpacing) {
		 
		 JSlider slider = new JSlider(orientation, minValue, maxValue, initValue);
		 slider.setPaintTicks(paintTicks);
		 slider.setMajorTickSpacing(majorTickSpacing);
		 slider.setMinorTickSpacing(minorTickSpacing);
		 slider.setPaintLabels(true);
		 container.add(slider);
		 return slider;
  }

  public class moveServos extends Thread {
	
    public void run() {
      byte[] servoCommand = new byte[3];
	  servoCommand[0] = (byte)0xff;
	  int tmpVal;
	  
    // First lets open up the serial port
    portList = CommPortIdentifier.getPortIdentifiers();

    while (portList.hasMoreElements()) {
        portId = (CommPortIdentifier) portList.nextElement();
        if (portId.getPortType() == CommPortIdentifier.PORT_SERIAL) {
          if (portId.getName().equals("COM1")) {
                try {
                    serialPort = (SerialPort)
                        portId.open("RobotGlove", 2000);
                } catch (PortInUseException e) {
				  		System.out.println("Port in Use "+e);
				}
                try {
                   outputStream = serialPort.getOutputStream();
                } catch (IOException e) {
				  	System.out.println("getOutputStream error "+e);
				}
                try {
                    serialPort.setSerialPortParams(9600,
                        SerialPort.DATABITS_8,
                        SerialPort.STOPBITS_1,
                        SerialPort.PARITY_NONE);
                } catch (UnsupportedCommOperationException e) {
				  		System.out.println("setSerialPortParams Failure"+e);
				} 
				try {
					serialPort.setFlowControlMode(serialPort.FLOWCONTROL_NONE);
                } catch (UnsupportedCommOperationException e) {
				  		System.out.println("setSerialPortParams Failure"+e);
				}
				 
			} /* If COM1 */
        } /* If serial port */
    } /* While */

	  while (true) {
	    // Move Base
		// Base rotates based upon glove yaw angle
		servoCommand[1] = (byte) 0x05;	 // Set servo number
		desiredServoPosition[5] = (255 * (yawSlider.getValue() + 90)) / 180;
		if (desiredServoPosition[5] == currentServoPosition[5]) {
		   currentServoPosition[5] = desiredServoPosition[5];
	    } else if (desiredServoPosition[5] > currentServoPosition[5]) {
		   currentServoPosition[5] = currentServoPosition[5] + 1;
	    } else {				   // desiredServoPosition < currentServoPosition
		   currentServoPosition[5] = currentServoPosition[5] - 1;
	    }
		servoCommand[2] = (byte)currentServoPosition[5];
        try {
             outputStream.write(servoCommand);
        } catch (IOException e) {
		   System.out.println("Serial port write error "+e);
		   System.exit(0);
	    }
		try {
		     outputStream.flush();	  // Wait for last write to finish before starting another
		} catch (IOException e) {
		   System.out.println("Serial Port flush error "+e);
		}

		// Move "shoulder" based  upon y-axis value
		servoCommand[1] = (byte) 0x00;	 // Set servo number
		desiredServoPosition[0] = (255 * (ySlider.getValue() + 1024)) / 2048;
		if (desiredServoPosition[0] == currentServoPosition[0]) {
		   currentServoPosition[0] = desiredServoPosition[0];
	    } else if (desiredServoPosition[0] > currentServoPosition[0]) {
		   currentServoPosition[0] = currentServoPosition[0] + 1;
	    } else {				   // desiredServoPosition < currentServoPosition
		   currentServoPosition[0] = currentServoPosition[0] - 1;
	    }
		servoCommand[2] = (byte)currentServoPosition[0];
        try {
             outputStream.write(servoCommand);
        } catch (IOException e) {
		   System.out.println("Serial port write error "+e);
		   System.exit(0);
	    }
		try {
		     outputStream.flush();	  // Wait for last write to finish before starting another
		} catch (IOException e) {
		   System.out.println("Serial Port flush error "+e);
		}

		// Move "wrist" based  upon pitch value
		servoCommand[1] = (byte) 0x03;	 // Set servo number
		desiredServoPosition[3] = (255 * (pitchSlider.getValue() + 90)) / 180;
		if (desiredServoPosition[3] == currentServoPosition[3]) {
		   currentServoPosition[3] = desiredServoPosition[3];
	    } else if (desiredServoPosition[3] > currentServoPosition[3]) {
		   currentServoPosition[3] = currentServoPosition[3] + 1;
	    } else {				   // desiredServoPosition < currentServoPosition
		   currentServoPosition[3] = currentServoPosition[3] - 1;
	    }
		servoCommand[2] = (byte)currentServoPosition[3];
        try {
             outputStream.write(servoCommand);
        } catch (IOException e) {
		   System.out.println("Serial port write error "+e);
		   System.exit(0);
	    }
		try {
		     outputStream.flush();	  // Wait for last write to finish before starting another
		} catch (IOException e) {
		   System.out.println("Serial Port flush error "+e);
		}

		// Move "elbow" based  upon xaxis value
		servoCommand[1] = (byte) 0x04;	 // Set servo number
		desiredServoPosition[4] = (255 * (xSlider.getValue() + 1024)) / 2048;
		if (desiredServoPosition[4] == currentServoPosition[4]) {
		   currentServoPosition[4] = desiredServoPosition[4];
	    } else if (desiredServoPosition[4] > currentServoPosition[4]) {
		   currentServoPosition[4] = currentServoPosition[4] + 1;
	    } else {				   // desiredServoPosition < currentServoPosition
		   currentServoPosition[4] = currentServoPosition[4] - 1;
	    }
		servoCommand[2] = (byte)currentServoPosition[4];
        try {
             outputStream.write(servoCommand);
        } catch (IOException e) {
		   System.out.println("Serial port write error "+e);
		   System.exit(0);
	    }
		try {
		     outputStream.flush();	  // Wait for last write to finish before starting another
		} catch (IOException e) {
		   System.out.println("Serial Port flush error "+e);
		}

		// Move "gripper" based  upon  ndex finger value
		servoCommand[1] = (byte) 0x02;	 // Set servo number
		desiredServoPosition[2] = (255 * (indexSlider.getValue() + 00)) / 64;
		desiredServoPosition[2] = 255 - desiredServoPosition[2];  // Invert so that fist closes gripper
		if (desiredServoPosition[2] == currentServoPosition[2]) {
		   currentServoPosition[2] = desiredServoPosition[2];
	    } else if (desiredServoPosition[2] > currentServoPosition[2]) {
		   currentServoPosition[2] = currentServoPosition[2] + 1;
	    } else {				   // desiredServoPosition < currentServoPosition
		   currentServoPosition[2] = currentServoPosition[2] - 1;
	    }
		servoCommand[2] = (byte)currentServoPosition[2];
        try {
             outputStream.write(servoCommand);
        } catch (IOException e) {
		   System.out.println("Serial port write error "+e);
		   System.exit(0);
	    }
		try {
		     outputStream.flush();	  // Wait for last write to finish before starting another
		} catch (IOException e) {
		   System.out.println("Serial Port flush error "+e);
		}

	  }
	}
  }	 
    
  public static void main(String[] args) {
    System.out.println("P5Glove Lynxmotion Test Program v1.0");
	// Create the main glove object
    gloves = new CP5DLL();
	// Initialize the glove
    gloves.P5_Init();
	gloves.P5_SetForwardZ(-1);
	gloves.P5_SetFilterMode((int)gloves.P5_FILTER_DEADBAND | (int)gloves.P5_FILTER_AVERAGE);
	gloves.P5_SetPrediction(1);
	gloves.P5_SetFilterAmount(20, (float)14.0);
    System.out.println(gloves.m_nNumP5);
    System.out.println("m_nNumP5='"+gloves.m_nNumP5+"'");
    System.out.println("VendorID = '" + gloves.m_P5Devices[0].VendorID + "', ProductID = '" + gloves.m_P5Devices[0].ProductID + "', Version = '" + gloves.m_P5Devices[0].Version + "'");
    System.out.println("ProductString = '" + gloves.m_P5Devices[0].ProductString + "', ProductID = '" + gloves.m_P5Devices[0].ManufacturerString + "', Version = '" + gloves.m_P5Devices[0].SerialNumString + "'");
    System.out.println("m_nDeviceID = '" + gloves.m_P5Devices[0].m_nDeviceID + "'");
    System.out.println("m_nMajorRevisionNumber = '" + gloves.m_P5Devices[0].m_nMajorRevisionNumber + "', m_nMinorRevisionNumber = '" + gloves.m_P5Devices[0].m_nMinorRevisionNumber+"'");
    System.out.println("m_nGloveType = '" + gloves.m_P5Devices[0].m_nGloveType+"'");
	for (int i=0; i<3; i++) {
	   gloves.m_P5Devices[0].updateFull();
	   int skipCounter = 0;
	   while (gloves.m_P5Devices[0].m_fx == 0.0) {
	   		 gloves.m_P5Devices[0].updateFull();
			 skipCounter++;
	   }
       System.out.println("m_fx = '"+gloves.m_P5Devices[0].m_fx+"'");
	   System.out.println("m_fyaw= "+gloves.m_P5Devices[0].m_fyaw);
	   System.out.println("m_byBendSensor_Data="+gloves.m_P5Devices[0].m_byBendSensor_Data[0]);
	   System.out.println("skipCounter="+skipCounter);
	   // System.out.println("m_byButtons="+gloves.m_byButtons[2]);
	}
	// Do the graphics thing
	JFrame f = new JFrame("P5Glove Lynxmotion");
	RobotGlove robotGlove = new RobotGlove();
	robotGlove.init();
	f.getContentPane().add(robotGlove);
	f.addWindowListener(new WindowEventHandler());
	f.setSize(800, 600);
	f.show();
	
  } /* Main */

  public static class WindowEventHandler extends WindowAdapter {
	public void windowClosing(WindowEvent e) {
		   gloves.P5_Close();
		   serialPort.close();
		   System.exit(0);
		}
  }
}