package Robot;

/*  Location.java
 *
 *  Revision made Sept 13 2005. Removed all references to MILLIMETERS
 *  will only use INCHES from now on.
 *
 *  This class now properly converts one dimension to another.
 *  The only thing left now is to do error checking.
 *
 *  Author: Michael A. Piskuric (2537405) Summer 2005
 */

public class Location {
    
    private static final double C = 2*Math.PI;                          //# of radians
    private static final double B=7072/C,S=7072/C,E=4158/C,W=1536/C;    //# of motor steps in 1 radian    
    private static final double H=7.68,L=7.00,LL=3.8;                   //lenghts of the arms and height of shoulder
    private double X,Y,Z,PITCH,ROLL;                                    //cartesian values
    private int BASE,SHOULDER,ELBOW,RIGHTWRIST,LEFTWRIST,GRIPPER;       //stepper motor values
    private String NAME;
    private String error = "";
    private boolean valid = true;
    private double OS = 1.625;
    
    // The base Location constructor that takes an X,Y,Z,Pitch,Roll and creates
    // a Location object that can be used in Java. It uses the BACKWARD SOLUTION
    public Location(double x, double y, double z, double p, double r, String n) {
        X = x - OS;
        Y = y;
        Z = z;
        PITCH = p;
        ROLL = r;
        BASE = findBase(X+OS,Y);
        double RR = getRR(X+OS,Y);
        double R0 = getR0(RR,PITCH);
        double alpha = findAlpha(RR,R0,Z,PITCH);
        double beta = findBeta(RR,R0,Z,PITCH);
        SHOULDER = findShoulder(alpha,beta);
        ELBOW = findElbow(alpha,beta);
        RIGHTWRIST = findRightWrist(PITCH, ROLL);
        LEFTWRIST = findLeftWrist(PITCH, ROLL);
        NAME = n;
        //checkValidity();
        if(!valid) new TVException(getError());
    }   //end backward solution
    
    // The secondary constructor that takes a BASE,SHOULDER,ELBOW,RIGHT WRIST
    // LEFT WRIST and GRIPPER stepper motors and creates a Location object
    public Location(int b, int s, int e, int r, int l, int g, String n) {
        BASE = b;
        SHOULDER = s;
        ELBOW = e;
        RIGHTWRIST = r;
        LEFTWRIST = l;
        GRIPPER = g;
        PITCH = Math.toDegrees(findP(RIGHTWRIST,LEFTWRIST));
        ROLL = Math.toDegrees(findR(RIGHTWRIST,LEFTWRIST));
        Z = findZ(SHOULDER,ELBOW,PITCH);
        X = (findX(BASE,SHOULDER,ELBOW,PITCH)-OS);
        Y = findY(BASE,SHOULDER,ELBOW,PITCH);
        NAME = n;
        //checkValidity();
        if(!valid) new TVException(getError());
    }   // steps to points
        
    private void checkValidity() {
        if(BASE<0){error += "Base out of range.";valid=false;} 
        
        double RR = getRR(X+OS,Y);        
        if((RR<2.25) && (Z<15)){error+="Hand too close to body.";valid=false;}
        if((RR>17.8)){error+="Reach out of range.";valid=false;}
        
        double R0 = getR0(RR,PITCH);        
        if((X<2.25) && (Z<1.25) && (R0<3.5)){
            if(PITCH<(-90/C)) {
                error += "Hand interference with base."; valid=false;}}
        
        
        
    }
    
    // Takes Location components and and calls the other move method
    public int[] move(double x, double y, double z, double p, double r) {
        return move(new Location(x,y,z,p,r,null));
    }
    
    // Accept a location object that we wanna move to and
    // calculate the number of motor steps that are required
    public int[] move(Location l) {
        int steps[] = new int[5];        
        steps[0] = l.getBase() - BASE;
        steps[1] = l.getShoulder() - SHOULDER;
        steps[2] = l.getElbow() - ELBOW;
        steps[3] = l.getRightWrist() - RIGHTWRIST;
        steps[4] = l.getLeftWrist() - LEFTWRIST;
        return steps;
    }
    
    // Changes the coordinates of the point to new coordinates
    public void shift(double x, double y, double z) {
        Location temp = new Location(X+x,Y+y,Z+z,PITCH,ROLL,NAME);
        X = temp.getX();
        Y = temp.getY();
        Z = temp.getZ();
        PITCH = temp.getPitch();
        ROLL = temp.getRoll();
        BASE = temp.getBase();
        SHOULDER = temp.getShoulder();
        ELBOW = temp.getElbow();
        RIGHTWRIST = temp.getRightWrist();
        LEFTWRIST = temp.getLeftWrist();
        GRIPPER = temp.getGripper();
    }
        
    /////////////////////////////////////
    // FORWARD SOLUTION
    //
    // The next five (5) methods are used to convert the motor
    // steps to the X,Y,X,Pitch and Roll coordinates
    /////////////////////////////////////    
    private double findX(int b, int s, int e, double p) {
        double RR = L*Math.cos(-s/S) + L*Math.cos(-e/E) + LL*Math.cos(Math.toRadians(p));
        return RR*Math.cos(b/B);
    }    
    private double findY(int b, int s, int e, double p) {
        double RR = L*Math.cos(-s/S) + L*Math.cos(-e/E) + LL*Math.cos(Math.toRadians(p));
        return RR*Math.sin(b/B);
    }    
    private double findZ(int s, int e, double p) {
        return H + (L*Math.sin(-s/S)) + (L*Math.sin(-e/E)) + (LL*Math.sin(Math.toRadians(p)));
    }        
    private double findP (int r, int l) {
        return 0.5*((-l/W) + (-r/W));
    }    
    private double findR (int r, int l) {
        return 0.5*((-l/W) - (-r/W));
    }    
    
    //////////////////////////////////////////////////////
    // BACKWARD SOLUTION
    //
    // The following nine (9) methods are used to convert
    // the cartesian coordinates to motor step coordinates
    //////////////////////////////////////////////////////
    private int findBase(double x, double y) {
        if(x==0.0) return Math.round((float)(Math.sin(y) *(Math.PI/2)));
        else return Math.round((float)((Math.atan(y/x))*B));
    }
    private int findShoulder(double a, double b) {
        return Math.round((float)(-(a+b)*S));
    }
    private int findElbow(double a, double b) {
        return Math.round((float)(-(b-a)*E));
    }
    private int findRightWrist(double p, double r) {
        return Math.round((float)(-((Math.toRadians(p) - Math.toRadians(r))*W)));
    }
    private int findLeftWrist(double p, double r) {
        return Math.round((float)(-((Math.toRadians(p) + Math.toRadians(r))*W)));
    }
    private double findAlpha (double RR, double R0, double z, double p) {
        double Z0 = (z - LL*Math.sin(Math.toRadians(p))) - H;
        double temp = Math.sqrt(((4*L*L)/((R0*R0)+(Z0*Z0))) -1);
        if(temp==0.0) return 0.0;      
        return Math.atan(temp);
    }
    private double findBeta (double RR, double R0, double z, double p) {
        double Z0 = (z - LL*Math.sin(Math.toRadians(p))) - H;
        if(R0==0.0) return 0.0;
        return Math.atan(Z0/R0);
    }
    private double getRR (double x, double y) {
        return Math.hypot(x,y);
    }
    private double getR0(double rr, double p) {
        return rr - LL*Math.cos(Math.toRadians(p));
    }
    
    ///////////////////////////////////////////////////////////////
    // ACCESSOR METHODS BELOW
    ///////////////////////////////////////////////////////////////
    public String getName() {
        return NAME;
    }    
    public int getBase() {
        return BASE;
    }
    public int getShoulder() {
        return SHOULDER;
    }
    public int getElbow() {
        return ELBOW;
    }
    public int getRightWrist() {
        return RIGHTWRIST;
    }
    public int getLeftWrist() {
        return LEFTWRIST;
    }
    public int getGripper() {
        return GRIPPER;
    }
    public double getX() {
        return (X+OS);
    }
    public double getY() {
        return Y;
    }
    public double getZ() {
        return Z;
    }
    public double getRoll() {
        return ROLL;
    }
    public double getPitch() {
        return PITCH;
    }
    public boolean isValid() {
        return valid;
    }
    public String getError() {
        return error;
    }
    public int[] getMotorSteps() {
        int[] temp = {BASE,SHOULDER,ELBOW,RIGHTWRIST,LEFTWRIST,GRIPPER};
        return temp;
    }
    
    // Return the object as a sting that shows all variables in the Location
    public String toString() {
        String temp = "Location: " + NAME + "\n";
        temp += "B="+BASE+" S="+SHOULDER+" E="+ELBOW+" R="+RIGHTWRIST+" L="+LEFTWRIST+" G="+GRIPPER+"\n";
        temp += "X="+(X+OS)+" Y="+Y+" Z="+Z+" PITCH="+PITCH+" ROLL="+ROLL+"\n";
        return temp;
    }
    
    // Return the object as a string that can be compiled in Java
    // used when compiling on Java window.
    public String toJavaString() {
        String temp = "Location " + NAME + " = new Location(";
        temp += (X+OS) + "," + Y + "," + Z + ",";
        temp += PITCH + "," + ROLL + ",\"" + NAME + "\"" + ");";   
        return temp;
    }         
}   // end Location class
