/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *    frankguettler@gmx.de                                                 *
 *                                                                         *
 *   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 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.             *
 *                                                                         *
 *   $Log: camera.cpp,v $
 *   Revision 1.15  2006/07/14 12:23:55  martius
 *   selforg becomes HEAD
 *
 *   Revision 1.14.4.2  2005/12/06 10:13:26  martius
 *   openscenegraph integration started
 *
 *   Revision 1.14.4.1  2005/11/14 17:37:24  martius
 *   moved to selforg
 *
 *   Revision 1.14  2005/11/14 12:47:36  martius
 *   removed printfs
 *
 *   Revision 1.13  2005/09/27 14:00:29  robot3
 *   printf removed
 *
 *   Revision 1.12  2005/09/27 12:22:52  robot3
 *   now changing camView by mouse movement really works :)
 *
 *   Revision 1.11  2005/09/27 12:11:16  robot3
 *   fixed bug not be able to change the camView through mouse movement
 *
 *   Revision 1.10  2005/09/27 10:49:20  robot3
 *   camera module rewritten, new features:
 *   -everytime there is a smooth move and view
 *   -some hacks are replaced through stable code
 *
 *   Revision 1.9  2005/09/20 10:55:14  robot3
 *   camera module:
 *   -pressing key c now centers on focused robot
 *   -pressing key b now moves 5.0f behind the robot
 *   -fixed a few bugs (nullpointer crashes etc.)
 *
 *   Revision 1.8  2005/09/02 17:20:18  martius
 *   advancedTV disabled
 *
 *   Revision 1.7  2005/08/23 11:41:20  robot1
 *   advancedFollowing mode included
 *
 *   Revision 1.6  2005/08/22 12:38:32  robot1
 *   -advancedTV mode implemented, early version
 *   -internal code optimized
 *   -printMode prints now the current camera mode on stdout
 *
 *   Revision 1.5  2005/08/16 10:06:48  robot1
 *   TV mode with horizontal centering implemented.
 *
 *   Revision 1.4  2005/08/12 11:56:46  robot1
 *   tiny bugfixing
 *
 *   Revision 1.3  2005/08/09 11:08:49  robot1
 *   following mode included
 *
 *
 *   Revision 1.1  2005/08/08 11:06:46  martius
 *   camera is a module for camera movements
 *   includes cleaned
 *
 *                                                                         *
 *                                                                         *
 ***************************************************************************/
 
#include "camera.h" 
#include <stdio.h>

namespace lpzrobots {

// #include <drawstuff/drawstuff.h>
// #include "ode/ode.h" 

// CameraType oldCamType;
// const OdeRobot* oldRobot;

// // all desired values that have to be in the end state
// float desiredCamPos[3];
// float desiredCamView[3];

// // for camera and robot: current (read and calculated) values
// float currentCamPos[3];
// float currentCamView[3];
// double currentRobotPos[3];
// double currentRobotView[3];

// // for camera and robot: old values from one timestep before
// float oldCamPos[3];
// float oldCamView[3];
// double oldRobotPos[3];
// double oldRobotView[3];


// // for advanced TV mode
// float maxAllowedDistance=10.0f; // enabled advanvedTV ;)
// float robCamDistance;



// // this is a bit faster than the compiler definition:
// // #define SQUARE(x) (x)*(x)
// // x is only calculated once, not twice
// double SQUARE(double x) {
// 	return (x*x);

// }

// // I didn't found the math library so fast, sorry.
// float ABS(float x) {
//   if (x<0)
//     return (x*(-1));
//   else
//     return x;
// }

// void followRobotsMove() {
// 	// now adjusting the desired position of the camera
// 	for (int i=0;i<=2;i++) {
// 		desiredCamPos[i]+=currentRobotPos[i]-oldRobotPos[i];
// 	}
// }

// // changes only the desiredCamView
// void centerViewOnRobot() {
// 	// no change for z
// 	if ((currentRobotPos[1]-desiredCamPos[1])!=0) { // division by zero
// 		// calculate the horizontal angle
// 		desiredCamView[0]=-atan((currentRobotPos[0]-desiredCamPos[0])/
// 					(currentRobotPos[1]-desiredCamPos[1]))
// 							/M_PI*180.0f+270.0f;
// 		if (desiredCamPos[1]-currentRobotPos[1]<0)
// 			desiredCamView[0]+=180.0f; // we must switch
// 	}
// 	if ((currentRobotPos[2]-desiredCamPos[2])!=0) { // division by zero
// 		// calculate the vertical angle
// 		// we need dz and sqrt(dx^2+dy^2) for calculation
// 		desiredCamView[1]=
// 			atan(
// 				(sqrt(
// 				SQUARE(desiredCamPos[0]-currentRobotPos[0]) +
// 				SQUARE(desiredCamPos[1]-currentRobotPos[1])))
// 					/(currentCamPos[2]-currentRobotPos[2]))
// 					/M_PI*180.0f+270.0f; // TODO: normalize
// 	}
// 	// z-angle (rotation) is not adjusted
// }



// void getRobotPosAndView(double *pos, double *view,OdeRobot& robot) {
// 	Position position=robot.getPosition();
// 	pos[0]=position.x;
// 	pos[1]=position.y;
// 	pos[2]=position.z;
// 	// now the view
// 	for (int i=0;i<=2;i++) {
// 		view[i]=pos[i]-oldRobotPos[i];
//         }
// 	// if no view can be detected, use old view!
// 	if (view[0]==0 && view[1]==0) {
// 		for (int i=0;i<=2;i++) {
// 			view[i]=oldRobotView[i];
//         	}
// 	}
// }

// // prints the current (used) mode on stdout
// void printMode(CameraType camType) {
// 	char* type;
// 	printf("Current Mode is now: ");
// 	switch (camType) {
// 		case Static:
// 			type="Static";
// 			break;
// 		case TV:
// 			type="TV";
// 			break;
// 		case advancedTV:
// 			type="advancedTV";
// 			break;
// 		case Following:
// 			type="Following";
// 			break;
// 		case advancedFollowing:
// 			type="advancedFollowing";
// 			break;
// 		default:
// 			type="unknown";
// 	}
// 	printf("%s\n",type);
// }

// // has to be called if CameraType or the Robot has changed.
// void initCamera(CameraType camType,OdeRobot& robot) {
// 	// getting first the position and view of robot
// 	getRobotPosAndView(oldRobotPos, oldRobotView, robot);
// 	// now getting the current angle of the camera
// 	dsGetViewpoint(oldCamPos,oldCamView);
// 	if (oldCamType!=camType)
// 		printMode(camType);
// 	if (oldRobot!=&robot) // Robot is changed
// 		// setting current cam values to desired values
// 		for (int i=0;i<=2;i++) {
// 			desiredCamPos[i]=oldCamPos[i];
// 			desiredCamView[i]=oldCamView[i];
// 		}
// 	// setting the new Values
// 	oldCamType=camType;
// 	oldRobot=&robot;
// }

// void StaticMode(OdeRobot& robot) {
// 	// do nothing
// }

// void TVMode(OdeRobot& robot) {
// 	centerViewOnRobot();
// }

// void advancedTVMode(OdeRobot& robot) {
// 	// check wether the camera is too far away
// 	robCamDistance= (sqrt(
// 			SQUARE(currentCamPos[0]-currentRobotPos[0]) +
// 			SQUARE(currentCamPos[1]-currentRobotPos[1])));
// 	if (robCamDistance>maxAllowedDistance) {
// 		// then the camera must get a new position, centering on position of robot
// 		desiredCamPos[0]=currentRobotPos[0];
// 		desiredCamPos[1]=currentRobotPos[1];
// 		desiredCamPos[2]=currentRobotPos[2]+2.0f; // must be higher than robPos...
// 		//		printf("I am too far away from maximum distance(%f): %f\n",maxAllowedDistance,robCamDistance);
// 	}
// 	// now do the rest of TV
// 	centerViewOnRobot();
// }

// void FollowingMode(OdeRobot& robot) {
// 	followRobotsMove();
// }

// void print3DimFloat(float vec[3]) {
// 	for (int i=0;i<=2;i++) {
// 		printf("\t%f",vec[i]);
// 	}
// }

// void moveOnRobot(OdeRobot& robot) {
//   //printf("moveOnRobot is now called!\n");
//   // now getting the current angle of the camera
// 	for (int i=0;i<=1;i++) {
// 		desiredCamPos[i]=currentRobotPos[i];
// 	}
// 	desiredCamPos[2]=currentRobotPos[2]+2.0f;
// 	// center the view on the robot
// 	centerViewOnRobot();
// }

// void moveBehindRobot(OdeRobot& robot) {
//   //printf("moveBehindRobot is now called!\n");
// 	robCamDistance=4.0f; // the distance of camera position
// 	// now get normalizing scalar of currentRobotView, only x and y is needed
// 	float n=sqrt(SQUARE(currentRobotView[0])+SQUARE(currentRobotView[1]));
// 	if (n>0) {
// 		for (int i=0;i<=1;i++)
// 		  desiredCamPos[i]=currentRobotPos[i]-currentRobotView[i]/n*robCamDistance;
// 		centerViewOnRobot();
// 	} else { // we think this is the best
//  		moveOnRobot(robot);
// 	}
// }


// void advancedFollowingMode(OdeRobot& robot) {
// 	float n;
// 	// first calculate current distance and set the desired position behind the robot
// 	robCamDistance= (sqrt(
// 			SQUARE(currentCamPos[0]-currentRobotPos[0]) +
// 			SQUARE(currentCamPos[1]-currentRobotPos[1])));
// 	// the direction of the robot is stored in currentRobotView!
// 	// now get normalizing scalar of currentRobotView, only x and y is needed
// 	n=sqrt(SQUARE(currentRobotView[0])+SQUARE(currentRobotView[1]));
// 	if (n>0) { // else no calculation can be made, then do not change the camPos
// 		for (int i=0;i<=1;i++) {
// 			// now set the camPos to robotPos-robCamDistance*robotView
// 			// this is an approximation, normally a rotation is needed
// 			// store it in newCamPos, so TV mode can be called unmodified
// 			desiredCamPos[i]=currentRobotPos[i]
// 			  -currentRobotView[i]*robCamDistance/n+currentRobotView[i];
// 		}
// 	}
// 	// now do centering the view on robot
// 	centerViewOnRobot();
// }


// void adjustPosByMouseMove() {
// 	for (int i=0;i<=2;i++) {
// 	  desiredCamPos[i]+=currentCamPos[i]-oldCamPos[i];
// 	  //	  if (oldCamPos[i]-currentCamPos[i]>0) 
// 	  // desiredCamPos[i]=currentCamPos[i];
// 	}
// }

// void adjustViewByMouseMove() {
// 	for (int i=0;i<=2;i++) {
// 		desiredCamView[i]-=oldCamView[i]-currentCamView[i];
// 	}
// }

// void storeOldValues(float *setCamPos, float *setCamView) {
// 	for (int i=0;i<=2;i++) {
// 		oldRobotPos[i]=currentRobotPos[i];
// 		oldRobotView[i]=currentRobotView[i];
// 		oldCamPos[i]=setCamPos[i];
// 		oldCamView[i]=setCamView[i];
// 	}
// }


// // wrapper function
// void doubleTofloatA(const double *doubleArray,float *floatArray) {
//   for (int i=0;i<=2;i++) {
//     floatArray[i]=doubleArray[i];
//   }
// }

// // wrapper function
// void floatTodoubleA(const float *floatArray,double *doubleArray) {
//   for (int i=0;i<=2;i++) {
//     doubleArray[i]=floatArray[i];
//   }
// }


// Position smoothMove() {
// 	double newPos[3];
// 	for (int i=0;i<=2;i++) {
// 	  if (ABS(desiredCamPos[i]-currentCamPos[i])<0.1f) {
// 	    newPos[i]=desiredCamPos[i];
// 	  } else
// 	    newPos[i]=desiredCamPos[i]*0.1f+currentCamPos[i]*0.9f;
// 	}
// 	return Position(newPos);
// }

// // normalizes the angle
// float normalize360(float angle) {
//   float result=angle;
//     while (result>=360.0f)
//       result-=360.0f;
//     while (result<0.0f)
//       result+=360.0f;
//     return result;
// }

// // normalizes the angle
// double normalize360(double angle) {
//   double result=angle;
//     while (result>=360.0f)
//       result-=360.0f;
//     while (result<0.0f)
//       result+=360.0f;
//     return result;
// }

// // normalizes the angle of all 3 components
// void normalize360vec3(float *angle) {
//   for (int i=0;i<=2;i++) {
//     angle[i]=normalize360(angle[i]);
//   }
// }

// // normalizes the angle of all 3 components
// void normalize360vec3(double *angle) {
//   for (int i=0;i<=2;i++) {
//     angle[i]=normalize360(angle[i]);
//   }
// }


// Position smoothView() {
// 	double newView[3];
// 	normalize360vec3(desiredCamView);
// 	normalize360vec3(currentCamView);
// 	for (int i=0;i<=2;i++) {
// 	  if (ABS(desiredCamView[i]-currentCamView[i])<0.2f) {
// 	    newView[i]=desiredCamView[i];
// 	  } else if (ABS(desiredCamView[i]-currentCamView[i])>180.0f) {
// 	    // we must first transform the angles into linear gradient space (+180 degrees)
// 	    // then make a backtransformation (-180 degrees)
// 	    newView[i]=normalize360(desiredCamView[i]+180.0f)*0.1f
// 	      +normalize360(currentCamView[i]+180.0f)*0.9f-180.0f;
// 	  normalize360(newView[i]);
// 	  } else { // use normal sliding
// 	    newView[i]=desiredCamView[i]*0.1f+currentCamView[i]*0.9f;
// 	  }
// 	}
// 	return Position(newView);
// }


// void moveCamera( CameraType camType,OdeRobot& robot) {
// 	// first look if someone is changed
// 	// only otherwise change the camera position and/or view.
// 	if (oldCamType!=camType || &robot!=oldRobot)
// 		initCamera(camType,robot);
// 	else {
// 		// first get all needed values
// 		// getting first the position and view of robot
// 		getRobotPosAndView(currentRobotPos,currentRobotView, robot);
// 		// now getting the current position and angle of the camera
// 		dsGetViewpoint(currentCamPos,currentCamView);
// 		// now compute
// 		// calculate the adjusted new settings from mouse movement
// 		adjustPosByMouseMove();
// 		adjustViewByMouseMove();
// 		// now adjust the desired values through the camera modes
// 		switch (camType) {
// 			case Static:
// 				StaticMode(robot);
// 				break;
// 			case advancedTV:
// 				advancedTVMode(robot);
// 				break;
// 			case TV:
// 				TVMode(robot);
// 				break;
// 			case Following:
// 				FollowingMode(robot);
// 				break;
// 			case advancedFollowing:
// 				advancedFollowingMode(robot);
// 				break;
// 		}
// 		// now execute :)
// 		float move[3];
// 		float view[3];
// 		doubleTofloatA(smoothMove().toArray(),move);
// 		doubleTofloatA(smoothView().toArray(),view);
// 		dsSetViewpoint(move,view);
// 		// now store the current (set) values to old values
// 		storeOldValues(move,view);
// 	}
// }

}
