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

  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>


  //errores
  float dx=0.0;
  float dy=0.0;
  float dz=0.0;
  
  //errores previos
  float Pdx=0.0;
  float Pdy=0.0;
  float Pdz=0.0;
  
  //PID Y
  float Kpro=4.0;
  float Kdif=200.0;
  //float Kint=0.0;
  //float ISum=0.0;
  
  
  //PID Z
  float Kpro2=1.0;
  float Kdif2=100.0;
  //float Kint2=0.0;
  //float ISum2=0.0;
  
  
 
  


// Matrix and scalar products

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



// Cycle time

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



// Sensors

DeviceTag camera;								// Embedded camera

DeviceTag gps;									// GPS

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



// Constants for dynamic model

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

const float m = 0.25;						// Blimp mass in [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) {

	camera = robot_get_device("camera");

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

	gps = robot_get_device("gps");

	if (gps==0) printf("GPS is missing!\n");
  
  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; 



	float FXWR=0, FYWR=0, FZWR=0;

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



	robot_live(reset);



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



	camera_enable(camera,(short unsigned int)(Dt*1000));
  
  
  
  
  float mForwardAxis, mVerticalAxis, mYawAxis;
	
	mForwardAxis = mVerticalAxis = mYawAxis = 0.0f;
  





	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);


  
  
	// Get the camera image

	camera_get_image(camera);



	// Get the position and orientation of the Blimp
    
    float Pos[3];
    float Ppos[3];
    float Euler[3];
    
    
		PosMatrix = gps_get_matrix(gps);
    
    Ppos[0] = 0.0;
    Ppos[1] = 0.0;
    Ppos[2] = 0.0;
    
    
    Ppos[1]=Pos[1];
  
    Pos[0] = gps_position_x(PosMatrix); //X
    Pos[1] = gps_position_y(PosMatrix); //Y
    Pos[2] = gps_position_z(PosMatrix); //Z
    
    
    
    gps_euler(PosMatrix, Euler);      //Angulos de euler





  //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.0;
  }
    
    
    
  
  //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
  float Dest[3];
  
  Dest[0]=0.8;
  Dest[1]=1.8;
  Dest[2]=-3.5;


 

  //Aplicar un controlador PD para cada eje
  
  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

  mForwardAxis = -mForwardAxis;	

 
  
 
 
  // Maniobra de emergencia
  
  if(ds0_value >= 800 || ds1_value >= 800){
  
    robot_console_printf("WARNING!!\tWARNING!! Altura demasiado baja!!\n");
    
  }


  if(ds0_value >= 900 || ds1_value >= 900){
  
    mVerticalAxis = -1.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;



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

}



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

