/* The original version of this file came from OpenTissue
 *
 * Copyright (C) 2003 Department of Computer Science, University of Copenhagen
 * Modifications copyright 2004, 2005 Nicholas Bishop
 * 
 * This file is part of SharpConstruct.
 *
 * SharpConstruct 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.
 *
 * SharpConstruct 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 SharpConstruct; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

#include <cmath>
#include "ArcBall.h"

ArcBall::ArcBall( const double r )
: radius( r )
{
    Reset();
}

ArcBall::~ArcBall()
{}

void ArcBall::NormalizeCoordinates( double &x, double &y, int w, int h )
{
	x = 2 * x / w  - 1;
	if( x < -1 )
		x = -1;
	if( x > 1 )
		x =  1;

	y = -( 2 * y / h - 1 );
	if (y < -1)
		y = -1;
	if (y > 1)
		y =  1;
}

void ArcBall::Reset()
{
	for( int i = 0; i < 3; i++ )
	{
		Panchor[ i ] = 0;
		Pcurrent[ i ] = 0;
		CurrentAxis[ i ] = 0;
	}
	CurrentAngle = 0;

	Identity( AnchorTransformation );
	Identity( IncrementalTransformation );
	Identity( CurrentTransformation );
  
	ProjectOntoSurface( Panchor );
	ProjectOntoSurface( Pcurrent );
}

void ArcBall::BeginDrag( const double x, const double y )
{
    CurrentAngle = 0;
    CurrentAxis[ 0 ] = 0;
    CurrentAxis[ 1 ] = 0;
    CurrentAxis[ 2 ] = 0;

    Copy( CurrentTransformation, AnchorTransformation );
    Identity( IncrementalTransformation );
    Identity( CurrentTransformation );

    Panchor[ 0 ] = x;
    Panchor[ 1 ] = y;
    Panchor[ 2 ] = 0;
    ProjectOntoSurface( Panchor );
    Pcurrent[ 0 ] = x;
    Pcurrent[ 1 ] = y;
    Pcurrent[ 2 ] = 0;
    ProjectOntoSurface( Pcurrent );
}

void ArcBall::Drag( const double x, const double y )
{
    Pcurrent[ 0 ] = x;
    Pcurrent[ 1 ] = y;
    Pcurrent[ 2 ] = 0;
    ProjectOntoSurface( Pcurrent );

    ComputeIncrementalTransformation( Panchor, Pcurrent,
	                                  IncrementalTransformation );
}

void ArcBall::ProjectOntoSurface( Vector &P )
{
	double radius2 = radius * radius;
	double length2 = P[ 0 ] * P[ 0 ] + P[ 1 ] * P[ 1 ];

	if( length2 <= radius2 / 2.0 )
		P[ 2 ] = sqrt(radius2 - length2);
	else
	{
		P[ 2 ] = radius2 / ( 2 * sqrt( length2 ) );
	
		double length = sqrt( length2 + P[ 2 ] * P[ 2 ] );
		P[ 0 ] /= length;
		P[ 1 ] /= length;
		P[ 2 ] /= length;
	}
	double lengthP = Length( P );
	for( int i = 0; i < 3; i++ )
		P[ i ] /= lengthP;
}

const ArcBall::Transform& ArcBall::Transformation()
{
	Multiply( IncrementalTransformation, AnchorTransformation,
	          CurrentTransformation );

	return CurrentTransformation;
}

// Just a little debugging helper
/*void printT( const ArcBall::Transform& t )
{
	for( unsigned i = 0; i < 4; i++ )
	{
		for( unsigned j = 0; j < 4; j++ )
			std::cout << std::setw( 9 ) << t[ i ][ j ] << " ";
		std::cout << std::endl;
	}
	std::cout << std::endl;
}*/

const ArcBall::Transform& ArcBall::SnapRotation( const Transform& t, float angle )
{
	//printT( t );

	const float PI = 3.1415;

	float aX, aY, aZ;
	float a, b, c, d, e, f, trX, trY;

	aY = asin( t[ 0 ][ 2 ] );
	c = cos( aY );

	if( fabs( c ) > 0.005f )
	{
		trX = t[ 2 ][ 2 ] / c;
		trY = -t[ 1 ][ 2 ]  / c;
		aX = atan2( trY, trX );
		trX = t[ 0 ][ 0 ] / c;
		trY = -t[ 0 ][ 1 ] / c;
		aZ = atan2( trY, trX );
	}
	else
	{
		aX = 0;
		trX = t[ 1 ][ 1 ];
		trY = t[ 1 ][ 0 ];
		aZ = atan2( trY, trX );
	}

	////
	aX = round( ( aX * ( 180 / PI ) ) * ( 1.0f / angle ) ) * angle;
	aY = round( ( aY * ( 180 / PI ) ) * ( 1.0f / angle ) ) * angle;
	aZ = round( ( aZ * ( 180 / PI ) ) * ( 1.0f / angle ) ) * angle;

	aX *= ( PI / 180 );
	aY *= ( PI / 180 );
	aZ *= ( PI / 180 );
	////

	a = cos( aX );
	b = sin( aX );
	c = cos( aY );
	d = sin( aY );
	e = cos( aZ );
	f = sin( aZ );

	SnapTransform[ 0 ][ 0 ] = c * e;
	SnapTransform[ 0 ][ 1 ] = -c * f;
	SnapTransform[ 0 ][ 2 ] = d;
	SnapTransform[ 0 ][ 3 ] = 0;
	SnapTransform[ 1 ][ 0 ] = b * d * e + a * f;
	SnapTransform[ 1 ][ 1 ] = -b * d * f + a * e;
	SnapTransform[ 1 ][ 2 ] = -b * c;
	SnapTransform[ 1 ][ 3 ] = 0;
	SnapTransform[ 2 ][ 0 ] = -a * d * e + b * f;
	SnapTransform[ 2 ][ 1 ] = a * d * f + b * e;
	SnapTransform[ 2 ][ 2 ] = a * c;
	SnapTransform[ 2 ][ 3 ] = 0;
	SnapTransform[ 3 ][ 0 ] = 0;
	SnapTransform[ 3 ][ 1 ] = 0;
	SnapTransform[ 3 ][ 2 ] = 0;
	SnapTransform[ 3 ][ 3 ] = 1;

	//printT( SnapTransform );
	//std::cout << "aX: " << aX << ", aY: " << aY << ", aZ: " << aZ << std::endl;
	//std::cout << "a: " << a << ", b: " << b << ", c: " << c << ", d: " << d << ", e: " << e << ", f: " << f << std::endl;	

	return SnapTransform;
}

void ArcBall::BakeSnapRotation()
{
	for( unsigned i = 0; i < 4; i++ )
		for( unsigned j = 0; j < 4; j++ )
			AnchorTransformation[ i ][ j ] = SnapTransform[ i ][ j ];

	Identity( IncrementalTransformation );
	Identity( CurrentTransformation );
}

void ArcBall::Identity( Transform &transformation ) const
{
	int i, j;
	
	for( i = 0; i < 4; i++ )
		for( j = 0; j < 4; j++ )
			transformation[ i ][ j ] = 0;

	for( i = 0; i < 4; i++ )
		transformation[ i ][ i ] = 1;
}

void ArcBall::Copy( const Transform &src, Transform &dst ) const
{
	int i, j;
	
	for( i = 0; i < 4; i++ )
		for( j = 0; j < 4; j++ )
			dst[ i ][ j ] = src[ i ][ j ];
}

void ArcBall::Multiply( const Transform &trans1, const Transform &trans2,
                        Transform &transformation ) const
{
	int i, j, k;
	double sum;
	
	for( i = 0; i < 4; i++ )
	{
		for( j = 0; j < 4; j++ )
		{
			for( k = 0, sum = 0; k < 4; k++ )
				sum += trans1[ i ][ k ] * trans2[ k ][ j ];
			transformation[ i ][ j ] = sum;
		}
	}
}

double ArcBall::Length( const Vector &vec ) const
{
	double length = 0;
	for( int i = 0; i < 3; i++ )
		length += vec[ i ] * vec[ i ];
	
	return sqrt(length);
}


void ArcBall::Copy( const Vector &src, Vector &dst ) const
{
	for( int i = 0; i < 3; i++ )
		dst[ i ] = src[ i ];
}

double ArcBall::Dot( const Vector &vec1, const Vector &vec2 ) const
{
	double dotprod( 0 );
	for( int i = 0; i < 3; i++ )
		dotprod += vec1[ i ] * vec2[ i ];
	
	return dotprod;
}


void ArcBall::Cross(const Vector& vec1, const Vector& vec2,
						 Vector& crossprod) const
{
	crossprod[ 0 ] = vec1[ 1 ] * vec2[ 2 ] - vec2[ 1 ] * vec1[ 2 ];
	crossprod[ 1 ] = vec2[ 0 ] * vec1[ 2 ] - vec1[ 0 ] * vec2[ 2 ];
	crossprod[ 2 ] = vec1[ 0 ] * vec2[ 1 ] - vec2[ 0 ] * vec1[ 1 ];
}

void ArcBall::ComputeIncrementalTransformation(const Vector& Panchor,
					       const Vector& Pcurrent,
					       Transform& transformation)
{
	double lengthPa = Length(Panchor);
	double lengthPc = Length(Pcurrent);
	Vector Pa;
	Vector Pc;
	for( int i = 0; i < 3; i++ )
	{
		Pa[ i ] = Panchor[i] / lengthPa;
		Pc[ i ] = Pcurrent[i] / lengthPc;
	}

	Vector Axis;
	Cross( Panchor, Pcurrent, Axis );
	double lengthAxis = Length(Axis);
	double Ux = Axis[ 0 ];
	double Uy = Axis[ 1 ];
	double Uz = Axis[ 2 ];
	if( lengthAxis != 0 )
	{
		Ux /= lengthAxis;
		Uy /= lengthAxis;
		Uz /= lengthAxis;
	}
	CurrentAxis[ 0 ] = Ux;
	CurrentAxis[ 1 ] = Uy;
	CurrentAxis[ 2 ] = Uz;
	
	double Theta = atan2( lengthAxis, Dot( Panchor, Pcurrent ) );
	CurrentAngle = Theta;
	
	double sinTheta = sin( Theta );
	double cosTheta = cos( Theta );

	transformation[ 0 ][ 0 ] = Ux * Ux + (1 - Ux * Ux) * cosTheta;
	transformation[ 0 ][ 1 ] = Ux * Uy * (1 - cosTheta) - Uz * sinTheta;
	transformation[ 0 ][ 2 ] = Ux * Uz * (1 - cosTheta) + Uy * sinTheta;
	transformation[ 0 ][ 3 ] = 0;
	transformation[ 1 ][ 0 ] = Ux * Uy * (1 - cosTheta) + Uz * sinTheta;
	transformation[ 1 ][ 1 ] = Uy * Uy + (1 - Uy * Uy) * cosTheta;
	transformation[ 1 ][ 2 ] = Uy * Uz * (1 - cosTheta) - Ux * sinTheta;
	transformation[ 1 ][ 3 ] = 0;
	transformation[ 2 ][ 0 ] = Ux * Uz * (1 - cosTheta) - Uy * sinTheta;
	transformation[ 2 ][ 1 ] = Uy * Uz * (1 - cosTheta) + Ux * sinTheta;
	transformation[ 2 ][ 2 ] = Uz * Uz + (1 - Uz * Uz) * cosTheta;
	transformation[ 2 ][ 3 ] = 0;
	transformation[ 3 ][ 0 ] = 0;
	transformation[ 3 ][ 1 ] = 0;
	transformation[ 3 ][ 2 ] = 0;
	transformation[ 3 ][ 3 ] = 1;
}