package Robot;

/*  Microbot.java
 *
 *  The Microbot class is the class that the user will need
 *  to communicate to the robot. The Microbot class communicates
 *  with the RobotComm class. 
 *
 *  Microbot ports the original TeachVAL commands and allows 
 *  Java control over the robot.
 * 
 *  Author: Michael A. Piskuric (2537405) Summer 2005
 */

public class Microbot {
    
    public static final Location HOME = new Location(5,0,0,-90,0,"HOME");   // The home location
    public static final int FAST = 240;                                     // default speed
    public static final int MED = 190;
    public static final int SLOW = 140;
    private int         speed = FAST;                                       // speed of movement
    private RobotComm   comm;                                               // communications port
    private Location    home = HOME;                                        // default Location
    private Location    currentLocation = home;                             // the current robot location
    private int         currentOpening = 0;                                 // the current gripper opening
    private boolean     allowExecute = false;                               // is execution allowed?
    private int[]       outBits = {0,0,0,0,0};                              // buffer for outbits
    
    // The default constructor that opens a communications port
    // and allows communication between the robot
    // the default home location is given above
    public Microbot (String whatPort) {
        comm = new RobotComm(whatPort);
        if(comm.isOpen()) allowExecute = true;
    }
    
    // The secondary constructor that opens up a communication 
    // port and allows the user to specify their own default location
    public Microbot (String whatPort, Location h) {
        comm = new RobotComm(whatPort);
        if(comm.isOpen()) allowExecute = true;
        currentLocation = home = h;
    }
    
    //////////////////////////////////////////////
    // ACTUAL TEACHVAL COMMANDS
    // 
    // These commands are outlined in the manual
    // and will do what the name of the method
    // says. They are exact ports of the original 
    // TeachVAL commands.
    //
    // Some of the methods have 2 or more versions
    // that allow default values
    /////////////////////////////////////////////
   
    public void speed(int sp) {
        if(sp<SLOW) speed = SLOW;
        else if (sp>FAST) speed = FAST;
        else speed = sp;
    }
    public void delay(double sec) {
        long time = (long)(sec*1000);
        try{synchronized (this) {wait(time);}}
        catch(Exception e){new TVException(e.getMessage());};
    }
    public void delay() {
        delay(2);
    }
    public void ready() {
        close();
        move(home);
    }
    public void type(String text) {
        System.out.println(text);
    }
    public void close() {
        if(allowExecute){
            comm.grip(speed); 
            currentOpening = read(comm.GRIPPER) - read(comm.ELBOW);
        }
    }
    public void close(double opening) {
        closei(opening);
    }
    public void closei() {        
        close();
    }
    public void closei(double opening) {        
        if(opening > 0) openClose(opening);
        else close();
    }    
    public void open(){
        openi(2);
    }
    public void open(double opening){
        openi(opening);
    }
    public void openi() {
        open();
    }
    public void openi(double opening){
        openClose(opening);
    }    
    public void move(Location l) {
        if(currentLocation != null && l != null && l.isValid()) {
            int steps[] = currentLocation.move(l);
            if(allowExecute) {
                if(comm.step(speed,steps[0],steps[1],steps[2],steps[3],steps[4],0) != RobotComm.SUCCESSFUL) allowExecute = false;
                else setCurrentLocation(new Location(l.getX(),l.getY(),l.getZ(),l.getPitch(),l.getRoll(),l.getName()));
            }            
        }
    }    
    public void movet(Location l, double opening) {
        openi(opening);
        move(l);
    }
    public void moves(Location l) {
        move(l);
    }
    public void movest(Location l, double opening) {
        movet(l,opening);
    }    
    public void shift(Location l, double x, double y, double z) {
        currentLocation = new Location(l.getX(),l.getY(),l.getZ(),l.getPitch(),l.getRoll(),l.getName());
        l.shift(x,y,z);
    }    
    public void appros(Location l, double offset) {
        appro(l, offset);
    }
    public void appro(Location l, double offset) {
        Location temp = new Location(l.getX(),l.getY(),(l.getZ()+offset),l.getPitch(),l.getRoll(),null);
        move(temp);
    }
    public void depart(double offset) {
        appro(currentLocation, offset);
    }
    public void departs(double offset) {
        appros(currentLocation, offset);
    }        
    public void draw(double x, double y, double z){
        double xx = currentLocation.getX()+x;
        double yy = currentLocation.getY()+y;
        double zz = currentLocation.getZ()+z;
        double p = currentLocation.getPitch();
        double r = currentLocation.getRoll();
        move(new Location(xx,yy,zz,p,r,null));        
    }    
    public void wait(int bit){
        if(bit>0)while(getInputBit(bit) != 1)delay(0.05);        
        else while(getInputBit(Math.abs(bit)) != 0)delay(0.05);
    }
    public void wait(int bits[]) {
        int theByte;
        boolean done = false;        
        while(!done) {
            theByte = getInByte();
            done = true;
            for(int i=0; i<bits.length; i++) {
                if(bits[i]>0){
                    if(((theByte&(1<<bits[i]))/(1<<bits[i])) == 0) done = false;                
                }
                else if(bits[i]<0) {
                    if(((theByte&(1<<Math.abs(bits[i])))/(1<<Math.abs(bits[i]))) != 0) done = false;                
                }
            }
        }       
    }    
    public void signal(int bit) {
        if(bit>0) outBits[bit-1] = 1;
        else outBits[Math.abs(bit)-1] = 0;   
        doSignal();                    
    }    
    public void signal(int bits[]) {
        for(int i=0; i<bits.length; i++) {
            if(bits[i]>0) outBits[bits[i]-1] = 1;
            else outBits[Math.abs(bits[i])-1] = 0;    
        }
        doSignal();
    }   
    public double grasp() {
        if(allowExecute && comm.grip(speed) != RobotComm.SUCCESSFUL) allowExecute = false;
        currentOpening = read(comm.GRIPPER) - read(comm.ELBOW);
        if(allowExecute && comm.step(speed,0,0,0,0,0,-50) != RobotComm.SUCCESSFUL) allowExecute = false;                 
        currentOpening = read(comm.GRIPPER) - read(comm.ELBOW);
        return currentOpening/371.0;
    }
    public Location here(String name) {
        if(comm.read() == RobotComm.SUCCESSFUL) {
            int[] data = comm.getAllData();
            int b = currentLocation.getBase() + data[1];
            int s = currentLocation.getShoulder() + data[2];
            int e = currentLocation.getElbow() + data[3];
            int rw = currentLocation.getRightWrist() + data[4];
            int lw = currentLocation.getLeftWrist() + data[5];
            int g = currentLocation.getGripper()+ data[6];
            return new Location(b,s,e,rw,lw,g,name);
        }
        return null;
    }
    private void doSignal() {
        int temp = 0;
        for(int i=0;i<5; i++) {
            if(outBits[i] == 1) temp += Math.pow(2,i+1);
        }
        if(allowExecute) comm.signal(temp);
    }
    private void openClose(double desiredOpening) {
        desiredOpening *= 371;
        int steps = (int)desiredOpening - currentOpening;        
        if(allowExecute) {
            if(comm.step(speed,0,0,0,0,0,steps) != RobotComm.SUCCESSFUL) allowExecute = false;
            else currentOpening = read(comm.GRIPPER) - read(comm.ELBOW);
        }
    }    
    
    ////////////////////////////////////////////////
    // METHODS TO SET VARIABLES
    ////////////////////////////////////////////////
    public void setCurrentLocation (Location l) {
        currentLocation = l;
    }        
    public void setHome(Location h) {
        home = h;
    }   
    
    ///////////////////////////////////////////////
    // ACCESSOR METHODS BELOW
    ///////////////////////////////////////////////
    public int getSpeed() {
        return speed;
    }
    
    // This will get the user the requested bit within 
    // the input byte. Useful for testing
    public int getInputBit(int what){
        int bitmask = 1<<what;
        if (what<8 & what>=0)return (getInByte() & bitmask)/bitmask;
        else return -1;
    }
    
    // This gets the last key that was pressed on the 
    // TeachControl pendant.
    public int getLastKeyPressed() {
        comm.read();
        return comm.getData(comm.OUTBYTE)/256; 
    }
    
    // This returns the entire input byte
    public int getInByte() {
        return read(comm.OUTBYTE)%256;
    }
    
    // Returns the current location of the robot
    public Location getCurrentLocation() {
        return currentLocation;
    }
    
    // Returns the current saved home location
    public Location getHome() {
        return home;
    }
    
    // Returns any of the bytes that are stored within 
    // the robot. Can read the gripepr motors and input bits
    private int read(int what) {
        comm.read();
        return comm.getData(what);
    }
    ///////////////////////////////////////////////////
    // OTHER MISC METHODS
    /////////////////////////////////////////////////
    
    // Closes the communications port.
    public void closePort() {
        if(comm.isOpen())comm.close();
    }
    
    // Resets the registers within the robot.
    public void reset() {
        comm.reset();
    }  
}   // end Microbot class.