/*
 * pbrt source code Copyright(c) 1998-2005 Matt Pharr and Greg Humphreys
 *
 * All Rights Reserved.
 * For educational use only; commercial use expressly forbidden.
 * NO WARRANTY, express or implied, for this software.
 * (See file License.txt for complete license)
 */

// blurred.cpp*
#include "camera.h"
#include "film.h"
#include "paramset.h"

#include <iostream>

#include "Vec3.cpp"


void orthoNormalize(float mat[4][4])
{
  float len, temp[3][3];

  temp[0][0] = mat[0][0];
  temp[0][1] = mat[0][1];
  temp[0][2] = mat[0][2];
  temp[1][0] = mat[1][0];
  temp[1][1] = mat[1][1];
  temp[1][2] = mat[1][2];

  /* normalize x */
  len = sqrtf (temp[0][0]*temp[0][0] + temp[0][1]*temp[0][1] + temp[0][2]*temp[0][2]);
  len = (len == 0.0f) ? 1.0f : 1.0f/len;
  temp[0][0] *= len; temp[0][1] *= len; temp[0][2] *= len;

  /* z = x cross y */
  temp[2][0] = temp[0][1]*temp[1][2] - temp[0][2]*temp[1][1];
  temp[2][1] = temp[0][2]*temp[1][0] - temp[0][0]*temp[1][2];
  temp[2][2] = temp[0][0]*temp[1][1] - temp[0][1]*temp[1][0];

  /* normalize z */
  len = sqrtf (temp[2][0]*temp[2][0] + temp[2][1]*temp[2][1] + temp[2][2]*temp[2][2]);
  len = (len == 0.0f) ? 1.0f : 1.0f/len;
  temp[2][0] *= len; temp[2][1] *= len; temp[2][2] *= len;

  /* y = z cross x */
  temp[1][0] = temp[2][1]*temp[0][2] - temp[2][2]*temp[0][1];
  temp[1][1] = temp[2][2]*temp[0][0] - temp[2][0]*temp[0][2];
  temp[1][2] = temp[2][0]*temp[0][1] - temp[2][1]*temp[0][0];

  /* normalize y */
  len = sqrtf (temp[1][0]*temp[1][0] + temp[1][1]*temp[1][1] + temp[1][2]*temp[1][2]);
  len = (len == 0.0f) ? 1.0f : 1.0f/len;
  temp[1][0] *= len; temp[1][1] *= len; temp[1][2] *= len;

  /* update matrix4 */
  mat[0][0] = temp[0][0]; mat[0][1] = temp[0][1]; mat[0][2] = temp[0][2];
  mat[1][0] = temp[1][0]; mat[1][1] = temp[1][1]; mat[1][2] = temp[1][2];
  mat[2][0] = temp[2][0]; mat[2][1] = temp[2][1]; mat[2][2] = temp[2][2];

}


class Quaternion {

public:
	float real;

	Vec3 img;

	// generate from 4x4 matrix
	Quaternion(const float* m);
	Quaternion();
	Quaternion(const Quaternion &q);


	friend Quaternion operator +( const Quaternion& q1, const Quaternion& q2 ) {
		Quaternion q;
		q.img = q1.img + q2.img;
		q.real = q1.real + q2.real;
		return q;
	}

	friend Quaternion operator -( const Quaternion& q1, const Quaternion& q2 ) {
		Quaternion q;
		q.img = q1.img - q2.img;
		q.real = q1.real - q2.real;
		return q;
	}


	friend Quaternion operator *( const Quaternion& q1, const Quaternion& q2 ) {
		Quaternion q;
		float dot_i = q1.img.dot(q2.img);
		Vec3 cross_i(q1.img);
		cross_i.cross(q2.img);
		
		q.real = q1.real*q2.real - dot_i;
		q.img = q1.real*q2.img + q2.real*q1.img + cross_i;
		return q;
	}

	friend Quaternion operator *( const float& f, const Quaternion& q1 ) {
		Quaternion q(q1);
		q.real = q.real * f;
		q.img = q.img * f;
		return q;
	}

	inline void getRotation(float *angle, Vec3* axis) const {
		float cos_angle = real;
		*angle = Degrees( 2.0f * acos( cos_angle ) );
		float sin_angle = sqrt( 1.0f - cos_angle * cos_angle );
		
		if( fabs( sin_angle ) < 0.0005f ) {
			sin_angle = 1.0f;
		}	else {
			sin_angle = 1.0f / sin_angle;
		}
		
		*axis = img * sin_angle;
		axis->normalize();
	}


	inline void normalize() {
		float len = sqrt(real*real + img.dot(img));
		real = real/len;
		img = img * (1.0/len);
	}
		

	inline void conjugate()
	{ img = -img; }
	

	void invert()
	{
		float l = img.dot(img) + real*real;
		if( l != 0.0f )	{
			l = 1.0f / l;
			conjugate();
			img = l * img;
			real *= l;
		}
	}

};

float dot(const Quaternion &q1, const Quaternion &q2) {
	float dot_i = q1.img.dot(q2.img);
	return q1.real *q2.real + dot_i;
}

Quaternion slerp(float t, const Quaternion &q1, const Quaternion &q2) {

	float cos_o = dot(q1, q2);
	float o = acos(cos_o);

	//std::cout << "cos_o: " << cos_o << " o: " << o << std::endl;
 
	return (sin( (1-t)*o )/sin(o))*q1 + (sin( t*o )/sin(o))*q2;
}


// assume no scaling
Quaternion::Quaternion(const float* m) {
	float ortho[4][4];
	memcpy(ortho, m, sizeof(float) * 16);
	orthoNormalize(ortho);

	//float trace = ortho[0][0] +
	//	ortho[1][1] + 
	//	ortho[2][2] + 1.0;

	//	std::cout << "trace: " << trace << std::endl;
		

	float S = 2 * sqrt(ortho[0][0] +
										 ortho[1][1] + 
										 ortho[2][2] + 1.0);
	
	real = 0.25 * S;

  img.x = ( ortho[2][1] - ortho[1][2] ) / S;
  img.y = ( ortho[0][2] - ortho[2][0] ) / S;
  img.z = ( ortho[1][0] - ortho[0][1] ) / S;

	/*
	for (int i = 0; i < 4; i++) { 
		for (int j = 0; j < 4; j++) { 
			std::cout << ortho[i][j] << "\t" ;
		}
		std::cout << std::endl;
	}
	*/

}

Quaternion::Quaternion(const Quaternion &q) {
	real = q.real;
	img = q.img;
}

Quaternion::Quaternion() {
	real = 1.0;
}


void makeQuat(const Quaternion &q, float mat[4][4]) {
  float sqw = q.real * q.real;
  float sqx = q.img.x * q.img.x;
  float sqy = q.img.y * q.img.y;
  float sqz = q.img.z * q.img.z;
  mat[0][0] = 1 - 2*sqy - 2*sqz;
  mat[1][1] = 1 - 2*sqx - 2*sqz;
  mat[2][2] = 1 - 2*sqx - 2*sqy;

  float xy = q.img.x * q.img.y;
  float wz = q.img.z * q.real;
  mat[0][1] = 2.0 * (xy - wz);
  mat[1][0] = 2.0 * (xy + wz);

  float zx = q.img.x * q.img.z;
  float yw = q.img.y * q.real;
  mat[0][2] = 2.0 * (zx + yw);
  mat[2][0] = 2.0 * (zx - yw);
  

	float yz = q.img.y * q.img.z;
  float wx = q.img.x * q.real;
  mat[1][2] = 2.0 * (yz - wx);
  mat[2][1] = 2.0 * (yz + wx);

  mat[3][3] = 1;

  mat[0][3] = mat[1][3] = mat[2][3] = mat[3][0] = mat[3][1] = mat[3][2] = 0.0f;


	// flip signs on third row for some reason...
	mat[2][0] = -mat[2][0];
	mat[2][1] = -mat[2][1];
	mat[2][2] = -mat[2][2];

}


// BlurredCamera Declarations

class BlurredCamera : public ProjectiveCamera {
public:
	// BlurredCamera Public Methods
	BlurredCamera(const Transform &world2cam,
		const float Screen[4], float hither, float yon,
		float sopen, float sclose,
		float lensr, float focald, float fov,
		Film *film, float mode, float _settime);
	float GenerateRay(const Sample &sample, Ray *) const;

protected:
  void RegenerateCamera(float at_time) const;

	Transform world2cam_open;
	Transform world2cam_close;
	
	//	mutable Transform* world2cam_current;

	mutable Transform WorldToCamera_current;
	mutable Transform CameraToWorld_current;
	mutable Transform WorldToScreen_current;

	float store_screen[4];
	float store_fov;

	mutable float currtime;
	float settime;
	int mode;
};
// BlurredCamera Method Definitions



void BlurredCamera::RegenerateCamera( float at_time ) const {

	//	std::cout << " at time " << at_time << std::endl; 


	// Make new camera for new ray with interpolated world2cam
	const float* open_mat = world2cam_open.getMatrix();
	const float* close_mat = world2cam_close.getMatrix();

	//std::cout << " time " << at_time << std::endl;

	float _mat4[4][4];

	if(mode == 0) {
		for (int i = 0; i < 4; i++) { 
			for (int j = 0; j < 3; j++) { 
				_mat4[i][j] = Lerp( at_time, *( open_mat + i*4 + j), *( close_mat + i*4 + j));
			}
		}

		Vec3 open_trans( *( open_mat + 0*4 + 3),  *( open_mat + 1*4 + 3), *( open_mat + 2*4 + 3));
		Vec3 close_trans( *( close_mat + 0*4 + 3),  *( close_mat + 1*4 + 3), *( close_mat + 2*4 + 3));

		Vec3 between_trans( Lerp(at_time, open_trans.x, close_trans.x),
												Lerp(at_time, open_trans.y, close_trans.y),
												Lerp(at_time, open_trans.z, close_trans.z));

		_mat4[0][3] = between_trans.x;
		_mat4[1][3] = between_trans.y;
		_mat4[2][3] = between_trans.z;
		_mat4[3][3] = 1;

	} else if (mode == 1) {

		Vec3 open_trans( *( open_mat + 0*4 + 3),  *( open_mat + 1*4 + 3), *( open_mat + 2*4 + 3));
		Vec3 close_trans( *( close_mat + 0*4 + 3),  *( close_mat + 1*4 + 3), *( close_mat + 2*4 + 3));

		Vec3 between_trans( Lerp(at_time, open_trans.x, close_trans.x),
												Lerp(at_time, open_trans.y, close_trans.y),
												Lerp(at_time, open_trans.z, close_trans.z));
		
		Quaternion open_quat(open_mat);
		open_quat.normalize();
		Quaternion close_quat(close_mat);
		close_quat.normalize();

		Quaternion between_quat = slerp(at_time, open_quat, close_quat);

		makeQuat(between_quat, _mat4);

		_mat4[0][3] = between_trans.x;
		_mat4[1][3] = between_trans.y;
		_mat4[2][3] = between_trans.z;

	} else if (mode == 2) {

		Vec3 open_trans( *( open_mat + 0*4 + 3),  *( open_mat + 1*4 + 3), *( open_mat + 2*4 + 3));
		Vec3 close_trans( *( close_mat + 0*4 + 3),  *( close_mat + 1*4 + 3), *( close_mat + 2*4 + 3));

		Vec3 between_trans( Lerp(at_time, open_trans.x, close_trans.x),
												Lerp(at_time, open_trans.y, close_trans.y),
												Lerp(at_time, open_trans.z, close_trans.z));
		
		Quaternion open_quat(open_mat);
		open_quat.normalize();
		Quaternion close_quat(close_mat);
		close_quat.normalize();

	}

	/*
	
		std::cout << "    -------    " << std::endl;
		for (int i = 0; i < 4; i++) { 
			for (int j = 0; j < 4; j++) { 
				std::cout << *( open_mat + i*4 + j) << "\t" ;
			}
			std::cout << std::endl;
		}
		std::cout << "    -------    " << std::endl;
		Quaternion foo(open_mat); 
		makeQuat(foo, _mat4);
		for (int i = 0; i < 4; i++) { 
			for (int j = 0; j < 4; j++) { 
				std::cout << _mat4[i][j] << "\t" ;
			}
			std::cout << std::endl;
		}
		
		std::cout << "    -------    " << std::endl;

	*/

	/*
	
		std::cout << "    -------    " << std::endl;
		for (int i = 0; i < 4; i++) { 
			for (int j = 0; j < 4; j++) { 
				std::cout << *( open_mat + i*4 + j) << "\t" ;
			}
			std::cout << std::endl;
		}
		std::cout << "    -------    " << std::endl;
		for (int i = 0; i < 4; i++) { 
			for (int j = 0; j < 4; j++) { 
				std::cout << _mat4[i][j] << "\t" ;
			}
			std::cout << std::endl;
		}
		std::cout << "    -------    " << std::endl;
		for (int i = 0; i < 4; i++) { 
			for (int j = 0; j < 4; j++) { 
				std::cout << *( close_mat + i*4 + j) << "\t" ;
			}
			std::cout << std::endl;
		}
		std::cout << "    -------    " << std::endl;
	
	*/		

	WorldToCamera_current = Transform(_mat4);
	CameraToWorld_current = WorldToCamera_current.GetInverse();
	WorldToScreen_current = CameraToScreen * WorldToCamera_current;

}


BlurredCamera::
    BlurredCamera(const Transform &world2cam,
		const float Screen[4], float hither, float yon,
		float sopen, float sclose,
		float lensr, float focald,
		float fov, Film *f, float _mode, float _settime)
	: ProjectiveCamera(world2cam,
	    Perspective(fov, hither, yon),
		Screen, hither, yon, sopen, sclose,
		lensr, focald, f), 
		world2cam_close(LookAt(Point(0, 3, 3), Point(.2, 0, 0), Vector(0, 1, 0)))
 {

	memcpy(store_screen, Screen, sizeof(float)*4);
	
	store_fov = fov;

	currtime = 0;

	mode = _mode;
	world2cam_open = world2cam;
	settime = _settime;

}

float BlurredCamera::GenerateRay(const Sample &sample,
		Ray *ray) const {

	// Set ray time value
	ray->time = Lerp(sample.time, ShutterOpen, ShutterClose);
	//	RegenerateCamera(ray->time);


	if (settime < 0) {

		if (currtime != ray->time) {
			RegenerateCamera(ray->time);
			currtime = ray->time;
		} 
	} else {
		RegenerateCamera(settime);
	}


	// Generate raster and camera samples
	Point Pras(sample.imageX, sample.imageY, 0);
	Point Pcamera;
	RasterToCamera(Pras, &Pcamera);
	ray->o = Pcamera;
	ray->d = Vector(Pcamera.x, Pcamera.y, Pcamera.z);

	// Modify ray for depth of field
	if (LensRadius > 0.) {
		// Sample point on lens
		float lensU, lensV;
		ConcentricSampleDisk(sample.lensU, sample.lensV,
		                     &lensU, &lensV);
		lensU *= LensRadius;
		lensV *= LensRadius;
		// Compute point on plane of focus
		float ft = (FocalDistance - ClipHither) / ray->d.z;
		Point Pfocus = (*ray)(ft);
		// Update ray for effect of lens
		ray->o.x += lensU;
		ray->o.y += lensV;
		ray->d = Pfocus - ray->o;
	}
	ray->d = Normalize(ray->d);
	ray->mint = 0.;
	ray->maxt = (ClipYon - ClipHither) / ray->d.z;
	CameraToWorld_current(*ray, ray);
	return 1.f;
}
extern "C" DLLEXPORT Camera *CreateCamera(const ParamSet &params,
		const Transform &world2cam, Film *film) {
	// Extract common camera parameters from _ParamSet_
	float hither = max(1e-4f, params.FindOneFloat("hither", 1e-3f));
	float yon = min(params.FindOneFloat("yon", 1e30f), 1e30f);
	float mode = params.FindOneFloat("mode", 0.f);
	float settime = params.FindOneFloat("settime", -1.f);

	float shutteropen = params.FindOneFloat("shutteropen", 0.f);
	float shutterclose = params.FindOneFloat("shutterclose", 1.f);
	float lensradius = params.FindOneFloat("lensradius", 0.f);
	float focaldistance = params.FindOneFloat("focaldistance", 1e30f);
	float frame = params.FindOneFloat("frameaspectratio",
		float(film->xResolution)/float(film->yResolution));
	float screen[4];
	if (frame > 1.f) {
		screen[0] = -frame;
		screen[1] =  frame;
		screen[2] = -1.f;
		screen[3] =  1.f;
	}
	else {
		screen[0] = -1.f;
		screen[1] =  1.f;
		screen[2] = -1.f / frame;
		screen[3] =  1.f / frame;
	}
	int swi;
	const float *sw = params.FindFloat("screenwindow", &swi);
	if (sw && swi == 4)
		memcpy(screen, sw, 4*sizeof(float));
	float fov = params.FindOneFloat("fov", 90.);
	return new BlurredCamera(world2cam, screen, hither, yon,
		shutteropen, shutterclose, lensradius, focaldistance,
		fov, film, mode, settime);
}