/*
QtWagon: a project about 3D objects.
Science and technology promotion license applied. Third party license automatically cascaded.
Zhikai Wang/ www.heteroclinic.net 2013
You can do anything with this file or any file(s) published as part QtWagon project, given this header is kept.
*/
#include "camera.h"

// This function does not change the orientation
// The focus move together with the global position.
void camera::panLeftRight(float delta) {

	mathPoint<float> o = position;
	mathVector<float> v = swirludvector;
	v.normalize();
	mathPoint<float> n = mathPoint<float>( o.getx() + v.getx() * delta,
		o.gety() + v.gety() * delta,o.getz() + v.getz() * delta);
	position = n;
	updateLookat();
};
// This function does not change the orientation
// The focus move together with the global position.
void camera::panUpDown(float delta){
	mathPoint<float> o = position;
	mathVector<float> v = mathVector<float>(0.0f,1.0f,0.0f);
	v.normalize();
	mathPoint<float> n = mathPoint<float>( o.getx() + v.getx() * delta,
		o.gety() + v.gety() * delta,o.getz() + v.getz() * delta);
	position = n;
	updateLookat();
};
// This function does not change the orientation
// The focus move together with the global position.
void camera::panLevelForwardBackward(float delta ){
	mathPoint<float> o = position;
	mathVector<float> f = mathVector<float>(front.getx(),0.0f,front.getz());
	f.normalize();
	mathVector<float> v = f;
	v.normalize();
	mathPoint<float> n = mathPoint<float>( o.getx() + v.getx() * delta,
		o.gety() + v.gety() * delta,o.getz() + v.getz() * delta);
	position = n;
	updateLookat();
};

void camera::setSwirlVectors(mathVector<float> r) {
	swirludvector = r;
}

void camera::initSwirlVectors() {
	mathVector<float> tfrontv 
		= (*this).getFrontVectorRef();
	//std::cout<<"tfrontv "<<tfrontv<<std::endl;
	mathVector<float> nfrontv 
		( -1.0f *( tfrontv.getx()),
		-1.0f * (tfrontv.gety()),
		-1.0f * (tfrontv.getz()) );
	//std::cout<<"nfrontv "<<nfrontv<<std::endl;
	swirludvector = cross<float >( nfrontv,
		mathVector<GLfloat>(0,1,0));
	//std::cout<<"swirludvector "<<swirludvector<<std::endl;
	//r = glOrientation<float>::rotateAboutAVector( tmpv  ,w ,current_camera->default_deta_swirl);
	//w.normalize ( );
	if (swirludvector.firstnorm () < ZERO_VECTOR_NORMBOUD) {
		swirludvector = (*this).getLeftVectorRef();
	}



	//// keep this, some important test result.
	//// How angle affects the norm of the result of the cross-product of two vectors.
	//float angles = 1.0f;
	//mathVector<float> r1( 1.0f,1.0f,1.0f);
	//r1.normalize ( );
	//mathVector<float> r2 = glOrientation<float>::rotateAboutAVector( mathVector<GLfloat>(0,1,0) ,r1 ,angles);
	//mathVector<float> r3 =  cross<float >(r1,r2);
	//float d =  dot<float >(r1,r2);
	//std::cout<<"Angles between r1 and r2 (degrees) "<< angles<<std::endl;
	//std::cout<<"r1: "<<r1;
	//std::cout<<"r2: "<<r2;
	//std::cout<<"r3: "<<r3;
	//std::cout<<"r3.norm( ): "<<r3.norm ( )<<std::endl;
	//std::cout<<"r3.firstnorm (): "<<r3.firstnorm ()<<std::endl;
	//   std::cout<<" dot<float >(r1,r2): "<<d <<std::endl;
	//std::cout<<std::endl;


	//		angles =0.01f;
	//r2 = glOrientation<float>::rotateAboutAVector( mathVector<GLfloat>(0,1,0) ,r1 ,angles);
	//r3 =  cross<float >(r1,r2);
	//d =  dot<float >(r1,r2);
	//std::cout<<"Angles between r1 and r2 (degrees) "<< angles<<std::endl;
	//std::cout<<"r1: "<<r1;
	//std::cout<<"r2: "<<r2;
	//std::cout<<"r3: "<<r3;
	//std::cout<<"r3.norm( ): "<<r3.norm ( )<<std::endl;
	//std::cout<<"r3.firstnorm (): "<<r3.firstnorm ()<<std::endl;
	//   std::cout<<" dot<float >(r1,r2): "<<d <<std::endl;
	//std::cout<<std::endl;

	//angles =0.1f;
	//r2 = glOrientation<float>::rotateAboutAVector( mathVector<GLfloat>(0,1,0) ,r1 ,angles);
	//r3 =  cross<float >(r1,r2);
	//d =  dot<float >(r1,r2);
	//std::cout<<"Angles between r1 and r2 (degrees) "<< angles<<std::endl;
	//std::cout<<"r1: "<<r1;
	//std::cout<<"r2: "<<r2;
	//std::cout<<"r3: "<<r3;
	//std::cout<<"r3.norm( ): "<<r3.norm ( )<<std::endl;
	//std::cout<<"r3.firstnorm (): "<<r3.firstnorm ()<<std::endl;
	//   std::cout<<" dot<float >(r1,r2): "<<d <<std::endl;
	//std::cout<<std::endl;

	//angles =0.5f;
	//r2 = glOrientation<float>::rotateAboutAVector( mathVector<GLfloat>(0,1,0) ,r1 ,angles);
	//r3 =  cross<float >(r1,r2);
	//d =  dot<float >(r1,r2);
	//std::cout<<"Angles between r1 and r2 (degrees) "<< angles<<std::endl;
	//std::cout<<"r1: "<<r1;
	//std::cout<<"r2: "<<r2;
	//std::cout<<"r3: "<<r3;
	//std::cout<<"r3.norm( ): "<<r3.norm ( )<<std::endl;
	//std::cout<<"r3.firstnorm (): "<<r3.firstnorm ()<<std::endl;
	//   std::cout<<" dot<float >(r1,r2): "<<d <<std::endl;
	//std::cout<<std::endl;




	//   angles = 2.0f;
	//r2 = glOrientation<float>::rotateAboutAVector( mathVector<GLfloat>(0,1,0) ,r1 ,angles);
	//r3 =  cross<float >(r1,r2);
	//d =  dot<float >(r1,r2);
	//std::cout<<"Angles between r1 and r2 (degrees) "<< angles<<std::endl;
	//std::cout<<"r1: "<<r1;
	//std::cout<<"r2: "<<r2;
	//std::cout<<"r3: "<<r3;
	//std::cout<<"r3.norm( ): "<<r3.norm ( )<<std::endl;
	//std::cout<<"r3.firstnorm (): "<<r3.firstnorm ()<<std::endl;
	//   std::cout<<" dot<float >(r1,r2): "<<d <<std::endl;
	//std::cout<<std::endl;

	//   angles = 10.0f;
	//r2 = glOrientation<float>::rotateAboutAVector( mathVector<GLfloat>(0,1,0) ,r1 ,angles);
	//r3 =  cross<float >(r1,r2);
	//d =  dot<float >(r1,r2);
	//std::cout<<"Angles between r1 and r2 (degrees) "<< angles<<std::endl;
	//std::cout<<"r1: "<<r1;
	//std::cout<<"r2: "<<r2;
	//std::cout<<"r3: "<<r3;
	//std::cout<<"r3.norm( ): "<<r3.norm ( )<<std::endl;
	//std::cout<<"r3.firstnorm (): "<<r3.firstnorm ()<<std::endl;
	//   std::cout<<" dot<float >(r1,r2): "<<d <<std::endl;
	//std::cout<<std::endl;

	//angles = 20.0f;
	//r2 = glOrientation<float>::rotateAboutAVector( mathVector<GLfloat>(0,1,0) ,r1 ,angles);
	//r3 =  cross<float >(r1,r2);
	//d =  dot<float >(r1,r2);
	//std::cout<<"Angles between r1 and r2 (degrees) "<< angles<<std::endl;
	//std::cout<<"r1: "<<r1;
	//std::cout<<"r2: "<<r2;
	//std::cout<<"r3: "<<r3;
	//std::cout<<"r3.norm( ): "<<r3.norm ( )<<std::endl;
	//std::cout<<"r3.firstnorm (): "<<r3.firstnorm ()<<std::endl;
	//   std::cout<<" dot<float >(r1,r2): "<<d <<std::endl;
	//std::cout<<std::endl;

	//angles = 45.0f;
	//r2 = glOrientation<float>::rotateAboutAVector( mathVector<GLfloat>(0,1,0) ,r1 ,angles);
	//r3 =  cross<float >(r1,r2);
	//d =  dot<float >(r1,r2);
	//std::cout<<"Angles between r1 and r2 (degrees) "<< angles<<std::endl;
	//std::cout<<"r1: "<<r1;
	//std::cout<<"r2: "<<r2;
	//std::cout<<"r3: "<<r3;
	//std::cout<<"r3.norm( ): "<<r3.norm ( )<<std::endl;
	//std::cout<<"r3.firstnorm (): "<<r3.firstnorm ()<<std::endl;
	//   std::cout<<" dot<float >(r1,r2): "<<d <<std::endl;
	//std::cout<<std::endl;

};

const mathVector<float> camera::getSwirludvector() {
	return swirludvector;

};
void camera::setLookedAt (mathPoint<float> nmpt) {
	LookedAt = nmpt;
	lookat_radius  = (position-LookedAt).norm();
	//std::cout<<"position 1 "<<position<<std::endl;
	//std::cout<<"LookedAt 1 "<<LookedAt<<std::endl;
	yawThenRollToPinAPoint(LookedAt);
	lastorientation = *this;
	//std::cout<<"position 2 "<<position<<std::endl;
	//std::cout<<"LookedAt 2 "<<LookedAt<<std::endl;
	initSwirlVectors();

}
void camera::drawFocus () {
	if (showFocus)  {
		drawVectorAxis<float>(focusSize,LookedAt,
			mathVector<float>((float)0.0,(float)1.0,(float)1.0),
			mathVector<float>((float)0.0,(float)1.0,(float)1.0),
			mathVector<float>((float)0.0,(float)1.0,(float)1.0)
			);
	}
}
const mathPoint<float> camera::getLookedAt() {
	return mathPoint<float>(LookedAt);
}
camera::camera( const mathPoint<float> & nglobalPosition )
	:glMovingObjectf(nglobalPosition)
{
	initSp();
	initSo();

	safe_lookat_radius = (float)1.0;
	LookedAt = mathPoint<float>((float)0.0,(float)0.0,(float)0.0);
	position = mathPoint<float>((float)5.0,(float)5.0,(float)5.0);

	lookat_radius  = (position-LookedAt).norm();
	//rollThenYawToPinAPoint(LookedAt); // you cannot do it here, it uses gl_functions call a function inside gl
	orthorgonal_disp = false;
	followed_obj = NULL;
	//Point tmp ;
	//tmp.x=0,tmp.y=0,tmp.z=0;
	//freePlaceMent(tmp);
	free_camera = true; 
	default_deta_fb = (float)0.2;
	default_deta_fovy = (float)0.5;
	default_deta_osize= (float)0.5;
	default_deta_swirl= (float)1.0;
	default_deta_pan= (float)0.2;

	//freePlaceMent(newPlaceMent);
	//position = mathPoint<float>(5.0f,5.0f,5.0f);

	//mathVector<float> tfrontv 
	//	= (*this).getFrontVectorRef();
	//mathVector<float> nfrontv 
	//	( -1.0f *( tfrontv.getx()),
	//	-1.0f * (tfrontv.gety()),
	//	-1.0f * (tfrontv.getz()) );
	//swirludvector = cross<float >( nfrontv,
	//	mathVector<GLfloat>(0,1,0));
	initSwirlVectors();
	showFocus = true;
	focusSize = (float)0.5;

	//swirludvector;

	lastorientation = *this;

};
void camera::initSp(){
	spStructPserpective.far_z =  (float)200;
	spStructPserpective.fovy  =  (float)90;
	spStructPserpective.fovy_min = (float)5;
	spStructPserpective.fovy_max = (float)170;
	spStructPserpective.near_z = (float)0.5;// can be the safe radius
};
void camera::initSo(){
	soStructOrthorgonal.far_z= (float)200.0;
	soStructOrthorgonal.near_z = (float)0.5;
	soStructOrthorgonal.o_size1= (float)10.0;
	soStructOrthorgonal.o_size2= (float)10.0;
	soStructOrthorgonal.o_size3= (float)10.0;
	soStructOrthorgonal.o_size4= (float)10.0;
	soStructOrthorgonal.o_size_limit= (float)20.0;//>= o_size!
};


camera::~camera() {
};


void camera::zoomIn() {
	/// forward /backward camera no use in O-view
	//		default_deta_fovy = 0.5;
	//default_deta_osize= 0.5;
	//if (orthorgonal_disp) {
		if (soStructOrthorgonal.o_size1-default_deta_osize>=
			soStructOrthorgonal.o_size_limit )
			soStructOrthorgonal.o_size1 -= default_deta_osize;
	//}
	//else {
		if (spStructPserpective.fovy-default_deta_fovy>=
			spStructPserpective.fovy_min)
			spStructPserpective.fovy-=default_deta_fovy;
	//}
}
//
void camera::zoomOut() {
	//if (orthorgonal_disp) {
		soStructOrthorgonal.o_size1 += default_deta_osize;
	//}
	//else {

		if (spStructPserpective.fovy+default_deta_fovy<=
			spStructPserpective.fovy_max)
			spStructPserpective.fovy+=default_deta_fovy;
	//}
}
//
void camera::increase_lookat_radius() {
	fb_lookat_radius(default_deta_fb );
}
void camera::decrease_lookat_radius() {
	fb_lookat_radius(-default_deta_fb );
}
void camera::fb_lookat_radius(float deta_fb) {
	/// forward /backward camera no use in Orthorgonalview
	//if (orthorgonal_disp)
	//return;
	lookat_radius -= deta_fb;
	if (lookat_radius < safe_lookat_radius)
		lookat_radius += deta_fb;
	else
		//ForwardMovement(deta_fb);
		forward(deta_fb);
	lastorientation = *this;
}
void camera::forward(float deta_fb){
	//std::cout<<"camera::forward(float deta_fb) "<< deta_fb<<std::endl;
	glMovingObjectf::forward(deta_fb);
	updateLookat();
	lastorientation = *this;
};
void camera::updateLookat() {
	//LookedAt.x = Captain.x + -normalVCaptain.x *  lookat_radius,
	//	LookedAt.y=Captain.y + -normalVCaptain.y *  lookat_radius,
	//	LookedAt.z=Captain.z + -normalVCaptain.z *  lookat_radius;
	LookedAt = mathPoint<float> ( position.getx() + (front*lookat_radius).getx(),
		position.gety() + (front*lookat_radius).gety(),
		position.getz() + (front*lookat_radius).getz()
		);
};


void camera::swirl(float angles,mathVector<float> tmpY ){
	//forward(lookat_radius);
	//glMovingObjectf::pitch(-angles);
	//	lastorientation = *this;
	//	forward(-lookat_radius);

	//std::cout<<"==== swirl ===="<<std::endl;
	forward(lookat_radius);
	//std::cout<<*this<<std::endl;
	//std::cout<<swirludvector<<std::endl;
	//rotateAboutMatrix( const mathVector<T> & v, T theta) 
	glMovingObjectf::rotateAboutMatrix( tmpY,angles);
	//std::cout<<*this<<std::endl;
	//std::cout<<swirludvector<<std::endl;
	forward(-lookat_radius);
	lastorientation = *this;
	//std::cout<<*this<<std::endl;
	//std::cout<<swirludvector<<std::endl;
	//std::cout<<"==== swirl ===="<<std::endl;
}
void camera::backward(float deta_fb){
	glMovingObjectf::backward(deta_fb);
	updateLookat();
	lastorientation = *this;
};
void camera::rotateAboutMatrix( const mathVector<float> & v, float theta)  {
	glMovingObjectf::rotateAboutMatrix(v,theta);
	updateLookat();
	lastorientation = *this;

}
void camera::RollYawPitch(unsigned char lastElement,float ROTATION_ANGLE) {
	glMovingObjectf::RollYawPitch( lastElement,ROTATION_ANGLE);
	updateLookat();
	lastorientation = *this;

}
void camera::roll(float degree){
	std::cout<<"camera::roll(float degree)"<<std::endl;
	glMovingObjectf::roll( degree);
	updateLookat();
	lastorientation = *this;
};
void camera::yaw(float degree) {
	std::cout<<"camera::yaw(float degree)"<<std::endl;
	glMovingObjectf::yaw( degree);
	updateLookat();
	lastorientation = *this;
};
void camera::pitch(float degree ) {
	std::cout<<"camera::pitch(float degree)"<<std::endl;
	glMovingObjectf::pitch( degree);
	updateLookat();
	lastorientation = *this;
};
void camera::setPosition(const mathPoint<float> &npos )  {
	glMovingObjectf::setPosition(npos);
	lookat_radius = (position - LookedAt).norm();
	yawThenRollToPinAPoint(LookedAt);
	lastorientation = *this;
}
const glOrientation<float> camera::getLastorientation() {
	return lastorientation;
};