/*
 * PP.java
 * This plans the path of the robot arm, while considering if a piece is about to be captured and removing that piece
 * If a lowpath (path between the pieces) can be found that will be used, if not a high path is used.
 * Roelof de Vries  (0335843) (ravries)
 * Douwe Oosterhout (0612667) (doosterh)
 * Date: 30-06-06
 */

import java.io.*;
import java.lang.*;
import java.util.Vector;

public class PP {
  private static double SAFE_HEIGHT=200;
  private static double LOW_HEIGHT=40;
  private static double OPEN_GRIP=30;
  private static double CLOSED_GRIP=0;

  public static void main(String[] args){
    Vector p = new Vector();
    String computerFrom, computerTo;
    ChessBoard b = new ChessBoard();
    b.normal = new Vector3D(0.1,0,1);  // give the board normal here
	
    System.out.println("**** THIS IS THE STUDENT PP MODULE IN JAVA"); 
    System.out.println("**** The human move was "+ args[0]);
    System.out.println("**** The computer move was "+ args[1]);
    System.out.println("**** The board before the move was:");       

    /* Read the board state*/
    b.read();
    /* print the board state*/
    b.print();
    
    computerFrom = args[1].substring(0,2);
    computerTo = args[1].substring(2,4);

    highPath(computerFrom, computerTo, b, p);

    /* move the computer piece */
    try {
      b.movePiece(computerFrom, computerTo);
    } catch (ChessBoard.NoPieceAtPositionException e) {
      System.out.println(e);
      System.exit(1);
    }

    System.out.println("**** The board after the move was:");       
    /* print the board state*/
    b.print();
    
    /* after done write the gripper positions */
    GripperPosition.write(p);
  }

  // transform_x is a rotation round the y-axis, so the x-coordinates are transformed

  static Point transform_x(Point point, Vector3D normal,ChessBoard b){
	double y, z, normal_angle, y_moved, y_moved_back;

	normal_angle = Math.atan(Math.abs(normal.y) / normal.z); //the angle of the normal with the table
	y_moved = point.y - b.coords.y; // first we move the board against the y-axis
	y = y_moved * Math.cos(normal_angle) + point.z * Math.sin(normal_angle); // then we translate the coordinates
	z = -y_moved * Math.sin(normal_angle) + point.z * Math.cos(normal_angle);// using our linear algebra skills
	y_moved_back = y + b.coords.y; // then we move the board back
	Point newPoint = new Point(point.x, y_moved_back, z); //and return the new point
	return newPoint;

  }

  // transform_y is a rotation round the y-axis, so the y-coordinates are transformed
  
  static Point transform_y(Point point, Vector3D normal, ChessBoard b){
	double x, z, normal_angle, extra_height, halfBoardLength;

	normal_angle = -Math.atan(normal.x / normal.z);
	x = point.x * Math.cos(normal_angle) - point.z * Math.sin(normal_angle);
	halfBoardLength = (2* b.sur_x + 8 * b.delta_x) / 2;
	extra_height = halfBoardLength * Math.sin(normal_angle); 
	/* 
	 * after translating part of the board is under the table, so we have to move the board up a few cm, 
	 * depending on the angle turned
	 */
	z = (point.x * Math.sin(normal_angle) + point.z * Math.cos(normal_angle)) + extra_height;
	Point newPoint = new Point(x, point.y, z);
	return newPoint;

  }

	/*
	 * gripperHeight determines the height of the gripper based on the normal vector
	 * the only problem is that we didn't succeed in getting te gripper to arrive at the normal angle
	 * the height is correct but the gripper is still hovering above the position used before we tilted the
	 * board
	 */

  static double gripperHeight(Point point, Vector3D normal, double extra_height){
	double angle = Math.atan(normal.z / Math.sqrt(normal.x * normal.x + normal.y * normal.y));
	double boardHeight = point.z / Math.sin(angle);
	double gripperHeight = point.z * (boardHeight + extra_height) / boardHeight;
	return gripperHeight;
  }

  private static void highPath(String from, String to, 
           ChessBoard b, Vector p) {

    System.out.println("**** In high path"); 

        // Use the boardLocation and toCartesian methods you wrote:
  	StudentBoardTrans studentBoardTrans = new StudentBoardTrans(from);

    	int fromColumn, toColumn, fromRow, toRow;
	double gripperHeight;
	double Half_piece_height = 0;
	Point tempPoint, point2, point3;
     	GripperPosition temp;
	Point point = new Point();
	
	try { // just in case that there isn't a piece at the given location we have to catch that exception
	Half_piece_height = 0.5 * b.getHeight(from);
	} catch (Exception e) {}

	/*
	 * The high path is done in 10 steps. First it goes to the piece that has to be picked up at a high altitude
	 * Then is loweres itself a little, then it loweres itself to half the piece height and picks it up.
	 * It moves itself to a safe height and moves to the new position. It repeats the steps from before and then
	 * lets the piece go.
	 */

	//1  Safe height over the piece and its gripper open
	fromColumn = studentBoardTrans.boardLocation.column; // first get the boardpositions
	fromRow = studentBoardTrans.boardLocation.row; // first get the boardpositions
	point3 = studentBoardTrans.toCartesian(fromColumn, fromRow); // translate to cartesian coordinates
	point2 = transform_x(point3,b.normal,b); // translate the x-coordinates by turning around the y-axis
	point = transform_y(point2,b.normal,b); // translate the y-coordinates by turning around the x-axis
	gripperHeight = gripperHeight(point,b.normal,SAFE_HEIGHT + point3.z); // get the gripperheight

	/*
	 * this is probably where the arrival vector of the gripper should be determined, we didn't succeed
	 * in doing this due to a time and experience shortage
	 * Again, the height is correct, but the x and y values are still the old ones.
	 */	

	tempPoint = new Point(point.x, point.y, gripperHeight); // create a point with added properties
     	temp = new GripperPosition(tempPoint,b.normal, b.theta, OPEN_GRIP); // create the gripperposition
	p.add(temp); // add the position to the vector list

	//2  Low height over the piece and its gripper open
	gripperHeight = gripperHeight(point,b.normal,LOW_HEIGHT + point3.z);
	tempPoint = new Point(point.x, point.y, gripperHeight);
     	temp = new GripperPosition(tempPoint,b.normal, b.theta, OPEN_GRIP);
	p.add(temp);

	//3  Half pieceheight and its grippen open
	gripperHeight = gripperHeight(point,b.normal,Half_piece_height + point3.z);
	tempPoint = new Point(point.x, point.y, gripperHeight);
	temp = new GripperPosition(tempPoint,b.normal, b.theta, OPEN_GRIP);
	p.add(temp);

	//4  Half pieceheight and its gripper closed
	temp = new GripperPosition(tempPoint,b.normal, b.theta, CLOSED_GRIP);
	p.add(temp);

	//5  Safe height with gripper closed
	gripperHeight = gripperHeight(point,b.normal,SAFE_HEIGHT + point3.z);
	tempPoint = new Point(point.x, point.y, gripperHeight);
	temp = new GripperPosition(tempPoint,b.normal, b.theta, CLOSED_GRIP);
	p.add(temp);

	//6  Safe height over the new position and the gripper is closed
	studentBoardTrans = new StudentBoardTrans(to);
	toColumn = studentBoardTrans.boardLocation.column;
	toRow = studentBoardTrans.boardLocation.row;
	point3 = studentBoardTrans.toCartesian(toColumn, toRow);
	point2 = transform_x(point3,b.normal,b);
	point = transform_y(point2,b.normal,b);
	gripperHeight = gripperHeight(point,b.normal,SAFE_HEIGHT + point3.z);
	tempPoint = new Point(point.x, point.y, gripperHeight);
	temp = new GripperPosition(tempPoint,b.normal, b.theta, CLOSED_GRIP);
	p.add(temp);

	//7  Low height plus half pieceheight and the gripper is closed
	gripperHeight = gripperHeight(point,b.normal,LOW_HEIGHT + point3.z);
	tempPoint = new Point(point.x, point.y, gripperHeight);
	temp = new GripperPosition(tempPoint,b.normal, b.theta, CLOSED_GRIP);
	p.add(temp);

	//8  Half pieceheight and the gripper is closed
	gripperHeight = gripperHeight(point,b.normal,Half_piece_height + point3.z);
	tempPoint = new Point(point.x, point.y, gripperHeight);
	temp = new GripperPosition(tempPoint,b.normal, b.theta, CLOSED_GRIP);
	p.add(temp);

	//9  Half pieceheight and the gripper is open
	temp = new GripperPosition(tempPoint,b.normal, b.theta, OPEN_GRIP);
	p.add(temp);

	//10  Safe height and the gripper is open
	gripperHeight = gripperHeight(point,b.normal,SAFE_HEIGHT + point3.z);
	tempPoint = new Point(point.x, point.y, gripperHeight);
	temp = new GripperPosition(tempPoint,b.normal, b.theta, OPEN_GRIP);
	p.add(temp);
	
  }
}