package Robot;

/*  RobotComm.java
 *
 *  Working version as of June 14, 05 @ 2:30PM (revised Sept 15, 2005)
 *
 *  This class allows communication between the robot
 *  and the computer.
 *
 *  Author: Michael A. Piskuric (2537405) Summer 2005
 */

import javax.comm.*;
import java.io.*;
import java.util.*;

public class RobotComm implements SerialPortEventListener {
	
    private int             data[] = new int[8];                // the data that is returned from the robot
    private SerialPort      port;                               // the communications port
    private OutputStream    out;                                // send data to robot
    private InputStream     in;                                 // receive data from robot
    private StringBuffer    inputBuffer = new StringBuffer();   // buffer for reading data
    private int             counter = 0;                        // used for knowing when to stop looping
    private boolean         isOpen = false;                     // is the port open?
    private boolean         complete = false;                   // is all data received?
    private double          timeout = Integer.MAX_VALUE;        // the timeout for communication
    public static final int SUCCESSFUL = 1;                     // public static variables below
    public static final int FAILURE = 0;
    public static final int STOPPED = 2;
    public static final int RESULT = 0;
    public static final int BASE = 1;
    public static final int SHOULDER = 2;
    public static final int ELBOW = 3;
    public static final int RIGHTWRIST = 4;
    public static final int LEFTWRIST = 5;
    public static final int GRIPPER = 6;
    public static final int OUTBYTE = 7;
    
    // default constructor opens a new port
    public RobotComm (String wp) {
        openPort(wp.trim());
    }
  
    /////////////////////////////////////////
    // Communication with the robot below
    //
    // The following seven (7) methods send commands
    // to the robot and return a single value depending
    // on the success of the execution. The values returned are:
    //
    // 0 = FAILURE
    // 1 = SUCCESSFUL
    // 2 = STOPPED BY PRESSING MODE BUTTON 
    //////////////////////////////////////////
    
    public int read() {
        return sendCommand("@READ\r");
    }	
    public int set(int sp) {
        return sendCommand("@SET " + sp + "\r");
    }	
    public int reset() {
        return sendCommand("@RESET\r");
    }	
    public int grip(int sp) {
        return sendCommand("@CLOSE " + sp + "\r");
    }	
    public int step(int SP, int B, int S, int E, int R, int L, int G){
        return sendCommand("@STEP "+SP+" "+B+" "+S+" "+E+" "+R+" "+L+" "+(G+E)+"\r");
    }    
    public int signal(int sig) {
        return sendCommand("@STEP 0 0 0 0 0 0 0 " + sig + "\r");
    }    
    private int sendCommand(String command){
        complete = false;
        Arrays.fill(data,0);
        double timer = 0;
        try {out.write(command.getBytes());}        
        catch(Exception e) {new TVException("Sending Error");}         
        do{timer++;}while(!complete && timer<timeout);
        return data[0];        
    }
    
    // closes the port if it is open
    public void close() {
        if(isOpen){
            port.close();
            isOpen = false;
        }
    }
    
    // Iterate through all available ports and open
    // the user selected port if available
    private void openPort(String whatPort){
        CommPortIdentifier portId;
        Enumeration en = CommPortIdentifier.getPortIdentifiers();
        
        while (en.hasMoreElements()) {
            portId = (CommPortIdentifier) en.nextElement();                
            if (portId.getPortType() == CommPortIdentifier.PORT_SERIAL) {
                if (portId.getName().equals(whatPort)) {
                    try {
                        port = (SerialPort)portId.open("RobotComm", 30000);
                        port.setSerialPortParams(9600,SerialPort.DATABITS_8,SerialPort.STOPBITS_1,SerialPort.PARITY_NONE);
                        out = port.getOutputStream();
                        in = port.getInputStream();
                        port.addEventListener(this);
                        port.notifyOnDataAvailable(true);
                        timeout = 100000000;
                        isOpen = check();
                        timeout = Integer.MAX_VALUE;
                        if(!isOpen) {
                            new TVException("Error in robot communication on " + port.getName() + ". Check connection.");
                            port.close();
                        }
                    } catch (Exception e) {new TVException(e.getMessage());}
                }
            }
        }
   }	// end openPort
    
    // Checks to see if the robot communication is possible
    // by sending a step command and checking if the returned
    // value is 1 meaning SUCCESSFUL
    private boolean check() {
        if(step(0,0,0,0,0,0,0) == SUCCESSFUL) return true;
        return false;              
    }
    
    // is the port open?
    public boolean isOpen() {
        return isOpen;
    }
        
    // returns an array of all the data in the robots registers
    public int[] getAllData() {
        return data;
    }
    
    // returns a specific data value from the robots registers
    public int getData(int i) {
        return data[i];
    }
    
    // This receives data from from the robot and consolidates
    // the data into the 8 inputs showing the values within the 
    // robots registers.
    public void serialEvent(SerialPortEvent event) {
        char newData;
        
        switch(event.getEventType()) {
        case SerialPortEvent.DATA_AVAILABLE: 
        try {
            while(in.available() > 0) {
                newData = (char)in.read();
                if((newData == 13)|| (newData ==  44)) {
                    data[counter] = new Integer(new String(inputBuffer));
                    inputBuffer = new StringBuffer();
                    counter++;
                }                
                else inputBuffer.append(newData); 
            }       				       			
        }        
        catch (Exception e) {new TVException(e.getMessage());} 
        if(counter==8 | counter == 1) {
            complete = true;
            counter = 0;
        }
        } // end case        
    }	// end serialEvent	
}