/* Panorama_Tools	-	Generate, Edit and Convert Panoramic Images
   Copyright (C) 1998,1999 - Helmut Dersch  der@fh-furtwangen.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, 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., 675 Mass Ave, Cambridge, MA 02139, USA.  */

/*------------------------------------------------------------*/


#include "filter.h"
#include <float.h>
#include "f2c.h"

#define R_EPS  1.0e-6	
#define MAXITER 100



#ifndef abs
#define abs(a) ( (a) >= 0 ? (a) : -(a) )
#endif

void 	matrix_matrix_mult	( double m1[3][3],double m2[3][3],double result[3][3]);
int 	polzeros_();


//------------------------- Some auxilliary math functions --------------------------------------------


void matrix_mult( double m[3][3], double vector[3] )
{
	register int i;
	register double v0 = vector[0];
	register double v1 = vector[1];
	register double v2 = vector[2];
	
	
	for(i=0; i<3; i++)
	{
		vector[i] = m[i][0] * v0 + m[i][1] * v1 + m[i][2] * v2;
	}
}
		
void matrix_inv_mult( double m[3][3], double vector[3] )
{
	register int i;
	register double v0 = vector[0];
	register double v1 = vector[1];
	register double v2 = vector[2];
	
	for(i=0; i<3; i++)
	{
		vector[i] = m[0][i] * v0 + m[1][i] * v1 + m[2][i] * v2;
	}
}
		
void matrix_matrix_mult( double m1[3][3],double m2[3][3],double result[3][3])
{
	register int i,k;
	
	for(i=0;i<3;i++)
	{
		for(k=0; k<3; k++)
		{
			result[i][k] = m1[i][0] * m2[0][k] + m1[i][1] * m2[1][k] + m1[i][2] * m2[2][k];
		}
	}
}

// Set matrix elements based on Euler angles a, b, c

void SetMatrix( double a, double b, double c , double m[3][3], int cl )
{
	double mx[3][3], my[3][3], mz[3][3], dummy[3][3];
	

	// Calculate Matrices;

	mx[0][0] = 1.0 ; 				mx[0][1] = 0.0 ; 				mx[0][2] = 0.0;
	mx[1][0] = 0.0 ; 				mx[1][1] = cos(a) ; 			mx[1][2] = sin(a);
	mx[2][0] = 0.0 ;				mx[2][1] =-mx[1][2] ;			mx[2][2] = mx[1][1];
	
	my[0][0] = cos(b); 				my[0][1] = 0.0 ; 				my[0][2] =-sin(b);
	my[1][0] = 0.0 ; 				my[1][1] = 1.0 ; 				my[1][2] = 0.0;
	my[2][0] = -my[0][2];			my[2][1] = 0.0 ;				my[2][2] = my[0][0];
	
	mz[0][0] = cos(c) ; 			mz[0][1] = sin(c) ; 			mz[0][2] = 0.0;
	mz[1][0] =-mz[0][1] ; 			mz[1][1] = mz[0][0] ; 			mz[1][2] = 0.0;
	mz[2][0] = 0.0 ;				mz[2][1] = 0.0 ;				mz[2][2] = 1.0;

	if( cl )
		matrix_matrix_mult( mz,	mx,	dummy);
	else
		matrix_matrix_mult( mx,	mz,	dummy);
	matrix_matrix_mult( dummy, my, m);
}


// Do 3D-coordinate Transformation

void doCoordinateTransform( CoordInfo *ci, tMatrix *t )
{
	double m[3][3],a,b,c;
	int i;
	double mx[3][3], my[3][3], mz[3][3], dummy[3][3];
	

	// Calculate Matrices;
	a = DEG_TO_RAD( t->alpha );
	b = DEG_TO_RAD( t->beta  );
	c = DEG_TO_RAD( t->gamma );

	mx[0][0] = 1.0 ; 				mx[0][1] = 0.0 ; 				mx[0][2] = 0.0;
	mx[1][0] = 0.0 ; 				mx[1][1] = cos(a) ; 			mx[1][2] = sin(a);
	mx[2][0] = 0.0 ;				mx[2][1] =-mx[1][2] ;			mx[2][2] = mx[1][1];
	
	my[0][0] = cos(b); 				my[0][1] = 0.0 ; 				my[0][2] =-sin(b);
	my[1][0] = 0.0 ; 				my[1][1] = 1.0 ; 				my[1][2] = 0.0;
	my[2][0] = -my[0][2];			my[2][1] = 0.0 ;				my[2][2] = my[0][0];
	
	mz[0][0] = cos(c) ; 			mz[0][1] = sin(c) ; 			mz[0][2] = 0.0;
	mz[1][0] =-mz[0][1] ; 			mz[1][1] = mz[0][0] ; 			mz[1][2] = 0.0;
	mz[2][0] = 0.0 ;				mz[2][1] = 0.0 ;				mz[2][2] = 1.0;

	matrix_matrix_mult( my,	mz,	dummy);
	matrix_matrix_mult( mx, dummy,	m);
	
	// Scale 
	
	for(i=0; i<3; i++)
		ci->x[i] *= t->scale;
	
	// Do shift
	
	for(i=0; i<3; i++)
		ci->x[i] += t->x_shift[i];
	
	// Do rotation
#if 0	
	SetMatrix( DEG_TO_RAD( t->alpha ) , 
			   DEG_TO_RAD( t->beta  ) ,
			   DEG_TO_RAD( t->gamma ) ,
			   m, 0 );

#endif
	matrix_inv_mult( m, ci->x );

}

void SettMatrixDefaults( tMatrix *t )
{
	int i;
	
	t->alpha = t->beta = t->gamma = 0.0;
	for(i=0; i<3; i++)
		t->x_shift[i] = 0.0;
	
	t->scale = 1.0;
}


	
	
	
	

//------------------------------- Transformation functions --------------------------------------------


#define 	distance	(*((double*)params))
#define 	shift		(*((double*)params))
#define		var0		((double*)params)[0]
#define		var1		((double*)params)[1]

// execute a stack of functions stored in stack


void execute_stack		( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	register double 		xd = x_dest, 
							yd = y_dest;
	register struct fDesc*  stack = (struct fDesc *) params;;
	
		
	while( (stack->func) != NULL )
	{
		(stack->func) ( xd, yd, x_src, y_src, stack->param );
		xd = *x_src;	
		yd = *y_src;
		stack++;
	}
}
	


// Rotate equirectangular image

void rotate_erect( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double 180degree_turn(screenpoints), double turn(screenpoints);

		*x_src = x_dest + var1;

		while( *x_src < - var0 )
			*x_src += 2 *  var0;

		while( *x_src >  var0 )
			*x_src -= 2 *  var0;

		*y_src = y_dest ;
}



// Calculate inverse 4th order polynomial correction using Newton
// Don't use on large image (slow)!


void inv_radial( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double coefficients[4]

	register double rs, rd, f, scale;
	int iter = 0;

	rd 		= (sqrt( x_dest*x_dest + y_dest*y_dest )) / ((double*)params)[4]; // Normalized 

	rs	= rd;				
	f 	= (((((double*)params)[3] * rs + ((double*)params)[2]) * rs + 
				((double*)params)[1]) * rs + ((double*)params)[0]) * rs;

	while( abs(f - rd) > R_EPS && iter++ < MAXITER )
	{
		rs = rs - (f - rd) / ((( 4 * ((double*)params)[3] * rs + 3 * ((double*)params)[2]) * rs  + 
						  2 * ((double*)params)[1]) * rs + ((double*)params)[0]);

		f 	= (((((double*)params)[3] * rs + ((double*)params)[2]) * rs + 
				((double*)params)[1]) * rs + ((double*)params)[0]) * rs;
	}

	scale = rs / rd;
//	printf("scale = %lg iter = %d\n", scale,iter);	
	
	*x_src = x_dest * scale  ;
	*y_src = y_dest * scale  ;
}

void inv_vertical( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double coefficients[4]

	register double rs, rd, f, scale;
	int iter = 0;

	rd 		= abs( y_dest ) / ((double*)params)[4]; // Normalized 

	rs	= rd;				
	f 	= (((((double*)params)[3] * rs + ((double*)params)[2]) * rs + 
				((double*)params)[1]) * rs + ((double*)params)[0]) * rs;

	while( abs(f - rd) > R_EPS && iter++ < MAXITER )
	{
		rs = rs - (f - rd) / ((( 4 * ((double*)params)[3] * rs + 3 * ((double*)params)[2]) * rs  + 
						  2 * ((double*)params)[1]) * rs + ((double*)params)[0]);

		f 	= (((((double*)params)[3] * rs + ((double*)params)[2]) * rs + 
				((double*)params)[1]) * rs + ((double*)params)[0]) * rs;
	}

	scale = rs / rd;
//	printf("scale = %lg iter = %d\n", scale,iter);	
	
	*x_src = x_dest  ;
	*y_src = y_dest * scale  ;
}



void resize( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double scale_horizontal, double scale_vertical;

		*x_src = x_dest * var0;
		*y_src = y_dest * var1;
}

void shear( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double shear_horizontal, double shear_vertical;

		*x_src  = x_dest + var0 * y_dest;
		*y_src  = y_dest + var1 * x_dest;
}

void horiz( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double horizontal shift

		*x_src	= x_dest + shift;	
		*y_src  = y_dest;
}

void vert( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double vertical shift

		*x_src	= x_dest;	
		*y_src  = y_dest + shift;		
}

	
void radial( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double coefficients[4], scale, correction_radius

	register double r, scale;

	r 		= (sqrt( x_dest*x_dest + y_dest*y_dest )) / ((double*)params)[4];
	if( r < ((double*)params)[5] )
	{
		scale 	= ((((double*)params)[3] * r + ((double*)params)[2]) * r + 
				((double*)params)[1]) * r + ((double*)params)[0];
	}
	else
		scale = 1000.0;
	
	*x_src = x_dest * scale  ;
	*y_src = y_dest * scale  ;
}

void vertical( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double coefficients[4]

	register double r, scale;

	r 		= y_dest / ((double*)params)[4];

	if( r < 0.0 ) r = -r;

	scale 	= ((((double*)params)[3] * r + ((double*)params)[2]) * r + 
				((double*)params)[1]) * r + ((double*)params)[0];
	
	*x_src = x_dest;
	*y_src = y_dest * scale  ;
}

void deregister( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params: double coefficients[4]

	register double r, scale;

	r 		= y_dest / ((double*)params)[4];

	if( r < 0.0 ) r = -r;

	scale 	= (((double*)params)[3] * r + ((double*)params)[2]) * r + 
				((double*)params)[1] ;
	
	*x_src = x_dest + abs( y_dest ) * scale;
	*y_src = y_dest ;
}



	
void persp_sphere( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params :  double Matrix[3][3], double distance

	register double theta,s,r;
	double v[3];

#if 0	// old 
	theta = sqrt( x_dest * x_dest + y_dest * y_dest ) / *((double*) ((void**)params)[1]);
	phi   = atan2( y_dest , x_dest );

	v[0] = *((double*) ((void**)params)[1]) * sin( theta ) * cos( phi );
	v[1] = *((double*) ((void**)params)[1]) * sin( theta ) * sin( phi );
	v[2] = *((double*) ((void**)params)[1]) * cos( theta );
	
	matrix_inv_mult( (double(*)[3]) ((void**)params)[0], v );

	theta = atan2( sqrt( v[0]*v[0] + v[1]*v[1] ), v[2] );
	phi   = atan2( v[1], v[0] );
	*x_src = *((double*) ((void**)params)[1]) * theta * cos( phi );
	*y_src = *((double*) ((void**)params)[1]) * theta * sin( phi );
#endif

	r 		= sqrt( x_dest * x_dest + y_dest * y_dest );
	theta 	= r / *((double*) ((void**)params)[1]);
	if( r == 0.0 )
		s = 0.0;
	else
		s = sin( theta ) / r;

	v[0] =  s * x_dest ;
	v[1] =  s * y_dest ;
	v[2] =  cos( theta );

	matrix_inv_mult( (double(*)[3]) ((void**)params)[0], v );

	r 		= sqrt( v[0]*v[0] + v[1]*v[1] );
	if( r == 0.0 )
		theta = 0.0;
	else
		theta 	= *((double*) ((void**)params)[1]) * atan2( r, v[2] ) / r;
	*x_src 	= theta * v[0];
	*y_src 	= theta * v[1];


}	

void persp_rect( double x_dest, double y_dest, double* x_src, double* y_src, void* params)
{
	// params :  double Matrix[3][3], double distance, double x-offset, double y-offset

	double v[3];
	
	v[0] = x_dest + *((double*) ((void**)params)[2]);
	v[1] = y_dest + *((double*) ((void**)params)[3]);
	v[2] = *((double*) ((void**)params)[1]);
	
	matrix_inv_mult( (double(*)[3]) ((void**)params)[0], v );
	
	*x_src = v[0] * *((double*) ((void**)params)[1]) / v[2] ;
	*y_src = v[1] * *((double*) ((void**)params)[1]) / v[2] ;
}



void rect_pano( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{									
								
	*x_src = distance * tan( x_dest / distance ) ;
	*y_src = y_dest / cos( x_dest / distance );
}

void pano_rect( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{	
	*x_src = distance * atan ( x_dest / distance );
	*y_src = y_dest * cos( *x_src / distance );
}

void rect_erect( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{	
	// params: double distance

	register double  phi, theta;

	phi 	= x_dest / distance;
	theta 	=  - y_dest / distance  + PI / 2.0;
	if(theta < 0)
	{
		theta = - theta;
		phi += PI;
	}
	if(theta > PI)
	{
		theta = PI - (theta - PI);
		phi += PI;
	}

#if 0
	v[2] = *((double*)params) * sin( theta ) * cos( phi );   //  x' -> z
	v[0] = *((double*)params) * sin( theta ) * sin( phi );	//  y' -> x
	v[1] = *((double*)params) * cos( theta );				//  z' -> y
	
	phi   = atan2( v[1], v[0] );
//  old:	
//	theta = atan2( sqrt( v[0]*v[0] + v[1]*v[1] ) , v[2] );
//	rho = *((double*)params) * tan( theta );
//  new:
	rho = *((double*)params) * sqrt( v[0]*v[0] + v[1]*v[1] ) / v[2];
	*x_src = rho * cos( phi );
	*y_src = rho * sin( phi );
#endif
#if 1
	*x_src = distance * tan(phi);
	*y_src = distance / (tan( theta ) * cos(phi));
#endif
}

void pano_erect( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{	
	// params: double distance

	*x_src = x_dest;
	*y_src = distance * tan( y_dest / distance);
}

void erect_pano( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{	
	// params: double distance

	*x_src = x_dest;
	*y_src = distance * atan( y_dest / distance);
}

void sphere_cp_erect( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance, double b

	register double phi, theta;
#if 0	
	phi 	= - x_dest /  ( var0 * PI / 2.0);
	theta 	=  - ( y_dest + var1 ) / (var0 * PI / 2.0) ;
	
	*x_src = var0 * theta * cos( phi );
	*y_src = var0 * theta * sin( phi );
#endif
	phi 	= - x_dest /  ( var0 * PI / 2.0);
	theta 	=  - ( y_dest + var1 ) / ( PI / 2.0) ;
	
	*x_src =  theta * cos( phi );
	*y_src =  theta * sin( phi );
}

void sphere_tp_erect( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance

	register double phi, theta, r,s;
	double v[3];

	phi 	= x_dest / distance;
	theta 	=  - y_dest / distance  + PI / 2;
	if(theta < 0)
	{
		theta = - theta;
		phi += PI;
	}
	if(theta > PI)
	{
		theta = PI - (theta - PI);
		phi += PI;
	}

#if 0	

	v[2] = *((double*)params) * sin( theta ) * cos( phi );   //  x' -> z
	v[0] = *((double*)params) * sin( theta ) * sin( phi );	//  y' -> x
	v[1] = *((double*)params) * cos( theta );				//  z' -> y
	
	theta = atan2( sqrt( v[0]*v[0] + v[1]*v[1] ) , v[2] );
	
	phi   = atan2( v[1], v[0] );
	
	*x_src = *((double*)params) * theta * cos( phi );
	*y_src = *((double*)params) * theta * sin( phi );
#endif
	s = sin( theta );
	v[0] =  s * sin( phi );	//  y' -> x
	v[1] =  cos( theta );				//  z' -> y
	
	r = sqrt( v[1]*v[1] + v[0]*v[0]);	

	theta = distance * atan2( r , s * cos( phi ) );
	
	*x_src =  theta * v[0] / r;
	*y_src =  theta * v[1] / r;

}

void erect_sphere_cp( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance, double b

	register double phi, theta;

#if 0
	theta = sqrt( x_dest * x_dest + y_dest * y_dest ) / var0;
	phi   = atan2( y_dest , -x_dest );
	
	*x_src = var0 * phi;
	*y_src = var0 * theta - var1;
#endif
	theta = sqrt( x_dest * x_dest + y_dest * y_dest ) ;
	phi   = atan2( y_dest , -x_dest );
	
	*x_src = var0 * phi;
	*y_src = theta - var1;
	
}

void rect_sphere_tp( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance

	register double rho, theta,r;

#if 0	
	theta = sqrt( x_dest * x_dest + y_dest * y_dest ) / distance;
	phi   = atan2( y_dest , x_dest );
	
	if( theta > PI /2.0  ||  theta < -PI /2.0 )
		theta = PI /2.0 ;

	rho = distance * tan( theta );

	*x_src = rho * cos( phi );
	*y_src = rho * sin( phi );
#endif
	r 		= sqrt( x_dest * x_dest + y_dest * y_dest );
	theta 	= r / distance;
	
	if( theta > PI /2.0   )
		theta = PI /2.0 ;

	if( theta == 0.0 )
		rho = 1.0;
	else
		rho =  tan( theta ) / theta;
	*x_src = rho * x_dest ;
	*y_src = rho * y_dest ;
}

void sphere_tp_rect( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{	
	// params: double distance

	register double  theta, r;

#if 0	
	theta = atan( sqrt(x_dest*x_dest + y_dest*y_dest) / *((double*)params));
	phi   = atan2( y_dest , x_dest );
	
	*x_src = *((double*)params) * theta * cos( phi );
	*y_src = *((double*)params) * theta * sin( phi );
#endif
	r 		= sqrt(x_dest*x_dest + y_dest*y_dest) / distance;
	if( r== 0.0 )
		theta = 1.0;
	else
		theta 	= atan( r ) / r;
	
	*x_src =  theta * x_dest ;
	*y_src =  theta * y_dest ;

}

void sphere_tp_pano( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance

	register double r, s, Phi, theta;
	
#if 0
	register double Theta, phi;
	double v[3];
	
	Phi = x_dest / *((double*)params);
	Theta = PI /2.0 - atan( y_dest / distance );
	

	v[2] = *((double*)params) * sin( Theta ) * cos( Phi );   //  x' -> z
	v[0] = *((double*)params) * sin( Theta ) * sin( Phi );	//  y' -> x
	v[1] = *((double*)params) * cos( Theta );				//  z' -> y
	
	theta = atan2( sqrt( v[0]*v[0] + v[1]*v[1] ) , v[2] );
	phi   = atan2( v[1], v[0] );
	
	*x_src = *((double*)params) * theta * cos( phi );
	*y_src = *((double*)params) * theta * sin( phi );
#endif
#if 1
	Phi = x_dest / distance;
		
	s =  distance * sin( Phi ) ;	//  y' -> x
	
	r = sqrt( s*s + y_dest*y_dest );
	theta = distance * atan2( r , (distance * cos( Phi )) ) / r;
	
	*x_src =  theta * s ;
	*y_src =  theta * y_dest ;
#endif

}

void pano_sphere_tp( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance
	register double r,s, theta;
	double v[3];

#if 0	
	theta = sqrt( x_dest * x_dest + y_dest * y_dest ) / distance;
	phi   = atan2( y_dest , x_dest );

	v[1] = *((double*)params) * sin( theta ) * cos( phi );   //  x' -> y
	v[2] = *((double*)params) * sin( theta ) * sin( phi );	//  y' -> z
	v[0] = *((double*)params) * cos( theta );				//  z' -> x

	theta = atan2( sqrt( v[0]*v[0] + v[1]*v[1] ) , v[2] );
	phi   = atan2( v[1], v[0] );

	*x_src = *((double*)params) * phi;
	*y_src = *((double*)params) * tan( (-theta + PI /2.0) );
#endif
	
	r = sqrt( x_dest * x_dest + y_dest * y_dest );
	theta = r / distance;
	if( theta == 0.0 )
		s = 1.0 / distance;
	else
		s = sin( theta ) /r;

	v[1] =  s * x_dest ;   //  x' -> y
	v[0] =  cos( theta );				//  z' -> x


	*x_src = distance * atan2( v[1], v[0] );
	*y_src = distance * s * y_dest / sqrt( v[0]*v[0] + v[1]*v[1] );


}


void sphere_cp_pano( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance

	register double phi, theta;
	
	
	phi 	= -x_dest / (distance * PI / 2.0) ;
	theta	= PI /2.0 + atan( y_dest / (distance * PI/2.0) );

	*x_src = distance * theta * cos( phi );
	*y_src = distance * theta * sin( phi );
}

void erect_rect( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance
#if 0
	theta = atan( sqrt(x_dest*x_dest + y_dest*y_dest) / distance );
	phi   = atan2( y_dest , x_dest );


	v[1] = distance * sin( theta ) * cos( phi );   //  x' -> y
	v[2] = distance * sin( theta ) * sin( phi );	//  y' -> z
	v[0] = distance * cos( theta );				//  z' -> x
	
	theta = atan2( sqrt( v[0]*v[0] + v[1]*v[1] ) , v[2] );
	phi   = atan2( v[1], v[0] );

	*x_src = distance * phi;
	*y_src = distance * (-theta + PI /2.0);
#endif

	*x_src = distance * atan2( x_dest, distance );
	*y_src = distance * atan2(  y_dest, sqrt( distance*distance + x_dest*x_dest ) );


}


void erect_sphere_tp( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance

	register double  theta,r,s;
	double	v[3];
#if 0	
	theta = sqrt( x_dest * x_dest + y_dest * y_dest ) / *((double*)params);
	phi   = atan2( y_dest , x_dest );
	
	v[1] = *((double*)params) * sin( theta ) * cos( phi );   //  x' -> y
	v[2] = *((double*)params) * sin( theta ) * sin( phi );	//  y' -> z
	v[0] = *((double*)params) * cos( theta );				//  z' -> x
	
	theta = atan( sqrt( v[0]*v[0] + v[1]*v[1] ) / v[2] ); //was atan2
	phi   = atan2( v[1], v[0] );

	*x_src = *((double*)params) * phi;
	if(theta > 0.0)
	{
		*y_src = *((double*)params) * (-theta + PI /2.0);
	}
	else
		*y_src = *((double*)params) * (-theta - PI /2.0);
#endif
	r = sqrt( x_dest * x_dest + y_dest * y_dest );
	theta = r / distance;
	if(theta == 0.0)
		s = 1.0 / distance;
	else
		s = sin( theta) / r;
	
	v[1] =  s * x_dest;   
	v[0] =  cos( theta );				
	

	*x_src = distance * atan2( v[1], v[0] );
	*y_src = distance * atan( s * y_dest /sqrt( v[0]*v[0] + v[1]*v[1] ) ); 
}

void mirror_sphere_cp( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance, double b

	register double rho, phi, theta;

	theta = sqrt( x_dest * x_dest + y_dest * y_dest ) / ((double*)params)[0];
	phi   = atan2( y_dest , x_dest );
	
	rho = ((double*)params)[1] * sin( theta / 2.0 );
	
	*x_src = - rho * cos( phi );
	*y_src = rho * sin( phi );
}

void mirror_erect( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance, double b, double b2

	register double phi, theta, rho;
	
	phi 	=  x_dest / ( ((double*)params)[0] * PI/2.0) ;
	theta 	=  - ( y_dest + ((double*)params)[2] ) / (((double*)params)[0] * PI/2.0)  ;
	
	rho = ((double*)params)[1] * sin( theta / 2.0 );
	
	*x_src = - rho * cos( phi );
	*y_src = rho * sin( phi );
}

void mirror_pano( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance, double b

	register double phi, theta, rho;
	
	
	phi 	= -x_dest / (((double*)params)[0] * PI/2.0) ;
	theta	= PI /2.0 + atan( y_dest / (((double*)params)[0] * PI/2.0) );

	rho = ((double*)params)[1] * sin( theta / 2.0 );
	
	*x_src = rho * cos( phi );
	*y_src = rho * sin( phi );
}

void sphere_cp_mirror( double x_dest,double  y_dest, double* x_src, double* y_src, void* params)
{
	// params: double distance, double b

	register double phi, theta, rho;

	rho = sqrt( x_dest*x_dest + y_dest*y_dest );
	
	theta = 2 * asin( rho/((double*)params)[1] );
	phi   = atan2( y_dest , x_dest );

	*x_src = ((double*)params)[0] * theta * cos( phi );
	*y_src = ((double*)params)[0] * theta * sin( phi );
}


void shift_scale_rotate( double x_dest,double  y_dest, double* x_src, double* y_src, void* params){
	// params: double shift_x, shift_y, scale, cos_phi, sin_phi
	
	register double x = x_dest - ((double*)params)[0];
	register double y = y_dest - ((double*)params)[1];

	*x_src = (x * ((double*)params)[3] - y * ((double*)params)[4]) * ((double*)params)[2];
	*y_src = (x * ((double*)params)[4] + y * ((double*)params)[3]) * ((double*)params)[2];
}













// Correct radial luminance change using parabel

unsigned char radlum( unsigned char srcPixel, int xc, int yc, void *params )
{
	// params: second and zero order polynomial coeff
	register double result;

	result = (xc * xc + yc * yc) * ((double*)params)[0] + ((double*)params)[1];
	result = ((double)srcPixel) - result;

	if(result < 0.0) return 0;
	if(result > 255.0) return 255;

	return( (unsigned char)(result+0.5) );
}


// Get smallest positive (non-zero) root of polynomial with degree deg and
// (n+1) real coefficients p[i]. Return it, or 1000.0 if none exists or error occured
// Changed to only allow degree 3
#if 0
double smallestRoot( double *p )
{
	doublecomplex 		root[3], poly[4];
	doublereal 			radius[3], apoly[4], apolyr[4];
	logical 			myErr[3];
	double 				sRoot = 1000.0;
	doublereal 			theEps, theBig, theSmall;
	integer 			nitmax;
	integer 			iter;
	integer 			n,i;
	
	n 		= 3;

	
	for( i=0; i< n+1; i++)
	{
		poly[i].r = p[i];
		poly[i].i = 0.0;
	}
	
	theEps   = DBL_EPSILON;  		// machine precision 
	theSmall = DBL_MIN ; 			// smallest positive real*8          
	theBig   = DBL_MAX ; 			// largest real*8  

	nitmax 	= 100;

    polzeros_(&n, poly, &theEps, &theBig, &theSmall, &nitmax, root, radius, myErr, &iter, apoly, apolyr);

	for( i = 0; i < n; i++ )
	{
//		PrintError("No %d : Real %g, Imag %g, radius %g, myErr %ld", i, root[i].r, root[i].i, radius[i], myErr[i]);
		if( (root[i].r > 0.0) && (dabs( root[i].i ) <= radius[i]) && (root[i].r < sRoot) )
			sRoot = root[i].r;
	}

	return sRoot;
}
#endif

void cubeZero( double *a, int *n, double *root );
void squareZero( double *a, int *n, double *root );
double cubeRoot( double x );


void cubeZero( double *a, int *n, double *root ){
	if( a[3] == 0.0 ){ // second order polynomial
		squareZero( a, n, root );
	}else{
		double p = ((-1.0/3.0) * (a[2]/a[3]) * (a[2]/a[3]) + a[1]/a[3]) / 3.0;
		double q = ((2.0/27.0) * (a[2]/a[3]) * (a[2]/a[3]) * (a[2]/a[3]) - (1.0/3.0) * (a[2]/a[3]) * (a[1]/a[3]) + a[0]/a[3]) / 2.0;
		
		if( q*q + p*p*p >= 0.0 ){
			*n = 1;
			root[0] = cubeRoot(-q + sqrt(q*q + p*p*p)) + cubeRoot(-q - sqrt(q*q + p*p*p)) - a[2] / (3.0 * a[3]); 
		}else{
			double phi = acos( -q / sqrt(-p*p*p) );
			*n = 3;
			root[0] =  2.0 * sqrt(-p) * cos(phi/3.0) - a[2] / (3.0 * a[3]); 
			root[1] = -2.0 * sqrt(-p) * cos(phi/3.0 + PI/3.0) - a[2] / (3.0 * a[3]); 
			root[2] = -2.0 * sqrt(-p) * cos(phi/3.0 - PI/3.0) - a[2] / (3.0 * a[3]); 
		}
	}
	// PrintError("%lg, %lg, %lg, %lg root = %lg", a[3], a[2], a[1], a[0], root[0]);
}

void squareZero( double *a, int *n, double *root ){
	if( a[2] == 0.0 ){ // linear equation
		if( a[1] == 0.0 ){ // constant
			if( a[0] == 0.0 ){
				*n = 1; root[0] = 0.0;
			}else{
				*n = 0;
			}
		}else{
			*n = 1; root[0] = - a[0] / a[1];
		}
	}else{
		if( 4.0 * a[2] * a[0] > a[1] * a[1] ){
			*n = 0; 
		}else{
			*n = 2;
			root[0] = (- a[1] + sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
			root[1] = (- a[1] - sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
		}
	}

}

double cubeRoot( double x ){
	if( x == 0.0 )
		return 0.0;
	else if( x > 0.0 )
		return pow(x, 1.0/3.0);
	else
		return - pow(-x, 1.0/3.0);
}

double smallestRoot( double *p ){
	int n,i;
	double root[3], sroot = 1000.0;
	
	cubeZero( p, &n, root );
	
	for( i=0; i<n; i++){
		// PrintError("Root %d = %lg", i,root[i]);
		if(root[i] > 0.0 && root[i] < sroot)
			sroot = root[i];
	}
	
	// PrintError("Smallest Root  = %lg", sroot);
	return sroot;
}



