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

  

  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

  

  This program is free software; any publications presenting results

  obtained with this program must mention it and its origin. 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 2 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, write to the Free Software

  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.



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



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



  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 <math.h>



#include "js.h"		// joystick



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



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

}



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



	// Joystick

	jsJoystick *joystick;

	joystick = new jsJoystick(0);

	float mForwardAxis, mVerticalAxis, mYawAxis;



	float jsAxes[6];

	int jsButtons[6];



	mForwardAxis = mVerticalAxis = mYawAxis = 0.0f;



	if(joystick->notWorking()) {

		jsAxes[0]=0;

		jsAxes[1]=0;

		jsAxes[2]=0;

		jsAxes[3]=0;

		printf("Joystick not found.\n");

	} else {

		printf("Joystick working OK.\n");

	}

	

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



		// Get joystick values

		joystick->read(jsButtons, jsAxes);



		// Handling of platform dependencies:

		// Forward:	 jsAxes[1]

		// Yaw:			 jsAxes[2] under Linux, [3] under Windows

		// Vertical: jsAxes[3] under Linux, [2] under Windows

		mForwardAxis = - jsAxes[1];

		#ifdef WIN32

			mYawAxis = jsAxes[3];

			mVerticalAxis = jsAxes[2];

		#else

			mYawAxis = jsAxes[2];

			mVerticalAxis = jsAxes[3];

		#endif



		// Get the camera image

		camera_get_image(camera);



		// Get the position and orientation of the Blimp

		PosMatrix = gps_get_matrix(gps);



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

}



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

