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

  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>

//ESTADOS
  #define BUSCA_ANGULO 1
  #define AVANZA 2
  #define PREDOCKING 3
  #define PREDOCKING2 4
  #define DOCKING 5
  
  
  //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=4.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;
  
  
  
  
  // destinos intermedios
  float Puertas[7][3]= {0.95,1.8,3.0,
                        1.15,1.8,-6.5,
                        -0.6,1.8,-3.5,
                        -0.6,1.8,4.8,
                        -1.0,1.8,8.0,
                        
                        
                        3.5,2.2,2.0,
                        3.5,2.2,0.0};   //ultimo punto, DOCK
  
 

// 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] 




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,64);
  distance_sensor_enable(ds1,64);
  


}







 
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));
  
  
  
  
  // 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 para los estados
  int i = 0;

 
  

	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]);
  */
  

  /**********************************************/
  /*              VERSION AUTOMATICA            */
  /**********************************************/
  
  
  // 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]=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
  
  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 = 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 = -(PI-alfa);  
    
  
  }
  
  
  
  // 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:
      
      // 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)*5.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)*5.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(i==5 && (fabs(dh)<=2.0))
        estado=PREDOCKING;
        
      
      // si llega al destino -> pasa a buscar el siguiente destino
      if( fabs(x)<= 0.4 && fabs(y)<= 0.1){
        i++;
        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=2.0;
        Kdifh=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.01){
            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){
            i++;
            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;

  } 



  
  // 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;
  
  robot_console_printf("%f\t", ASpeed[0]);
  robot_console_printf("%f\t", ASpeed[1]);
  robot_console_printf("%f\t", ASpeed[2]);
  
  // 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;

}



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

