import com.cyberbotics.webots.Controller;
import java.io.*;

/**Permet de créer un robot Judoka. Cette classe va permettre de mettre en place l'architecture nécessaire pour pouvoir faire combattre le robot. Notre robot est capable de marcher, de trouver son adversaire, de le contourner et de le pousser.
 *@author Julien VAN DEN BOSSCHE / Benoît MOULIN   cmoi@julienvdb.com / bmoulin@etu.info.unicaen.fr
 */


public abstract class Judoka0 extends Controller {
    public static void die() {
	System.out.println("die method called");
    }
    
    static int [] joint = new int[22];
    static int camera;
    static int distance;	 
    static int left_touch;
    static int right_touch;
    static int gps;
    static int emitter;
    static int front_led;
    static float pos[];// position du robot
    static float orient[]; //oriente du robot
    static float matrix[];
    
    
    /**permet d'assigner chaque device à une case du tableau joint.
     */
    public static void reset() {
	joint[0]=robot_get_device("back_1");
	joint[1]=robot_get_device("back_2");
	joint[2]=robot_get_device("left_hip_1");
	joint[3]=robot_get_device("left_hip_2");
	joint[4]=robot_get_device("left_hip_3");
	joint[5]=robot_get_device("left_knee");
	joint[6]=robot_get_device("left_ankle_1");
	joint[7]=robot_get_device("left_ankle_2");
	joint[8]=robot_get_device("left_shoulder_1");
	joint[9]=robot_get_device("left_shoulder_2");
	joint[10]=robot_get_device("left_elbow");
	joint[11]=robot_get_device("neck");
	joint[12]=robot_get_device("right_hip_1");
	joint[13]=robot_get_device("right_hip_2");
	joint[14]=robot_get_device("right_hip_3");
	joint[15]=robot_get_device("right_knee");
	joint[16]=robot_get_device("right_ankle_1");
	joint[17]=robot_get_device("right_ankle_2");
	joint[18]=robot_get_device("right_shoulder_1");
	joint[19]=robot_get_device("right_shoulder_2");
	joint[20]=robot_get_device("right_elbow");
	joint[21]=robot_get_device("neck_tilt");
	for(int i=0;i<22;i++) if (joint[i]==0)
	    System.out.println("Joint "+i+" is missing");
	camera=robot_get_device("camera");
	distance=robot_get_device("distance");
	left_touch=robot_get_device("left touch");
	right_touch=robot_get_device("right touch");
	gps=robot_get_device("gps");
	emitter=robot_get_device("emitter");
	front_led=robot_get_device("front eye");
    }
    
    /**permet de mettre tous les servomoteurs à 0*/
    public static void init(){
	for(int i=0;i<22;i++){
	    servo_set_position(joint[i],0.0f);
	    servo_set_velocity(joint[i],2);
	    servo_enable_position(joint[i],150);
	   
	}
    }

    /**permet de mettre tous les servomoteurs à 0 et de changer la vélocité pour que le robot se relève*/
     public static void initReleve(){
	for(int i=0;i<22;i++){
	    servo_set_position(joint[i],0.0f);
	    servo_set_velocity(joint[i],1);
	    servo_enable_position(joint[i],150);
	   
	}
    }
    


    /**permet de faire marcher le robot tout droit*/
    
    public static void marche (){
	matrix=gps_get_matrix(gps);
	float[] dirRobot=gps_euler(matrix);
	float directionRobot = dirRobot[1];
	System.out.println("direction marche="+directionRobot);
	
	robot_step(200);
	servo_set_position(joint[0],0.5f);
	
	robot_step(1000);
	
	servo_set_position(joint[3],0.5f);
	servo_set_position(joint[5],0.5f);
	servo_set_position(joint[20],1f);
	servo_set_position(joint[18],0.5f);
	robot_step(500);
	servo_set_position(joint[3],0f);
	servo_set_position(joint[5],0f);
	servo_set_position(joint[20],0f);
	servo_set_position(joint[18],0f);
	
	robot_step(1000);
	
	servo_set_position(joint[13],0.5f);
	servo_set_position(joint[15],0.5f);
	servo_set_position(joint[10],1f);
	servo_set_position(joint[8],0.5f);
	robot_step(500);
	servo_set_position(joint[13],0f);
	servo_set_position(joint[15],0f);
	servo_set_position(joint[10],0f);
	servo_set_position(joint[8],0f);
    }
    

    /**permet de doner la direction à prendre pour retourner au centre du tatami*/
    public static double retourneDirToCenter(){
	
	matrix=gps_get_matrix(gps);
	float x0=0.0f;
	float z0=0.0f;
	float xPos;
	float zPos;
	float deltaX;
	float deltaZ;
	double dir;
	double quotient;
	//   double pi=3.141592654;
	xPos = gps_position_x(matrix);
	zPos = gps_position_z(matrix);
	deltaX=xPos-x0;
	deltaZ=zPos-z0;
	quotient=(double)((Math.abs(deltaZ)/Math.abs(deltaX)));
	dir=Math.atan(quotient);
	dir=((Math.PI)/2)-dir;
	if((xPos<0)&&(zPos>0)){
	    dir+=Math.PI;
	}
	if((xPos<0)&&(zPos<0)){
	    dir=(2*Math.PI)-dir;
	}
	if((xPos>0)&&(zPos>0)){
	    dir=(Math.PI)-dir;
	}
	return dir;
    }
    
    
    /**permet de renvoyer le robot au centre du tatami*/
    public static void goToCenter(){
	gps_enable(gps,64);
	matrix=gps_get_matrix(gps);
	double direction;
	float[] dirRobot=gps_euler(matrix);
	float directionRobot;
	float positionRobotX = gps_position_x(matrix);
	float positionRobotZ = gps_position_z(matrix);
	direction=retourneDirToCenter();
	gps_enable(gps,64);
	dirRobot=gps_euler(matrix); 
	directionRobot = dirRobot[1];
	double diff;
	if(directionRobot<0){
	   diff = (double)(Math.abs(directionRobot-Math.abs(direction)));
	    System.out.println("la diff="+diff+"et pi/2="+(Math.PI/2));
	}
	
	else{
	    diff = (double)(Math.abs((Math.abs(directionRobot)-Math.abs(direction))));
	    System.out.println("la diff="+diff+"et pi/2="+(Math.PI/2));
	}
	if(diff>(Math.PI/2)){
	    tourne90D();
	    tourne90D();
	}
	init();
	robot_step(200);
	//le robot se tourne dans la bonne direction  
	for(;;){
	    if(estTombe()){
		strategie(0);
		break;
	    }
	    direction=retourneDirToCenter();
	    gps_enable(gps,64);
	    //float[] dirRobot=gps_euler(matrix);
	    dirRobot=gps_euler(matrix); 
	    directionRobot = dirRobot[1];
	    
	    if(directionRobot<0){
		directionRobot=(float)((2*(Math.PI)))+directionRobot;
	    }
	    if((directionRobot>direction-0.07) && (directionRobot<direction+0.07)){  
		for(int j=0; j<5; j++){	  
		    tourne();
	    }
		//tourne();
		init();
		robot_step(150); 
		System.out.println("Centre du tatami répéré");
		break;      
	    }
	    else{
		System.out.println("direction voulue="+direction);
		System.out.println("direction robot="+directionRobot);
		tourne();
	    }
	}
	//on le fait marcher jusqu'au centre
	for(;;){
	    if(estTombe()){
		strategie(0);
		break;
	    }
	    matrix=gps_get_matrix(gps);
	    gps_enable(gps,64);
	    positionRobotX = gps_position_x(matrix);
	    positionRobotZ = gps_position_z(matrix);
	    //si on a atteint le centre
	    if(((positionRobotX>-0.2)&&(positionRobotX<0.2))&&((positionRobotZ>-0.3)&&(positionRobotZ<0.3))){
		System.out.println("centre du tatami atteint");
		robot_step(150);
		init();
		robot_step(150);
		scrute();
		break;
	    }
	    else{
		System.out.println("pos x="+positionRobotX);
		System.out.println("pos z="+positionRobotZ);
		marche();
		robot_step(150);
	    }
	}
}

    /**permet faire un tour sur soit même et de regarger si un adversaire se trouve à proximité. Si c'est le cas on va le faire marcher jusqu'a l'adversaire. Méthode gere(0)*/
    public static void scrute(){
	distance_sensor_enable(distance,64);
	for(;;){
	    if(estTombe()){
		strategie(0);
		break;
	    }
	    tourne();
	    System.out.println("dist ressentie="+distance_sensor_get_value(distance));
	    if((distance_sensor_get_value(distance))<478){
		for(int i=0; i<3; i++){
		    tourne();
		}
		init();
		robot_step(150);
		strategie(1);
		break;
	    }
	   
	}
    }
    
    
    /**permet de contourner l'adversaire et de le pousser. Si on est trop proche du tatami on se contentera de le pousser pour ne pas tomber hors de la zone*/
    public static void contourne(){
	matrix=gps_get_matrix(gps);
	if((gps_position_z(matrix)<-0.65)||(gps_position_z(matrix)>0.65)||(gps_position_x(matrix)<-0.65)||(gps_position_x(matrix)>0.65)||(distance_sensor_get_value(distance)<100)){
	    if(estTombe()){
		strategie(0);
		
	    }
	    else{
		robot_step(150);
		init();
		marche();
		robot_step(150);
	    }
	}
	else{
	    if(estTombe()){
		strategie(0);
		    }
	    else{
		tourne90D();
		for(int i=0; i<4; i++){
		    marche();
		    robot_step(150);
		}
		
		init();
		tourne90G();
		robot_step(200);
		for(int i=0; i<7; i++){
		    marche();
		    robot_step(150);
		}
		
		init();
		tourne90G();
		robot_step(200);
		for(int i=0; i<4; i++){
		    marche();
		    robot_step(150);
		    
		}
		
		init();
		tourne90G();
		robot_step(150);
		init();
		robot_step(150);
		int l=2;
	while (l>0) {
	    servo_set_position(joint[2],1.45f);
	    robot_step(1000);
	    servo_set_position(joint[1],-0.45f);
	    robot_step(1000);
	    servo_set_position(joint[2],0.0f);
	    robot_step(1000);
	    servo_set_position(joint[1],0.0f);
	    robot_step(1000);
	    l--;
	}
	
		robot_step(200);
		init();
		robot_step(200);
	    }
	    int d=distance_sensor_get_value(distance);
	    System.out.println("distance après contournement="+ d);
	    
		marche();
		marche();
	    
	}
	    
	init();
	robot_step(150);
	servo_set_position(joint[8], 1.5f);
	servo_set_position(joint[18], 1.5f);
	robot_step(5000);
	  if(estTombe()){
  	    strategie(0);
  	}
	  init();
  	strategie(2);
    }
    
    /**permet de  pousser l'adversaire, à utiliser dans le cas où l'on veut faire un combat entre robot*/
   //   public static void contourne(){
//  	matrix=gps_get_matrix(gps);
	
//  	    if(estTombe()){
//  		strategie(0);
//  	    }
//  	    else{
//  		robot_step(150);
//  		init();
//  		marche();
//  		marche();
//  		robot_step(150);
//  		init();
//  		robot_step(150);
//  		servo_set_velocity(joint[8],3.0f);
//  		servo_set_velocity(joint[18],3.0f);
//  		robot_step(150);
//  		servo_set_position(joint[8], 1.5f);
//  		servo_set_position(joint[18], 1.5f);
//  		robot_step(150);
//  		servo_set_position(joint[8], 0.0f);
//  		servo_set_position(joint[18], 0.0f);
//  		robot_step(150);
//  		servo_set_position(joint[8], 1.5f);
//  		servo_set_position(joint[18], 1.5f);
//  		robot_step(150);
//  		servo_set_position(joint[8], 0.0f);
//  		servo_set_position(joint[18], 0.0f);
//  		robot_step(150);
//  		servo_set_position(joint[8], 1.5f);
//  		servo_set_position(joint[18], 1.5f);
//  		robot_step(5000);
//  		init();
//  		strategie(2);
//  	    }
//      }
    


    /**permet de tourner d'un angle proche de 5 degrés à droite*/
    public static void tourne (){
	int cpt=0;
	int i;
	robot_step(2000);
	while(cpt<20){
	    servo_set_position(joint[2],2f);
	    servo_set_velocity(joint[2],0.7f);
	    
	    servo_set_position(joint[12],0.2f);
	    servo_set_velocity(joint[12],1f);
	    cpt++;
	}
	
	robot_step(2500);
	float t1 = servo_get_position(joint[12]);

	if(cpt==20 && t1==servo_get_position(joint[12])) {
	    for(i=0;i<22;i++){
		servo_set_position(joint[i],0);
		servo_set_velocity(joint[i],1f);
	    }  
	    cpt++;
	}
    }
    

    /**permet de tourner à gauche de 5 degrés*/
    public static void tourneG (){
	int cpt=0;
	int i;
	robot_step(2000);
	while(cpt<20){
	    servo_set_position(joint[12],2f);
	    servo_set_velocity(joint[12],0.7f);
	    servo_set_position(joint[2],0.2f);
	    servo_set_velocity(joint[2],1f);
	    cpt++;
	}
	
	robot_step(2500);
	float t1 = servo_get_position(joint[12]);
	
	if(cpt==20 && t1==servo_get_position(joint[12])) {
	    for(i=0;i<22;i++){
		servo_set_position(joint[i],0);
		servo_set_velocity(joint[i],1f);
	    }  
	    cpt++;
	}
	
    }
    
    /**permet de tourner de 90 degrés à gauche */
    public static void tourne90G(){
	int l=3;
	while (l>0) {
	    servo_set_position(joint[2],1.45f);
	    robot_step(1000);
	    servo_set_position(joint[1],-0.45f);
	    robot_step(1000);
	    servo_set_position(joint[2],0.0f);
	    robot_step(1000);
	    servo_set_position(joint[1],0.0f);
	    robot_step(1000);
	    l--;
	}
    }

    /**permet de tourner de 90 degrés à droite */
    public static void tourne90D(){
	int l=3;
	while (l>0) {
	    servo_set_position(joint[12],1.45f);
	    robot_step(1000);
	    servo_set_position(joint[1],0.45f);
	    robot_step(1000);
	    servo_set_position(joint[12],0.0f);
	    robot_step(1000);
	    servo_set_position(joint[1],0.0f);
	    robot_step(1000);
	    l--;
	}
    }

    /**méthode que l'on appelle dans le main quand on veut décaler le robot et qu'il ne parte pas selon l'axe OZ mais de biais*/
    public static void tourneStart(){
	int l=1;
	while (l>0) {
	    servo_set_position(joint[12],1.45f);
	    robot_step(1000);
	    servo_set_position(joint[1],0.45f);
	    robot_step(1000);
	    servo_set_position(joint[12],0.0f);
	    robot_step(1000);
	    servo_set_position(joint[1],0.0f);
	    robot_step(1000);
	    l--;
	}
    }

    /**permet de renvoyer des infos sur la position du robot*/
    public static void getPos(){
	matrix=gps_get_matrix(gps);
	float[] x=gps_euler (matrix); 
	System.out.println("direction="+x[1]);
	System.out.println("directionToCenter="+retourneDirToCenter());
	System.out.println("distance="+distance_sensor_get_value(distance));
}
 
   /**Méthode qui gére les actions à faire selon une priorité. Par exemple on va chercher l'adversaire avant d'essayer de le contourner*/
    public static void strategie(int couche){
	if(couche==0){
	    releve();
	    robot_step(2000);
	    init();
	    robot_step(1500);
	    strategie(2);
	}
	if(couche==1){
	    for(;;){
		marche();
		robot_step(150);
		matrix=gps_get_matrix(gps);
		//System.out.println("positionZ*="+gps_position_z(matrix));
		System.out.println("distance="+distance_sensor_get_value(distance));
		if(estTombe()){
		    strategie(0);
		    break;
		}
		//retour au milieu du tatami
		if((gps_position_z(matrix)<-0.8)||(gps_position_z(matrix)>0.8)||(gps_position_x(matrix)<-0.8)||(gps_position_x(matrix)>0.8)){
		    System.out.println("au centre");
		    robot_step(200);
		    init();
		    robot_step(200);
		    strategie(2);
		    break;
		}
		//si proche de l'adversaire le faire tomber
		if(distance_sensor_get_value(distance)<200){
		    strategie(3);
		    break;
		}
	    }
	}
	if(couche==2){
	    //init();
	    //	robot_step(128);
	    if(estTombe()){
		strategie(0);
	    }
	    else{
		goToCenter();
	    }
	}
	if(couche==3){
	    robot_step(200);
	    init();
	    robot_step(200);
	    if(estTombe()){
		strategie(0);
	    }
	    else{
		contourne();
	    }
	}
	if(couche==4){
	    init();
	}
    }
    
 /**Méthode qui permet de savoir si le robot est tombé*/
    public static boolean estTombe(){
	boolean flag=false;
	matrix=gps_get_matrix(gps);
	if(gps_position_y(matrix)<0.5){
	    flag=true;
	}
	return flag;
    }

/**Méthode qui permet de retourner le robot s'il est sur le dos*/
    public static void retourne(){
	servo_set_position(joint[18],-3f);
	servo_set_position(joint[8],5f);
	servo_set_position(joint[13],-2.5f); 
	servo_set_position(joint[15],1f); 
    }



/**Méthode qui permet de relever le robot quelque soit sa position (sur le dos ou sur le ventre)*/
    public static void releve(){
	 robot_step(2000);
	 initReleve();
	 robot_step(2000);
	 if(distance_sensor_get_value(distance)>900){
	     retourne();
	     robot_step(5000);
	}
	
	releveVentre();
	
    }
    /**Méthode qui permet de relever le robot quand il est sur le ventre*/
    public static void releveVentre(){
	robot_step(2000);
	init();
	robot_step(1000);
	robot_step(2000);
	servo_set_position(joint[5],4f);
	servo_set_position(joint[15],4f);
	servo_set_position(joint[3],3f);
	servo_set_position(joint[13],3f);
	robot_step(2000);
	servo_set_position(joint[8],-5f);
	servo_set_position(joint[18],-5f);
	servo_set_position(joint[0],3f);
        robot_step(2000);
	servo_set_position(joint[6],3f);
	servo_set_position(joint[16],3f);
	robot_step(2000);
	servo_set_position(joint[8],0f);
	servo_set_position(joint[18],0f);
	robot_step(2000);
	servo_set_position(joint[8],-3f);
	servo_set_position(joint[18],-3f);
	robot_step(3000);
	servo_set_position(joint[8],-3.5f);
	servo_set_position(joint[18],-3.5f);
	robot_step(2000);
	servo_set_position(joint[3],0.5f);
	servo_set_position(joint[13],0.5f);
	robot_step(4000);
	servo_set_position(joint[5],1.5f);
	servo_set_position(joint[15],1.5f);
	robot_step(2000);
	servo_set_position(joint[6],1f);
	servo_set_position(joint[16],1f);
	robot_step(2000);
	servo_set_position(joint[3],0f);
	servo_set_position(joint[13],0f);
	robot_step(2000);
	servo_set_position(joint[0],1f);
	robot_step(2000);
	servo_set_position(joint[5],0f);
	servo_set_velocity(joint[5],0.5f);
	servo_set_position(joint[15],0f);
	servo_set_velocity(joint[15],0.5f);
	servo_set_position(joint[6],0f);
	servo_set_velocity(joint[6],0.2f);
	servo_set_position(joint[16],0f);
	servo_set_velocity(joint[16],0.2f); 
	servo_set_position(joint[0],0f);
	servo_set_velocity(joint[0],0.2f); 
	robot_step(5000);
	init();
    }

    
    
    public static void main() {
	
	float [] futurPosition = new float[22];
	float t=0.0f;
	int i,targetJoint;
	int[] image;
	int l;
	float f;
	float[] matrix;
	float x,y,z;
	String message="stand me up 0 15 15 0";
	byte [] msg=null;
	int send = 0;
	int my_score,opponent_score;
	 // During contest matches, the current set score is saved in a file
	// named score.txt in the directory of each controller involved in
	// in the match. The file contains two integer values written in ASCII
	// encoding in the range [0;2]. The first value is the score of the
	// current controller and the second value is the score of its opponent.
	// By using these values, you can determine if you won or lost the
	// previous set. If you lost, you may decide to change your strategy...
	// It might be useful for your controller to save some stragegy info
	// a in local file that you will reread when your controller starts up
	// again for a new match set (just to know which strategy you used last
	// time).
	try {
	    BufferedReader in = new BufferedReader(new FileReader("score.txt"));
	    String line = in.readLine();
	    my_score = Integer.parseInt(line.substring(0,1));
	    opponent_score = Integer.parseInt(line.substring(2,3));
	    System.out.println("Score:   me: " + my_score +
			       " - opponent: " + opponent_score);
	    in.close();
	} catch (IOException e) {
	    // System.out.println("Cannot read score.txt file: "+e);
	    my_score=-1;
	    opponent_score=-1;
	}
	try { // convert the String into bytes using the ISO-8859-1 encoding
	    msg=message.getBytes("ISO-8859-1");
	} catch(Exception e) {}
	
	
	robot_step(150); 
	for(i=0;i<22;i++) {
	    servo_set_position(joint[i],0);
	    futurPosition[i]=0.0f;
	    servo_enable_position(joint[i],150);
	}
	//camera_enable(camera,64);
	distance_sensor_enable(distance,64);
	gps_enable(gps,64);
	

	/*enlever ces commentaires pour voir le partir dans une autre direction que celle de l'adversaire 
         puis se resituer vers le milieu et aller chercher son adversaire.
	 metter en commentaire la méthode contourne() actuelle et enlever les commentaires de l'autre méthode contourne()*/

//      tourneStart();
//  	tourneStart();
//  	init();
//  	robot_step(150);
  	
	strategie(1);
	led_set(front_led,3);
	// blink using all the 8 available colors of the LED
	t += 0.1;
	
    }
}