/****************************************************************************

  Robot VolaTOR - Un modelo de globo dirigible y controlador que evita colisiones para Webots5.1.0 (Cyberbotics)
  
  Copyright (C) 2010 Hector Díaz Bachiller

  Instituto de Automática e Informática Industrial (ai2), UPV, Valencia
  
  Author: Hector Díaz Bachiller
  
  Email: hecdiaba@fiv.upv.es     Web: https://sites.google.com/site/torsweb/



  NOTA: Esta traducción de la GPL es informal y no ha sido aprobada 
  oficialmente por la Fundación para el Software Libre como válida. 
  Para estar completamente seguro de lo que está permitido, consulte 
  la GPL original (en inglés).

  Este programa es software libre: puedes redistribuirlo y/o modificarlo
  bajo los términos de GNU General Public License (GPL) como publica la Free
  Software Fundation, tanto en su versión 3, o (bajo tu criterio) cualquiera posterior.

  Este programa es distribuido con la esperanza de ser útil, pero SIN NINGUNA GARANTÍA;
  sin ni siquiera la garantía implícita de COMERCIABILIDAD o ADECUACIÓN A UN USO PARTICULAR.
  Ver la GNU General Public License para más detalles.

  Deberías haber recibido una copia de la GNU General Public License junto con este
  programa. Si no es así, visita <http://www.gnu.org/licenses/> o escribe a la
  Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  Boston, MA  02111-1307, USA.

  

  Este código se ha basado en:

  blimp_asl2 -- A blimp simulator and controller for Webots4 (Cyberbotics)
  Copyright (C) 2003 Autonomous Systems Lab 2, EPFL, Lausanne
  Authors: Jean-Christophe Zufferey & Alexis Guanella
  Email: name.surname@epfl.ch     Web: http://asl.epfl.ch


  ******************************** ////\\\\ ***********************************
  ******************************** \\\\//// ***********************************

  Robot VolaTOR - A blimp model and controller that avoids collisions for Webots5.1.0 (Cyberbotics)
  
  Copyright (C) 2010 Hector Díaz Bachiller

  Instituto de Automática e Informática Industrial (ai2), UPV, Valencia
  
  Author: Hector Díaz Bachiller
  
  Email: hecdiaba@fiv.upv.es     Web: https://sites.google.com/site/torsweb/  



  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/> or
  write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  Boston, MA  02111-1307, USA.



  This code is based on:

  blimp_asl2 -- A blimp simulator and controller for Webots4 (Cyberbotics)
  Copyright (C) 2003 Autonomous Systems Lab 2, EPFL, Lausanne
  Authors: Jean-Christophe Zufferey & Alexis Guanella
  Email: name.surname@epfl.ch     Web: http://asl.epfl.ch

 
******************************************************************************/



/****************************************************************************
  Important notice: 

    Set tab-size = 2 in your editor to view this file in good conditions.

******************************************************************************/





#include <device/robot.h>

#include <device/custom_robot.h>

#include <device/gps.h>

#include <device/camera.h>

#include <device/distance_sensor.h>





#include <math.h>
#include <stdlib.h>
#include "pila.cpp" //dentro de la clase pila ya se incluye la clase "punto"
 
 
//ESTADOS
#define BUSCA_ANGULO 1
#define AVANZA 2
#define PREDOCKING 3
#define PREDOCKING2 4
#define DOCKING 5







// estructuras deteccion colision
typedef struct Esfera{
  Punto c;
  float r;
};



typedef struct Biesfera{
  Punto c0;
  Punto c1;
  float r0;
  float r1;
  float lambda;
  float mtd;
  float radio;
  Punto proy;
};



typedef struct Triesfera{
  Punto c0;
  Punto c1;
  Punto c2;
  float r0;
  float r1;
  float r2;
};



typedef struct Tetraesfera{
  Punto c0;
  Punto c1;
  Punto c2;
  Punto c3;
  float r0;
  float r1;
  float r2;
  float r3;
};



//errores
float dx=0.0;
float dy=0.0;
float dz=0.0;
float dh=0.0;

//errores previos
float Pdx=0.0;
float Pdy=0.0;
float Pdz=0.0;
float Pdh=0.0;

//PID Y
float Kpro=2.0;
float Kdif=80.0;
//float Kint=0.0;
//float ISum=0.0;


//PID Z
float Kpro2=2.0;
float Kdif2=80.0;
float Kint2=0.0;
float ISum2=0.0;


//PID X
float Kpro3=10.0;
float Kdif3=1.0;
float Kint3=0.1;
float ISum3=0.0;





//PID Hipotenusa
float Kproh=2.0;
float Kdifh=80.0;
//float Kinth=0.0;
//float ISumh=0.0;


// flag para el piloto automatico
bool PA = true;



// destinos intermedios
/*
int P = 6;
float Puertas[P][3]= {3.5,2.2,0.0,    //ultimo punto, DOCK
                      3.5,2.2,2.0,
                      
                      //0.95,1.8,3.0,
                      
                      //-0.6,1.8,-3.5,
                      //-0.6,1.8,4.8,
                      -1.0,1.8,8.0,
                      1.15,1.8,-6.5,
                      -1.0,1.8,8.0,
                      //1.15,1.8,-6.5,
                      1.15,1.8,-6.5,   
};
*/
#define NUM_OBS 2
float Obstaculos[NUM_OBS][3]= {0.95,3.0,2.0,
                                -0.6,3.2,0.0
};


// Producto matricial y escalar

float DotProd(float v[3],float w[3]);

float MatrixProd(float A[3][3], float B[3][3]);



const float PI = 3.14159265;		// M_Pi is not avaible in all C++ compilers



// Tiempo de ciclo

const float Dt=0.032;						// contrller robot step [s]	    



// Sensores

DeviceTag camera;								// Camara frontal

DeviceTag gps;									// GPS

DeviceTag ds0, ds1;							// Sensores de distancia al suelo



// Constantes para el modelo dinamico

const float g = 9.81;						// Gravedad [m/s^2]

const float m = 0.25;						// Masa del robot [kg]

const float CDTransl[] = {0.07, 0.25, 0.25};	// Translational drag coefficients
																							// from wind tunnel experiments [kg/m]
																							// include air density, surface and 1/2 factor

const float CDRot[] = {0.016, 0.016, 0.01};		// Rotational drag coefficients
																							// from empirical experiments [kg*m^2]
																							// include air density, surface 

const float r[] = {0.005,0,-0.25};						// Vector (COB - COG): center of buoyancy, center of gravity
																							// expressed in Aeronautical relative gravity ref. [m]

const float angles[] = {0,0.04,0.12};					// Angles [rad] betwenn COG and motors

const float distance[] = {0.47,0.52,0.12};		// Distance [m] between COG and motors

const float motMaxThrust = 0.05;							// Motors max propulsion force [N] 




// calcula el modulo de un vector
float modulo_vector(Punto ori, Punto dest){
  
  float modulo;
  
  modulo = sqrt( (dest.x-ori.x)*(dest.x-ori.x) +
                  (dest.y-ori.y)*(dest.y-ori.y) +
                  (dest.z-ori.z)*(dest.z-ori.z));
  
  return modulo;

}


// calcula el modulo de un vector, pero sin calcular la raiz cuadrada (para modulos al cuadrado)
float modulo_sin_raiz(Punto ori, Punto dest){
  
  float modulo;
  
  modulo = (dest.x-ori.x)*(dest.x-ori.x) +
            (dest.y-ori.y)*(dest.y-ori.y) +
            (dest.z-ori.z)*(dest.z-ori.z);
  
  return modulo;

}


// normaliza un vector
Punto normalizar(Punto ori, Punto dest){
  
  Punto normal;
  float modulo = modulo_vector(ori, dest);
  
  normal.x = (dest.x-ori.x) / modulo;
  normal.y = (dest.y-ori.y) / modulo;
  normal.z = (dest.z-ori.z) / modulo;
  
  return normal;

}


// rutina que calcula si habra colision
void detecta_colision(Esfera ori, Esfera dest, Biesfera* obs){
  
  Punto intermedio;
  
  float mtdcalculado=0.0;
  float lambda=0.0;
  float lambda1=0.0;
  float lambda2=0.0;
  
  int mtd=0;
  
  
  
  
  /*******************************/
  /*  Diferencias de MINKOSWSKI  */
  /*******************************/
  Esfera m0,m1,m2,m3;
  
  m0.c = ori.c - (obs->c0);
  m1.c = ori.c - (obs->c1);
  m2.c = dest.c - (obs->c0);
  m3.c = dest.c - (obs->c1);
  
  m0.r = ori.r + (obs->r0);
  m1.r = ori.r + (obs->r1);
  m2.r = dest.r + (obs->r0);
  m3.r = dest.r + (obs->r1);





  
  /*******************************/
  /*      calculo de LAMBDAS     */
  /*******************************/
  float l1,l2,l;
  
  
  float a;
  float a1, a2;
  a1= (m2.c - m0.c)*(m0.c);
  a2= (m3.c - m0.c)*(m2.c - m0.c);
  
  a= a1 * a2;
  
  /***************************************/
  
  float b;
  float b1;
  float b2;
  b1= (m3.c - m0.c)*(m0.c);
  b2= modulo_sin_raiz( m0.c,m2.c );
  
  b= b1 * b2;
  
  
  /***************************************/
  
  float c,c1,c2;
  c1= modulo_sin_raiz( m0.c,m3.c );
  c2= modulo_sin_raiz( m0.c,m2.c );
  
  c= c1*c2;
  
  
  /***************************************/
  
  float d;
  float d1;
  d1= (m3.c - m0.c)*(m2.c - m0.c);
  
  d= d1 * d1;
  
  
  /*********************/
  /*        l1         */
  /*                   */
    l1 =  (a-b) / (c-d);
  /*                   */
  /*********************/

  
  
  /***************************************/
  //   cambios para l2
  /***************************************/
  
  a1= (m3.c - m0.c)*(m0.c);
  a2= (m3.c - m0.c)*(m2.c - m0.c);
  
  a= a1 * a2;
  
  /***************************************/
  
  
  b1= (m2.c - m0.c)*(m0.c);
  b2= modulo_sin_raiz( m0.c,m3.c );
  
  b= b1 * b2;

  /***************************************/
  
  /*********************/
  /*        l2         */
  /*                   */
    l2 =  (a-b) / (c-d);
  /*                   */
  /*********************/
  
  /*********************/
  /*        l         */
  /*                   */
    l = l1 + l2;
  /*                   */
  /*********************/
    
    
    
    
  
  
  // conociendo las lambdas, determinamos que mtd debemos aplicar
  
  if(l<=1){
    
    if(l2>=0){
      
      if(l1>=0){
        mtd=1;
      }else{
        mtd=2;
      }
    
    }else{
      
      if(l1>=0){
        mtd=3;
      }else{
        mtd=4;
      }
    
    }
    
  } else{
    
    if(l2>=0){
      
      if(l1>=0){
        mtd=5;
      }else{
        mtd=6;
      }
    
    }else{
      
      mtd=7;
      
    }
    
  }

  
  // en caso de que afecte a la otra triesfera
  float l1bi=0.0;
  float l2bi=0.0;
  float lbi=0.0;
  
  if(mtd==3 || mtd==7){
    
    // cambio de lambdas
    l1bi=-l2;
    l2bi=l1+l2;
    lbi=l1bi+l2bi;
    
    
    if(lbi<=1){
    
    if(l2bi>=0){
      
      if(l1bi>=0){
        mtd=8;
      }else{
        mtd=0;
      }
    
    }else{
      
      if(l1bi>=0){
        mtd=9;
      }else{
        mtd=0;
      }
    
    }
    
  } else{
    
    if(l2bi>=0){
      
      if(l1bi>=0){
        mtd=10;
      }else{
        mtd=0;
      }
    
    }else{
      
      mtd=11;
      
    }
    
  }
    
  }


  // segun el mtd aplicaremos unos calculos u otros
  Triesfera tri;
  Biesfera bi;
  Punto cero;
  Punto proyeccion;
  Punto proyeccion1;
  Punto proyeccion2;
  float d0;
  
  float mtd1=0.0;
  float mtd2=0.0;
  float radio1;
  float radio2;
  int tipo=0;       //0->nada, 1->esfera, 2->biesfera, 3->triesfera
  
  
  switch(mtd){
  
    case 0:
    
      robot_console_printf("\n\n\t¡¡¡¡ERROR EN LOS CALCULOS!!!!\t\n\n");
      break;
      
      
      
      
    case 1:
      
      tipo=3;
      //S032
      tri.r0=m0.r;
      tri.r1=m3.r;
      tri.r2=m2.r;
      tri.c0=m0.c;
      tri.c1=m3.c;
      tri.c2=m2.c;
  
      
      proyeccion= (tri.c0 + ((tri.c1-tri.c0)^l1) + ((tri.c2-tri.c0)^l2));
      
      d0=modulo_vector(cero,proyeccion);
      
      mtdcalculado=d0-(tri.r0+((tri.r1-tri.r0)*l1)+((tri.r2-tri.r0)*l2));
      
      //mtdcalculado=d0-(tri.r0+((tri.r1-tri.r0)*l1)+((l2/(1.0-l1))*( ((tri.r0-tri.r2)*l1)+tri.r2-tri.r0 )));
      
      obs->radio=(tri.r0+((tri.r1-tri.r0)*l1)+((tri.r2-tri.r0)*l2));
      obs->lambda=l;
      obs->mtd=mtdcalculado;
      obs->proy=proyeccion;
      
      
      // mensajes para mostrar calculos realizados
      /*
      robot_console_printf("c0x:%f\t",tri.c0.x);
      robot_console_printf("c0y:%f\t",tri.c0.y);
      robot_console_printf("c0z:%f\t",tri.c0.z);
      robot_console_printf("r0:%f\t",tri.r0);
      
      robot_console_printf("c1x:%f\t",tri.c1.x);
      robot_console_printf("c1y:%f\t",tri.c1.y);
      robot_console_printf("c1z:%f\t",tri.c1.z);
      robot_console_printf("r1:%f\t",tri.r1);
      
      robot_console_printf("c2x:%f\t",tri.c2.x);
      robot_console_printf("c2y:%f\t",tri.c2.y);
      robot_console_printf("c2z:%f\t",tri.c2.z);
      robot_console_printf("r2:%f\t",tri.r2);
      
      robot_console_printf("Px:%f\t",proyeccion.x);
      robot_console_printf("Py:%f\t",proyeccion.y);
      robot_console_printf("Pz:%f\t",proyeccion.z);
      robot_console_printf("d0:%f\t",d0);
      robot_console_printf("mtd:%f\t",mtdcalculado);
      */

      break;
      
      
      
    
    case 2:
     
      tipo=2;
      //S02
      bi.r0=m0.r;
      bi.r1=m2.r;      
      bi.c0=m0.c;
      bi.c1=m2.c;
      
  
      lambda=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion= (bi.c0 + ((bi.c1-bi.c0)^lambda));
      
      if(lambda<0.0){
        tipo=1;
        mtdcalculado=modulo_vector(cero,bi.c0)-bi.r0;
        obs->radio=bi.r0;
      }else if(lambda>1.0){
        tipo=1;
        mtdcalculado=modulo_vector(cero,bi.c1)-bi.r1;
        obs->radio=bi.r1;
      }else{
        mtdcalculado=modulo_vector(cero,proyeccion) - (bi.r0 + ((bi.r1-bi.r0)*lambda));
        obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda));
      }
      
      
      obs->lambda=lambda;
      obs->mtd=mtdcalculado;
      obs->proy=proyeccion;
      break;
      
      
      
      
    case 3:
      break;
     
     
     
     
      
    case 4:
            
      tipo=2;
      //S02
      bi.r0=m0.r;
      bi.r1=m2.r;      
      bi.c0=m0.c;
      bi.c1=m2.c;
      
  
      lambda2=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion2= (bi.c0 + ((bi.c1-bi.c0)^lambda2));
      
      if(lambda2<0.0){
        tipo=1;
        mtd2=modulo_vector(cero,bi.c0)-bi.r0;
        radio2=bi.r0;
      }else if(lambda2>1.0){
        tipo=1;
        mtd2=modulo_vector(cero,bi.c1)-bi.r1;
        radio2=bi.r1;
      }else{
        mtd2=modulo_vector(cero,proyeccion2) - (bi.r0 + ((bi.r1-bi.r0)*lambda2));
        radio2=(bi.r0 + ((bi.r1-bi.r0)*lambda2));
      }
      
      
       
      
      //S01 --> S10
      bi.r0=m1.r;
      bi.r1=m0.r;      
      bi.c0=m1.c;
      bi.c1=m0.c;
      
  
      lambda1=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion1= (bi.c0 + ((bi.c1-bi.c0)^lambda1));
      
      if(lambda1<0.0){
        tipo=1;
        mtd1=modulo_vector(cero,bi.c0)-bi.r0;
        radio1=bi.r0;
      }else if(lambda1>1.0){
        tipo=1;
        mtd1=modulo_vector(cero,bi.c1)-bi.r1;
        radio1=bi.r1;
      }else{
        mtd1=modulo_vector(cero,proyeccion1) - (bi.r0 + ((bi.r1-bi.r0)*lambda1));
        radio1=(bi.r0 + ((bi.r1-bi.r0)*lambda1));
      }
      
      
      
      
      if(mtd1<=mtd2){
        mtdcalculado=mtd1;
          
        obs->lambda=lambda1;
        obs->mtd=mtdcalculado;
        obs->proy=proyeccion1;
        obs->radio=radio1;
      }else{
        mtdcalculado=mtd2;
  
        obs->lambda=lambda2;
        obs->mtd=mtdcalculado;
        obs->proy=proyeccion2;
        obs->radio=radio2;
      }
      break;
      
      
     
     
     
    case 5:
           
      tipo=2;
      //S32 --> S23
      bi.r0=m2.r;
      bi.r1=m3.r;      
      bi.c0=m2.c;
      bi.c1=m3.c;
      
  
      lambda=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion= (bi.c0 + ((bi.c1-bi.c0)^lambda));
      
      if(lambda<0.0){
        tipo=1;
        mtdcalculado=modulo_vector(cero,bi.c0)-bi.r0;
        obs->radio=bi.r0;
      }else if(lambda>1.0){
        tipo=1;
        mtdcalculado=modulo_vector(cero,bi.c1)-bi.r1;
        obs->radio=bi.r1;
      }else{
        mtdcalculado=modulo_vector(cero,proyeccion) - (bi.r0 + ((bi.r1-bi.r0)*lambda));
        obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda));
      }
      
      
      obs->lambda=lambda;
      obs->mtd=mtdcalculado;
      obs->proy=proyeccion;
      break;
      
    
    
    
    
    
    case 6:
            
      tipo=2;     
      //S02
      bi.r0=m0.r;
      bi.r1=m2.r;      
      bi.c0=m0.c;
      bi.c1=m2.c;
      
  
      lambda2=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion2= (bi.c0 + ((bi.c1-bi.c0)^lambda2));
      
      if(lambda2<0.0){
        tipo=1;
        mtd2=modulo_vector(cero,bi.c0)-bi.r0;
        obs->radio=bi.r0;
      }else if(lambda2>1.0){
        tipo=1;
        mtd2=modulo_vector(cero,bi.c1)-bi.r1;
        obs->radio=bi.r1;
      }else{
        mtd2=modulo_vector(cero,proyeccion2) - (bi.r0 + ((bi.r1-bi.r0)*lambda2));
        obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda2));
      }
      
      
      
      
      
      //S32 --> S23
      bi.r0=m2.r;
      bi.r1=m3.r;      
      bi.c0=m2.c;
      bi.c1=m3.c;
      
  
      lambda1=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion1= (bi.c0 + ((bi.c1-bi.c0)^lambda1));
      
      if(lambda<0.0){
        tipo=1;
        mtd1=modulo_vector(cero,bi.c0)-bi.r0;
        obs->radio=bi.r0;
      }else if(lambda1>1.0){
        tipo=1;
        mtd1=modulo_vector(cero,bi.c1)-bi.r1;
        obs->radio=bi.r1;
      }else{
        mtd1=modulo_vector(cero,proyeccion1) - (bi.r0 + ((bi.r1-bi.r0)*lambda1));
        obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda1));
      }
      
      
      
      if(mtd1<=mtd2){
        mtdcalculado=mtd1;
          
        obs->lambda=lambda1;
        obs->mtd=mtdcalculado;
        obs->proy=proyeccion1;
        obs->radio=radio1;
      }else{
        mtdcalculado=mtd2;
          
        obs->lambda=lambda2;
        obs->mtd=mtdcalculado;
        obs->proy=proyeccion2;
        obs->radio=radio2;
      }
      break;
     
     
     
     
     
    case 7:
      break;
      
     
     
     
     
     
    case 8:
      
      
      tipo=3;
      //S013
      tri.r0=m0.r;
      tri.r1=m1.r;
      tri.r2=m3.r;
      tri.c0=m0.c;
      tri.c1=m1.c;
      tri.c2=m3.c;
  
      
      /*proyeccion= tri.c0 + ((tri.c1-tri.c0)^l1bi) + ((tri.c2-tri.c0)^l2bi);
      
      d0=modulo_vector(cero,proyeccion);
      
      mtdcalculado=d0-(tri.r0+((tri.r1-tri.r0)*l1bi)+((tri.r2-tri.r0)*l2bi));
      
      
      obs->lambda=lbi;
      obs->mtd=mtdcalculado;
      obs->proy=proyeccion;
      obs->radio=(tri.r0+((tri.r1-tri.r0)*l1bi)+((tri.r2-tri.r0)*l2bi));
      break;*/
      proyeccion= (tri.c0 + ((tri.c1-tri.c0)^l1bi) + ((tri.c2-tri.c0)^l2bi));
      
      d0=modulo_vector(cero,proyeccion);
      
      mtdcalculado=d0-(tri.r0+((tri.r1-tri.r0)*l1bi)+((tri.r2-tri.r0)*l2bi));
      
      //mtdcalculado=d0-(tri.r0+((tri.r1-tri.r0)*l1)+((l2/(1.0-l1))*( ((tri.r0-tri.r2)*l1)+tri.r2-tri.r0 )));
      
      obs->radio=(tri.r0+((tri.r1-tri.r0)*l1bi)+((tri.r2-tri.r0)*l2bi));
      obs->lambda=lbi;
      obs->mtd=mtdcalculado;
      obs->proy=proyeccion;
      break;
     
     
     
    
    
    case 9:
      
      
      tipo=2;
      //S01 --> S10
      bi.r0=m1.r;
      bi.r1=m0.r;      
      bi.c0=m1.c;
      bi.c1=m0.c;
      
  
      lambda=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion= (bi.c0 + ((bi.c1-bi.c0)^lambda));
      
      if(lambda<0.0){
        tipo=1;
        mtdcalculado=modulo_vector(cero,bi.c0)-bi.r0;
        obs->radio=bi.r0;
      }else if(lambda>1.0){
        tipo=1;
        mtdcalculado=modulo_vector(cero,bi.c1)-bi.r1;
        obs->radio=bi.r1;
      }else{
        mtdcalculado=(modulo_vector(cero,proyeccion)) - (bi.r0 + ((bi.r1-bi.r0)*lambda));
        obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda));
      }
      
      
      obs->lambda=lambda;
      obs->mtd=mtdcalculado;
      obs->proy=proyeccion;
      break;
  
  
  
  
  
  
  
    case 10:
      
      
      tipo=2;
      //S13
      bi.r0=m1.r;
      bi.r1=m3.r;      
      bi.c0=m1.c;
      bi.c1=m3.c;
      
  
      lambda=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion= (bi.c0 + ((bi.c1-bi.c0)^lambda));
      
      if(lambda<0.0){
        tipo=1;
        mtdcalculado=modulo_vector(cero,bi.c0)-bi.r0;
        obs->radio=bi.r0;
      }else if(lambda>1.0){
        tipo=1;
        mtdcalculado=modulo_vector(cero,bi.c1)-bi.r1;
        obs->radio=bi.r1;
      }else{
        mtdcalculado=modulo_vector(cero,proyeccion) - (bi.r0 + ((bi.r1-bi.r0)*lambda));
        obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda));
      }
      
      
      obs->lambda=lambda;
      obs->mtd=mtdcalculado;
      obs->proy=proyeccion;
      break;
     
     
     
     
     
      
    case 11:
            
      
      tipo=2;
      //S01 --> S10
      bi.r0=m1.r;
      bi.r1=m0.r;      
      bi.c0=m1.c;
      bi.c1=m0.c;
      
  
      lambda2=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion2= (bi.c0 + ((bi.c1-bi.c0)^lambda2));
      
      if(lambda2<0.0){
        tipo=1;
        mtd2=modulo_vector(cero,bi.c0)-bi.r0;
        radio2=bi.r0;
      }else if(lambda2>1.0){
        tipo=1;
        mtd2=modulo_vector(cero,bi.c1)-bi.r1;
        radio2=bi.r1;
      }else{
        mtd2=modulo_vector(cero,proyeccion2) - (bi.r0 + ((bi.r1-bi.r0)*lambda2));
        radio2=(bi.r0 + ((bi.r1-bi.r0)*lambda2));
      }
      
      
      
      
      
      //S13
      bi.r0=m1.r;
      bi.r1=m3.r;      
      bi.c0=m1.c;
      bi.c1=m3.c;
      
  
      lambda1=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) );
      
      proyeccion1= (bi.c0 + ((bi.c1-bi.c0)^lambda1));
      
      if(lambda1<0.0){
        tipo=1;
        mtd1=modulo_vector(cero,bi.c0)-bi.r0;
        radio1=bi.r0;
      }else if(lambda1>1.0){
        tipo=1;
        mtd1=modulo_vector(cero,bi.c1)-bi.r1;
        radio1=bi.r1;
      }else{
        mtd1=modulo_vector(cero,proyeccion1) - (bi.r0 + ((bi.r1-bi.r0)*lambda1));
        radio1=(bi.r0 + ((bi.r1-bi.r0)*lambda1));
      }
      
      
      
      
      if(mtd1<=mtd2){
        mtdcalculado=mtd1;
        
        obs->lambda=lambda1;
        obs->mtd=mtdcalculado;
        obs->proy=proyeccion1;
        obs->radio=radio1;
      }else{
        mtdcalculado=mtd2;
          
        obs->lambda=lambda2;
        obs->mtd=mtdcalculado;
        obs->proy=proyeccion2;
        obs->radio=radio2;
      }
      
      
      break;
  
  
    default:
      break;
  
  
  }
  
  // para mostrar calculos realizados
   
  robot_console_printf("estado:%d\t",mtd);
  
  /* 
  robot_console_printf("\tradio ori:%f\t",ori.r);
  
  robot_console_printf("mtd1:%f\t",mtd1);
  robot_console_printf("mtd2:%f\t",mtd2);
  robot_console_printf("mtd:%f\t",mtdcalculado);
  
  robot_console_printf("lambda:%f\t",lambda);
  robot_console_printf("lambda1:%f\t",lambda1);
  robot_console_printf("lambda2:%f\t",lambda2);
  
  robot_console_printf("l:%f\t",l);
  robot_console_printf("l1:%f\t",l1);
  robot_console_printf("l2:%f\t",l2);
  
  robot_console_printf("lbi:%f\t",lbi);
  robot_console_printf("l1bi:%f\t",l1bi);
  robot_console_printf("l2bi:%f\t",l2bi);
  */


}




// funcion de RESET
void reset(void) {

	
  // camara
  camera = robot_get_device("camera");
	if (camera==0) printf("Camera is missing!\n");

	
  // GPS
  gps = robot_get_device("gps");
	if (gps==0) printf("GPS is missing!\n");
  
 
  // sensores de distancia al suelo
  ds0 = robot_get_device("ds0");
  if (ds0==0) printf("Distance Sensor 0 is missing!\n");
  
  ds1 = robot_get_device("ds1");
  if (ds1==0) printf("Distance Sensor 1 is missing!\n");
  
  distance_sensor_enable(ds0,32);
  distance_sensor_enable(ds1,32);
  
}







// funcion MAIN 
int main() {

	
 

// ###### GPS BUG ######

	int firstStep=0;

// #####################

	

	const float *PosMatrix;									// GPS



	float APos[] = {0,0,0};									// Position of the blimp expressed
																					// in Aeronautical referential [m]



	float YW=0;															// position of the blimp expressed
																					// in Webots referential [m]



	float pAPos[] = {0,0,0};								// previous positions [m]



	float ASpeed[] = {0,0,0};								// linear velocities along X, Y, and Z body axes [m/s]

	float ARSpeed[] = {0,0,0};							// linear velocities expressed in Aeronautical
																					// rel. referential [m/s]



	float ARotSpeed[] = {0,0,0}; 						// angular velocities around X, Y, and Z body axes [rad/sec]

	float pARotSpeed[] = {0,0,0};						// previous angular velocities



	float FFMotAR=0, FYawMotAR=0, FVMotAR=0;// motor forces [N] expressed in Aeronautical
																					// relative referential (fixed to body)



	float MA[3][3];
				// rotation Matrix that transform a vector

				// expressed in AERONAUTICAL EARTH referential

				// in a vector expressed in AERONAUTICAL BODY ref.

				// x: forward, y: right, z: down.



	// Euler angles                                                     
	float Psi=0, Theta=0, Phi=0;			// Euler aeronautical angles [rad]

	float pPsi=0, pTheta=0, pPhi=0;		// previous angles [rad]


	float sinTheta;										// to compute angles!



	// forces expressed in Aeronautical relative referential [N]
	float FXAR=0, FYAR=0, FZAR=0; 



	// torques expressed in Aeronautical relative referential [m*N]
	float MXAR=0, MYAR=0, MZAR=0; 


  // fuerzas y momentos para Webots
	float FXWR=0, FYWR=0, FZWR=0;

	float MXWR=0, MYWR=0, MZWR=0;




  // llamada al robot
	robot_live(reset);

  gps_enable(gps,(short unsigned int)(Dt*1000));

	camera_enable(camera,(short unsigned int)(Dt*1000));
  
  robot_keyboard_enable((short unsigned int)(Dt*1000));
  
  
  
  
  // multiplicadores de la potencia de los motores
  float mForwardAxis, mVerticalAxis, mYawAxis;
	
	mForwardAxis = mVerticalAxis = mYawAxis = 0.0f;
  
  

  // estado inicial del automata
  int estado=BUSCA_ANGULO;
  
  
  // contador de destinos para los estados
  //int i = P-1;
  
  
  // botones para la version manual (se reserva para 6 teclas simultaneas pero se tienen en cuenta 4)
  int botones[6];
  
  // creamos la pila de puertas (destinos)
  PILA stack;
  stack.reset();
  
  stack.push(Punto(3.5,2.2,0.0)); //Docking
  stack.push(Punto(3.5,2.2,1.0)); // PREdocking2
  stack.push(Punto(3.5,2.2,3.0)); // PREdocking
  
  stack.push(Punto(-1.0,3.0,8.0));  // ultimo destino
  stack.push(Punto(1.15,3.0,-6.5));
  //stack.push(Punto(-1.0,1.8,8.0));
  //stack.push(Punto(1.15,1.8,-6.5)); // primer destino
  

	for(;;) { // The robot never dies!

  
  
  // Distancia al suelo
  unsigned short ds0_value;
  unsigned short ds1_value;
  
  ds0_value = distance_sensor_get_value(ds0);
  ds1_value = distance_sensor_get_value(ds1);



  // Obtener la imagen de la camara
	camera_get_image(camera);



	// Obtener la posicion y orientacion del robot
  float Pos[3];
  float Ppos[3];
  float Euler[3];
 
  
  // obtenemos datos del GPS
	PosMatrix = gps_get_matrix(gps);
  
  
  // guardamos las posiciones previas
  Ppos[0]=Pos[0];
  Ppos[1]=Pos[1];
  Ppos[2]=Pos[2];  
  
  
  // obtenemos las posiciones actuales
  Pos[0] = gps_position_x(PosMatrix); //X
  Pos[1] = gps_position_y(PosMatrix); //Y
  Pos[2] = gps_position_z(PosMatrix); //Z
  
  
  // obtenemos la orientacion con los angulos de Euler
  gps_euler(PosMatrix, Euler);      


  
  // Imprime posicion y orientacion
  /*
  robot_console_printf("%f\t", Pos[0]);
  robot_console_printf("%f\t", Pos[1]);
  robot_console_printf("%f\t", Pos[2]);
  
 
  robot_console_printf("%f\t", Euler[0]);
  robot_console_printf("%f\t", Euler[1]);
  robot_console_printf("%f\n", Euler[2]);
  */
  
  /*
  // prueba de la pila
  
  PILA stack; 
  stack.reset(); 
  
  stack.push(Punto(0.1,0.1,0.1));
  stack.push(Punto(0.2,0.2,0.2));
  stack.push(Punto(0.3,0.3,0.3));
  stack.push(Punto(0.4,0.4,0.4));
  
  Punto tope = stack.top();
  robot_console_printf("%f\t", tope.x);
  robot_console_printf("%f\t", tope.y);
  robot_console_printf("%f\t", tope.z);
  
  stack.pop();
  
  tope = stack.top();
  robot_console_printf("%f\t", tope.x);
  robot_console_printf("%f\t", tope.y);
  robot_console_printf("%f\t", tope.z);
  
  stack.reset(); 
  tope = stack.top();
  robot_console_printf("%f\t", tope.x);
  robot_console_printf("%f\t", tope.y);
  robot_console_printf("%f\t", tope.z);
  
  // FUNCIONAAA!!!
*/

  /**********************************************/
  /*              VERSION AUTOMATICA            */
  /**********************************************/
  if(PA){
    
    // punto destino
    float Dest[3];
    
    // distancia al destino
    float x=0.0;
    float y=0.0;
    
    // angulo hacia el destino
    float alfa=0.0;
    float angulofinal=0.0;
    
    // paramos los motores
    mVerticalAxis=0.0;
    mForwardAxis=0.0;
    mYawAxis=0.0;
  
    // PID error en X
    float Px=0.0;
    float Px2=0.0;
    float Sum=0.0;
    
    
    // actualizamos el destino
    Dest[0]=stack.top().x;
    Dest[1]=stack.top().y;
    Dest[2]=stack.top().z;
    /*
    Dest[0]=Puertas[i][0];
    Dest[1]=Puertas[i][1];
    Dest[2]=Puertas[i][2];
    */
    
    
    // calculamos distancias desde el robot al destino
    x=Dest[0]-Pos[0];   //X es x en el plano
    y=Dest[2]-Pos[2];   //Z es y en el plano
      
     
     
    // dependiendo del cuadrante donde se encuentre el destino,
    // nuestro angulo final cambiara
    // 2 | 1
    // --+--
    // 3 | 4
    
    if(x>=0 && y<0){     // destino en cuadrante 1
    
      alfa = atan(y/x);
      angulofinal = alfa;
   
    }
    
    if(x<=0 && y<0){     // destino en cuadrante 2
    
      alfa = atan(y/x);
      angulofinal = -(PI-alfa);  
      
    }
    
    if(x<=0 && y>0){     // destino en cuadrante 3
    
      alfa = atan(y/x);
      angulofinal = PI+alfa;
    
    }
    
    if(x>=0 && y>0){     // destino en cuadrante 4
    
      alfa = atan(y/x);
      angulofinal = alfa;
    
    }
    
   
    
    
    
    // inicializacion origen/destino
    bool colision;              // TRUE si se calcula colision
    Esfera origen;
    Esfera destino;
    Punto p_intermedio;         // punto donde se calcula no habra colision
    Biesfera obstaculo;
    float minlambda=100000;     // para saber el obstaculo mas cercano en la trayectoria
    int ilambda=0;              // indice del obstaculo mas cercano
    
    Biesfera obs[NUM_OBS];      // vector de obstaculos
    Punto centro;               // centro del obstaculo a tratar en la iteracion
    float distX=0.0;            // radio de la esfera que envuelve al robot
    Punto cero;                 // punto (0.0,0.0,0.0)

    
    
    
    
    // AUTOMATA DE ESTADOS
    switch(estado){
    
      case BUSCA_ANGULO:
      
        // restauramos los sumatorios del error para la parte integral de los controladores
        ISum3=0.0;
        Sum=0.0;
    
        
        // controlador PD para orientar el robot hacia el destino
        dx=angulofinal-Euler[1];         // el error es destino - pos actual
        mYawAxis = 
          Kpro3 * dx  +                  // parte proporcional
          //Kint3 * ISum3 +              // parte integral (no hace falta)
          Kdif3 * (dx - Pdx);            // parte derivada es error actual - error ant
        //ISum3 += dz;                   // sumatorio para parte integral
        Pdx = dx;                        // guardamos el error anterior
        
        
       
        // robot orientado hacia el destino -> pasamos al siguiente estado
        if(fabs(angulofinal-Euler[1])<0.002)
          estado=AVANZA;
           
        
        break;
      
      
      
      
      
      
      
      case AVANZA:
      
      
      
      /*********************************/
      /*   DETECCION DE COLISIONES     */
      /*********************************/
      
      colision = false;
      
      
      
      // el radio es proporcional a la distancia al eje X o el radio del robot
      if( x > 0.2 )
        distX = x;      
      else
        distX = 0.2;
      
      
      // inicializacion origen y destino
      origen.r = distX;            
      origen.c.x = Pos[0];
      origen.c.y = Pos[1];
      origen.c.z = Pos[2];
      
      destino.r = 0.2;
      destino.c.x = Dest[0];
      destino.c.y = Dest[1];
      destino.c.z = Dest[2];
      
      
      // imprime la posicion actual (origen de la iteracion)
      
      robot_console_printf("%f\t", origen.c.x);
      robot_console_printf("%f\t", origen.c.y);
      robot_console_printf("%f\t", origen.c.z);
      
      
      // imprime el destino actual
      robot_console_printf("DESTx:%f\t", Dest[0]);
      robot_console_printf("DESTy:%f\t", Dest[1]);
      robot_console_printf("DESTz:%f\t", Dest[2]);
      
      
        
      for(int j=0; j<NUM_OBS; j++){
        
        // inicializacion del obstaculo
        obs[j].lambda=10000;
        centro.Escribir(Obstaculos[j][0],Obstaculos[j][1],Obstaculos[j][2]);
        obs[j].c0 = centro;
        obs[j].c1 = centro;
        
        obs[j].r0 = 0.2;
        obs[j].r1 = 0.2;
        obs[j].c0.y = centro.y + 0.2;
        obs[j].c1.y = centro.y - 0.2;
        
        
        
        // llamada a la rutina de deteccion de colision
        // dado un origen, un destino y un obstaculo (biesfera)
        // completa los campos lambda, mtd y proyectado del obstaculo
        detecta_colision(origen, destino, &obs[j]);
        
        
        // imprime el lambda y el mtd del obstaculo de la iteracion
        /*
        robot_console_printf("LAMBDA:%f\t", obs[j].lambda);
        robot_console_printf("MTD:%f\t", obs[j].mtd);
        */
      }
        
      
      
      
      // buscamos el mas cercano que colisionara (lambda mas pequeña con mtd negativo)
      minlambda=10000;
      for(int k=0; k<NUM_OBS; k++){
        if( 
            obs[k].mtd < 0.0              // habra colision
            && obs[k].lambda < minlambda  // es la mas cercana
            && obs[k].lambda>0.0          // esta por delante del robot
            && obs[k].lambda<1.0          // esta antes del destino (podemos dejar margen)
          ){
          minlambda=obs[k].lambda;
          ilambda=k;
          
        }
      
      
      }
      
      // imprime los datos del obstaculo mas cercano con colision
      /*
      robot_console_printf("%f\t", obs[ilambda].lambda);
      robot_console_printf("%f\t", obs[ilambda].mtd);
      
      robot_console_printf("%f\t", obs[ilambda].c0.x);
      robot_console_printf("%f\t", obs[ilambda].c0.y-0.2);
      robot_console_printf("%f\t", obs[ilambda].c0.z);
      */

      // SI DISTANCIA <= 0 HAY COLISION
      if(obs[ilambda].mtd <= 0.0){
      
        
        // determinar posicion de maxima penetracion
        Punto maxpenetracion;
        
        // vector de la MTD
        Punto vmtd;
        
        
        robot_console_printf("lambda:%f\t", obs[ilambda].lambda);
        robot_console_printf("mtd:%f\t", obs[ilambda].mtd);
        
        maxpenetracion= origen.c + ((destino.c - origen.c)^obs[ilambda].lambda);
        
        vmtd = obs[ilambda].proy / modulo_vector(origen.c,obs[ilambda].proy);
        
        p_intermedio = maxpenetracion - ((normalizar(obs[ilambda].proy,origen.c))^((vmtd^obs[ilambda].mtd)*1.15));
        
        //robot_console_printf("x:%f\t", p_intermedio.x);
        //robot_console_printf("y:%f\t", p_intermedio.y);
        //robot_console_printf("z:%f\t", p_intermedio.z);
        
        // para evitar un globo submarino
        if(p_intermedio.y < 1.8){
          
          robot_console_printf("HOOOOOOOLAA");
          robot_console_printf("RADIO:%f\t", obs[ilambda].radio);
          robot_console_printf("MTD:%f\t", obs[ilambda].mtd);
          float mtd_nueva = 0.0;
          
          mtd_nueva = obs[ilambda].mtd + (obs[ilambda].radio*2.0);
          
          if(mtd_nueva > 0)
            mtd_nueva = -mtd_nueva;
          
          robot_console_printf("NEW_MTD:%f\t", mtd_nueva);
          
          
          obs[ilambda].proy.x=obs[ilambda].proy.x-obs[ilambda].c0.x;
          obs[ilambda].proy.y=obs[ilambda].proy.y-obs[ilambda].c0.y-0.2;
          obs[ilambda].proy.z=obs[ilambda].proy.z-obs[ilambda].c0.z;
          
          obs[ilambda].proy.x=-obs[ilambda].proy.x;
          obs[ilambda].proy.y=-obs[ilambda].proy.y;
          obs[ilambda].proy.z=-obs[ilambda].proy.z;
          
          obs[ilambda].proy.x=obs[ilambda].proy.x+obs[ilambda].c0.x;
          obs[ilambda].proy.y=obs[ilambda].proy.y+obs[ilambda].c0.y-0.2;
          obs[ilambda].proy.z=obs[ilambda].proy.z+obs[ilambda].c0.z;
          
          
          p_intermedio = maxpenetracion - ((normalizar(origen.c,obs[ilambda].proy))^((vmtd^mtd_nueva)*1.15));
          
        }
        
        
        
        
        /*
        // control de altitud
        if(p_intermedio.y < 1.6){
          p_intermedio.y = 1.6;
        }
        
        if(p_intermedio.y > 4.0){
          p_intermedio.y = 4.0;
        }
        */
        
      
        /*
        
        robot_console_printf("mtd:%f\t", obs[ilambda].mtd);
        
        
        robot_console_printf("x:%f\t", (normalizar(origen.c,obs[ilambda].proy)).x);
        robot_console_printf("y:%f\t", (normalizar(origen.c,obs[ilambda].proy)).y);
        robot_console_printf("z:%f\t", (normalizar(origen.c,obs[ilambda].proy)).z);
        
        
        robot_console_printf("Px:%f\t", obs[ilambda].proy.x);
        robot_console_printf("Py:%f\t", obs[ilambda].proy.y);
        robot_console_printf("Pz:%f\t", obs[ilambda].proy.z);
        
        robot_console_printf("x:%f\t", maxpenetracion.x);
        robot_console_printf("y:%f\t", maxpenetracion.y);
        robot_console_printf("z:%f\t", maxpenetracion.z);
        */
        robot_console_printf("x:%f\t", p_intermedio.x);
        robot_console_printf("y:%f\t", p_intermedio.y);
        robot_console_printf("z:%f\t", p_intermedio.z);
        
        
        
        
        // gestionar que no haya demasiados puntos intermedios
        //if(stack.elementos()>2 && fabs(stack.top().z-p_intermedio.z)<1.5 && origen.c.z>0){
        if(stack.elementos()>2 && fabs(stack.top().z-p_intermedio.z)<1.5){
         //p_intermedio.z=stack.pop().z;
          stack.pop();
        }
        
        
    
        stack.push(p_intermedio);
        
        //mVerticalAxis = -2.0;
        robot_console_printf("cuidadiTOOOOOOORRRR!!\n");
        /*
        Dest[0] = p_intermedio.x;
        Dest[1] = p_intermedio.y;
        Dest[2] = p_intermedio.z;*/
        
      }  else{
     
        robot_console_printf("\n");
        
      }
        
        
        
        
        
        
        // nos aseguramos de que entramos bien orientados por la puerta
        if(y<=0.6 && y>=0)
          angulofinal=1.57;
        if(y>=-0.6 && y<=0)
          angulofinal=-1.57;
        
        
        
        
        // controlador PID para orientar el robot hacia el destino
        // (nos interesa minimo error en el transitorio)
        dx=angulofinal-Euler[1];         // el error es destino - pos actual
        mYawAxis = 
          Kpro3 * dx  +                  // parte proporcional
          Kint3 * ISum3 +                // parte integral que parece no hacer falta
          Kdif3 * (dx - Pdx);            // parte derivada es error actual - error ant
        ISum3 += dx;                     // sumatorio para parte integral
        Pdx = dx;                        // para saber el error anterior
        
        // aplicamos un controlador PID sobre el error en X que cometemos al orientarnos
        // (caso de angulos negativos)
        if(Euler[1]<=0){
          x=Dest[0]-Pos[0];
          if(fabs(x)>=0.001)
            Pdx=Pdx -
                x*1.0 -
                Sum*1.5 -
                (x-Px)*3.0;
          Sum+=x;
          Px=x;
        }
        
        // aplicamos un controlador PID sobre el error en X que cometemos al orientarnos
        // (caso de angulos positivos)
        if(Euler[1]>0){
          x=Dest[0]-Pos[0];
          if(fabs(x)>=0.001)
            Pdx=Pdx +
                x*1.0 +
                Sum*1.5 +
                (x-Px2)*3.0;
          Sum+=x;
          Px2=x;
        }
        
        
        // controlador PD para mantener el robot en Y
        dy=Dest[1] - Pos[1];            // el error es destino - pos actual
        mVerticalAxis = 
          Kpro * dy  +                  // parte proporcional
          //Kint * ISum +               // parte integral (no hace falta)
          Kdif * (dy - Pdy);            // parte derivada es error actual - error ant
        //ISum += dy;                   // sumatorio para parte integral
        Pdy = dy;                       // para saber el error anterior
    
        // cambiamos la potencia por la posicion del motor
        mVerticalAxis = -mVerticalAxis;	
    
    
        // controlador PD para avanzar en Z
        // (teniendo en cuenta la distancia, hipotenusa, al destino)
        dh=fabs( sqrt( (x*x) + (y*y) ) );         // el error es destino - pos actual
        mForwardAxis = 
          Kproh * dh  +                           // parte proporcional
          //Kinth * ISum2 +                       // parte integral (no hace falta)
          Kdifh * (dh - Pdh);                     // parte derivada es error actual - error ant
        //ISumh += dh;                            // sumatorio para parte integral
        Pdh = dh;
        
        
        
        // si es la ultima puerta, pasamos a la maniobra de PREDOCKING
        if(stack.elementos()==2 ){
       
          estado=PREDOCKING;
        
        }
          
        
        
        // si llega al destino -> pasa a buscar el siguiente destino
        if( fabs(x)<= 0.4 && fabs(y)<= 0.1){
          stack.quitar();
          estado=BUSCA_ANGULO;
          robot_console_printf("\nHe llegado a mi destino!!!\n");
        }
        
        
        break;
          
      
      
      
      case PREDOCKING:
          
          // valores mas adecuados a la maniobra
          Kpro=4.0;
          Kdif=80.0;
          
          Kproh=1.0;
          Kdifh=180.0;
          
          Kpro2=1.0;
          Kdif2=180.0;
          
          
          
          
          
          
          dx=-1.57-Euler[1];            // el error es destino - pos actual
          mYawAxis = 
            Kpro3 * dx  +                  // parte proporcional
            //Kint3 * ISum3 +                 // parte integral que parece no hacer falta
            Kdif3 * (dx - Pdx);            // parte derivada es error actual - error ant
          //ISum3 += dz;                     // sumatorio para parte integral
          Pdx = dx;                       // para saber el error anterior
          
          
          dy=Dest[1] - Pos[1];            // el error es destino - pos actual
          mVerticalAxis = 
            Kpro * dy  +                  // parte proporcional
            //Kint * ISum +                 // parte integral que parece no hacer falta
            Kdif * (dy - Pdy);            // parte derivada es error actual - error ant
          //ISum += dy;                     // sumatorio para parte integral
          Pdy = dy;                       // para saber el error anterior
      
          mVerticalAxis = -mVerticalAxis;	
      
      
      
          dz=Dest[2] - Pos[2];            // el error es destino - pos actual
          mForwardAxis = 
            Kpro2 * dz  +                  // parte proporcional
            //Kint2 * ISum2 +                 // parte integral que parece no hacer falta
            Kdif2 * (dz - Pdz);            // parte derivada es error actual - error ant
          //ISum2 += dz;                     // sumatorio para parte integral
          Pdz = dz;                       // para saber el error anterior
      
      
          dh=fabs( sqrt( (x*x) + (y*y) ) );            // el error es destino - pos actual
          mForwardAxis = 
            Kproh * dh  +                  // parte proporcional
            //Kinth * ISum2 +                 // parte integral que parece no hacer falta
            Kdifh * (dh - Pdh);            // parte derivada es error actual - error ant
          //ISumh += dh;                     // sumatorio para parte integral
          Pdh = dh;
      
      
          if(fabs(dz)<=0.1){
              estado=PREDOCKING2;
          }
          
          break;
      
      
      case PREDOCKING2:
          
          
          Kpro=4.0;
          Kdif=100.0;
          
          Kpro2=4.0;
          Kdif2=180.0;
          
          Kproh=2.0;
          Kdifh=180.0;
          
          
          
          
          dx=0.0-Euler[1];            // el error es destino - pos actual
          mYawAxis = 
            Kpro3 * dx  +                  // parte proporcional
            //Kint3 * ISum3 +                 // parte integral que parece no hacer falta
            Kdif3 * (dx - Pdx);            // parte derivada es error actual - error ant
          //ISum3 += dz;                     // sumatorio para parte integral
          Pdx = dx;                       // para saber el error anterior
          
          
          dy=Dest[1] - Pos[1];            // el error es destino - pos actual
          mVerticalAxis = 
            Kpro * dy  +                  // parte proporcional
            //Kint * ISum +                 // parte integral que parece no hacer falta
            Kdif * (dy - Pdy);            // parte derivada es error actual - error ant
          //ISum += dy;                     // sumatorio para parte integral
          Pdy = dy;                       // para saber el error anterior
      
          mVerticalAxis = -mVerticalAxis;	
      
          
      
          dh=Dest[0]-Pos[0];            // el error es destino - pos actual
          mForwardAxis = 
            Kproh * dh  +                  // parte proporcional
            //Kinth * ISum2 +                 // parte integral que parece no hacer falta
            Kdifh * (dh - Pdh);            // parte derivada es error actual - error ant
          //ISumh += dh;                     // sumatorio para parte integral
          Pdh = dh;
          
      
          if(fabs(dh)<=0.001){
              stack.quitar();
              estado=DOCKING;
          }
          
          break;
          
        
      
      case DOCKING:
          
          
          Kpro=4.0;
          Kdif=120.0;
          
          Kpro2=4.0;
          Kdif2=180.0;
          
          
          
          
          dx=1.57-Euler[1];            // el error es destino - pos actual
          mYawAxis = 
            Kpro3 * dx  +                  // parte proporcional
            //Kint3 * ISum3 +                 // parte integral que parece no hacer falta
            Kdif3 * (dx - Pdx);            // parte derivada es error actual - error ant
          //ISum3 += dz;                     // sumatorio para parte integral
          Pdx = dx;                       // para saber el error anterior
          
          
          dy=Dest[1] - Pos[1];            // el error es destino - pos actual
          mVerticalAxis = 
            Kpro * dy  +                  // parte proporcional
            //Kint * ISum +                 // parte integral que parece no hacer falta
            Kdif * (dy - Pdy);            // parte derivada es error actual - error ant
          //ISum += dy;                     // sumatorio para parte integral
          Pdy = dy;                       // para saber el error anterior
      
          mVerticalAxis = -mVerticalAxis;	
      
      
      
          dz=Dest[2] - Pos[2];            // el error es destino - pos actual
          mForwardAxis = 
            Kpro2 * dz  +                  // parte proporcional
            //Kint2 * ISum2 +                 // parte integral que parece no hacer falta
            Kdif2 * (dz - Pdz);            // parte derivada es error actual - error ant
          //ISum2 += dz;                     // sumatorio para parte integral
          Pdz = dz;                         // para saber el error anterior
     
     
     
          if(fabs(dz)<=0.001){
            robot_console_printf("\nDocking COMPLETADO\n");
          }
            
          
          break;
        
      
      
        
        
      default:
          break;
  
    } 
  }
  /**********************************************/
  /*           FIN VERSION AUTOMATICA           */
  /**********************************************/
  
  
  

  // Obtenemos los valores de las teclas pulsadas
  botones[0]=robot_keyboard_get_key();
  botones[1]=robot_keyboard_get_key();
  botones[2]=robot_keyboard_get_key();
  botones[3]=robot_keyboard_get_key();
  
  
  
 	// si se presiona la 'P', se cambia de modo automatico/manual
  if(botones[0]==80 || botones[1]==80 || botones[2]==80 ||botones[3]==80){
    if(PA)
      PA=false;
    else
      PA=true;
  }






  /**********************************************/
  /*              VERSION MANUAL                */
  /**********************************************/
    
  if(!PA){
    
    if(botones[0]==317 || botones[1]==317 || botones[2]==317)
      mForwardAxis = 20.0;
    else if(botones[0]==319 || botones[1]==319 || botones[2]==319)
      mForwardAxis = -20.0;
    else
      mForwardAxis = 0.0;
      
  
      
    if(botones[0]==318 || botones[1]==318 || botones[2]==318)
      mYawAxis = 10.0;
    else if(botones[0]==316 || botones[1]==316 || botones[2]==316)
      mYawAxis = -10.0;
    else
      mYawAxis = 0.0;
      
      
    if(botones[0]==90 || botones[1]==90 || botones[2]==90)
      mVerticalAxis = 20.0;
    else if(botones[0]==65 || botones[1]==65 || botones[2]==65)
      mVerticalAxis = -20.0;
    else
      mVerticalAxis = 0.0;
    
  }
  /**********************************************/
  /*            FIN VERSION MANUAL              */
  /**********************************************/

  
  
  
  // Retenedores
  if(mVerticalAxis>=2.0)
    mVerticalAxis=2.0;
        
  if(mForwardAxis>=2.0)
    mForwardAxis=2.0;
  
  if(mYawAxis>=2.0)
    mYawAxis=2.0;
    
  if(mVerticalAxis<=-2.0)
    mVerticalAxis=-2.0;
        
  if(mForwardAxis<=-2.0)
    mForwardAxis=-2.0;
  
  if(mYawAxis<=-2.0)
    mYawAxis=-2.0;
  
  
  // imprime estado
  //robot_console_printf("%d\n", estado);
     
     
     
  // imprime multiplicadores de los motores
  /*
      robot_console_printf("%f\t", mVerticalAxis);
      robot_console_printf("%f\t", mForwardAxis);
      robot_console_printf("%f\t", mYawAxis);
  */
  
  
  
  
  
  
  // Maniobra automática para evitar que salga hacia arriba
  if(Pos[1]>=4.0){
    robot_console_printf("P.A. on: Evitando salida a la estratosfera\n");
    mVerticalAxis = 1.5;
  }
    
    
  //Maniobra automática para evitar que choque contra el suelo 
  if((ds0_value >= 800 || ds1_value >= 800) && !(estado==DOCKING) && !(estado==PREDOCKING2)){
  
    //robot_console_printf("WARNING!!\tWARNING!! Altura demasiado baja!!\n");
    
  }
  if((ds0_value >= 900 || ds1_value >= 900) && !(estado==DOCKING) && !(estado==PREDOCKING2)){
  
    mVerticalAxis = -2.0;
    
    //robot_console_printf("P.A. on: INICIANDO MANIOBRA DE EMERGENCIA!!\n");

  }  
    
    
   
    
    
    
    
		// Compute Rotation Matrix MA

		MA[0][0] = PosMatrix[0]; MA[0][1] =  PosMatrix[2]; MA[0][2] = -PosMatrix[1];

		MA[1][0] = PosMatrix[8]; MA[1][1] =  PosMatrix[10]; MA[1][2] = -PosMatrix[9];

		MA[2][0] = -PosMatrix[4]; MA[2][1] = -PosMatrix[6]; MA[2][2] = PosMatrix[5];





// ###### GPS BUG ######

		//printf("Matrice gps [0][0] %f\n", MA[0][0]);

		//printf("Matrice gps [0][1] %f\n", MA[0][1]);

		//printf("Matrice gps [0][2] %f\n", MA[0][2]);

		//printf("APos 0 %f\n", APos[0]);

		//printf("APos 1 %f\n", APos[1]);

		//printf("APos 2 %f\n", APos[2]);

//######################





		// Save previous values for next time step	

		pPsi = Psi;

		pTheta = Theta;

		pPhi = Phi;

		

		// Compute Euler angles

		// Finding Theta

		sinTheta = -MA[0][2];

		Theta = asin(sinTheta); // it works because Theta is in -Pi/2, Pi/2.

		// Finding Phi and Psi

		// works if Theta != Pi/2, that's the case for the blimp



		Phi = atan2(MA[1][2], MA[2][2]);

		Psi = atan2(MA[0][1], MA[0][0]);



		// Save previous values for next time step



		pAPos[0] = APos[0];

		pAPos[1] = APos[1];

		pAPos[2] = APos[2];



		APos[0] = PosMatrix[12];

		APos[1] = PosMatrix[14];

		APos[2] = -PosMatrix[13];

		YW = -APos[2];



		pARotSpeed[0] = ARotSpeed[0];

		pARotSpeed[1] = ARotSpeed[1];

		pARotSpeed[2] = ARotSpeed[2];

		

		ASpeed[0] = (APos[0]-pAPos[0])/(Dt);

		ASpeed[1] = (APos[1]-pAPos[1])/(Dt);

		ASpeed[2] = (APos[2]-pAPos[2])/(Dt);



		ARSpeed[0] = cos(Psi)*cos(Theta)*ASpeed[0]     // Rotation form Aero. ref. to Aero rel. ref.

			+ sin(Psi)*cos(Theta)*ASpeed[1]

			- sin(Theta)*ASpeed[2];



		ARSpeed[1] = (-sin(Psi)*cos(Phi)+cos(Psi)*sin(Theta)*sin(Phi))*ASpeed[0]

			+ (cos(Psi)*cos(Phi)+sin(Psi)*sin(Theta)*sin(Phi))*ASpeed[1]

			+ cos(Theta)*sin(Phi)*ASpeed[2];



		ARSpeed[2] = (sin(Psi)*sin(Phi) + cos(Psi)*sin(Theta)*cos(Phi))*ASpeed[0]

			+ (-cos(Psi)*sin(Phi) + sin(Psi)*sin(Theta)*cos(Phi))*ASpeed[1]

			+ cos(Theta)*cos(Phi)*ASpeed[2];



		ARotSpeed[0] = (Phi-pPhi)/(Dt);

		if (fabs(Phi-pPhi)> PI) ARotSpeed[0] =pARotSpeed[0]; 			// Can't find the speed if abs(Psi-pPsi) > PI!

		ARotSpeed[1] = (Theta-pTheta)/(Dt);

		if (fabs(Theta-pTheta)> PI) ARotSpeed[1] =pARotSpeed[1];	// Can't find the speed if abs(Psi-pPsi) > PI!

		ARotSpeed[2] = (Psi-pPsi)/(Dt);

		if (fabs(Psi-pPsi)> PI) ARotSpeed[2] =pARotSpeed[2]; 			// Can't find the speed if abs(Psi-pPsi) > PI!



		// Compute propulsion forces

		FFMotAR =  mForwardAxis*motMaxThrust;

		FYawMotAR = mYawAxis*motMaxThrust;

		FVMotAR = mVerticalAxis*motMaxThrust;



		// Calculate rel. generalized forces



		// FORCES

		//	restoring forces   	 	 	      motor forces   	        aerodynamical forces

		FXAR = m*g*sin(Theta)           + (FFMotAR)            - CDTransl[0]*fabs(ARSpeed[0])*ARSpeed[0];

		FYAR = -m*g*cos(Theta)*sin(Phi)                        - CDTransl[1]*fabs(ARSpeed[1])*ARSpeed[1];

		FZAR = -m*g*cos(Theta)*cos(Phi) + FVMotAR              - CDTransl[2]*fabs(ARSpeed[2])*ARSpeed[2];



		// We supposed that buoyancy = weight (that is implemented in WEBOTS)



		// TORQUES

		//      restoring torques   motor torques   	aerodynamical decrease of torques

		MXAR = m*g*r[2]*cos(Theta)*sin(Phi)

															- distance[1]*FYawMotAR*sin(angles[1])

																								- CDRot[0]*fabs(ARotSpeed[0])*ARotSpeed[0];

		MYAR = m*g*r[2]*sin(Theta)

    + m*g*r[0]*cos(Theta)*cos(Phi)

															+ distance[0]*FFMotAR*sin(angles[0])

															-	distance[2]*FVMotAR*cos(angles[2]) 

																								- CDRot[1]*fabs(ARotSpeed[1])*ARotSpeed[1];

		MZAR = - m*g*r[0]*cos(Theta)*sin(Phi)

															+ FYawMotAR*distance[1]*cos(angles[1])

																								- CDRot[2]*fabs(ARotSpeed[2])*ARotSpeed[2]; 



		// Transformation into webots relative coordinates

		FYWR = -FZAR;  

		FXWR =  FXAR;  

		FZWR =  FYAR;

		MYWR = -MZAR;

		MXWR =  MXAR;

		MZWR =  MYAR;

/*
      robot_console_printf("%f\t", FXWR);
      robot_console_printf("%f\t", FYWR);
      robot_console_printf("%f\t", FZWR);
      
      robot_console_printf("%f\t", MXWR);
      robot_console_printf("%f\t", MYWR);
      robot_console_printf("%f\n", MZWR);

*/
		// Set rel. forces and torque

// ###### GPS BUG ######

		if (firstStep<2) { custom_robot_set_rel_force_and_torque(0,9.8*0.25,0,0,0,0); firstStep++; }

		else 

// #####################

		custom_robot_set_rel_force_and_torque(FXWR,FYWR,FZWR,MXWR,MYWR,MZWR);



		robot_step((int)(Dt*1000)); /* run one step */

	}

	return 0;

}



//------------------------------------------------------------------------------

