/*******************************************************************************
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * 
 * This library 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 Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 * 
 * Contact Info:
 * 	Bruce Donald
 * 	Duke University
 * 	Department of Computer Science
 * 	Levine Science Research Center (LSRC)
 * 	Durham
 * 	NC 27708-0129 
 * 	USA
 * 	brd@cs.duke.edu
 * 
 * Copyright (C) 2011 Jeffrey W. Martin and Bruce R. Donald
 * 
 * <signature of Bruce Donald>, April 2011
 * Bruce Donald, Professor of Computer Science
 ******************************************************************************/


package edu.duke.donaldLab.share.geom;

import java.util.ArrayList;

public class WelzlSphereSolver
{
	/* Jeff: 5/16/2008 - this is an implementation of Welzl's algorithm for finding min spheres
		http://www.gamedev.net/reference/articles/article2484.asp
		Adapted from Bernd Gaertner's implementation
		http://www.inf.ethz.ch/personal/gaertner/miniball.html
		This implementation is fixed in 3 dimensions, but Gaertner's implementation is technically d-dimensional
	*/
	
	/**************************
	 *   Definitions
	 **************************/
	
	private static final int d = 3;
	
	private class MaxExcessData
	{
		public double max_e;
		public int pivot;
		
		public MaxExcessData( double max_e, int pivot )
		{
			this.max_e = max_e;
			this.pivot = pivot;
		}
	}
	
	private class Miniball
	{
		/**************************
		 *   Data Members
		 **************************/
		
		private ArrayList<Vector3> L;
		private MiniballB B;
		private int support_end;
		
		
		/**************************
		 *   Constructors
		 **************************/
		
		public Miniball( )
		{
			L = new ArrayList<Vector3>();
			B = new MiniballB();
			support_end = 0;
		}

		/**************************
		 *   Methods
		 **************************/
		
		public void check_in( Vector3 p )
		{
			L.add( p );
		}
		
		public void build( )
		{
			// TEMP
			//System.out.println( "\n==Start==\n" );
			
			// just in case
			assert( L.size() > 0 );
			
			B.reset();
			support_end = 0;
			pivot_mb( L.size() );
		}
		
		public Vector3 center( )
		{
			return B.center();
		}
		
		public double squared_radius( )
		{
			return B.squared_radius();
		}
		
		
		/**************************
		 *   Functions
		 **************************/
		
		private void mtf_mb( int i )
		{
			support_end = 0;
			
			if( B.size() == d + 1 )
			{
				return;
			}
			
			for( int k=0; k!=i; )
			{
				int j = k++;
				
				if( B.excess( L.get( j ) ) > 0 )
				{
					if( B.push( L.get( j ) ) )
					{
						mtf_mb( j );
						B.pop();
						move_to_front( j );
					}
				}
			}
		}
		
		private void pivot_mb( int i ) // double check
		{
			int t = 1;
			
			mtf_mb( t );
			
			double max_e, old_sqr_r = -1;
			
			MaxExcessData med = null;
			
			do
			{
				int pivot = -1;
				
				// TEMP
				//System.out.println( "t=" + t );
				//dumpL();
				
				med = max_excess( t, i );
				max_e = med.max_e;
				pivot = med.pivot;
				
				// TEMP
				//System.out.println( "\tt=" + t );
				//System.out.println( "\tsupport_end=" + support_end );
				//System.out.println( "\tmax_e=" + max_e );
				//System.out.println( "\tpivot=" + pivot );
				
				if( max_e > 0 )
				{
					t = support_end;
					
					// TEMP
					//System.out.println( "\tt=" + t );
					
					if( t == pivot )
					{
						// TEMP
						//System.out.println( "\tincrement t!" );
						
						++t;
					}
					
					// TEMP
					//System.out.println( "\tt=" + t );
					//System.out.println( "\tpivot=" + pivot );
					//System.out.println( "\tsupport_end=" + support_end );
					
					// save the reference t points to
					Vector3 ref = L.get( t );
					
					old_sqr_r = B.squared_radius();
					B.push( L.get( pivot ) );
					mtf_mb( support_end );
					B.pop();
					move_to_front( pivot );
					
					// since the list was rearranged, we need to figure out what t should be
					t = getIndexFromRef( ref );
				}
				
				
				// TEMP
				//System.out.println( "\tt=" + t );
				//System.out.println( "\tsupport_end=" + support_end );
			}
			while( ( max_e > 0 ) && ( B.squared_radius() > old_sqr_r ) );
		}
		
		/* TEMP
		private void dumpL( )
		{
			System.out.println( "L:" );
			for( int i=0; i<L.size(); i++ )
			{
				System.out.println( L.get( i ) );
			}
		}
		*/
		
		private int getIndexFromRef( Vector3 ref )
		{
			for( int n=0; n<L.size(); n++ )
			{
				if( L.get( n ) == ref ) // yeah, we want to compare references
				{
					return n;
				}
			}
			
			// just in case
			assert( false ) : "We didn't find an index for this reference!";
			
			// just to make the compiler happy
			return -1;
		}
		
		private void move_to_front( int j )
		{
			if( support_end == j )
			{
				support_end++;
			}
			
			// need to preserve the object support_end points to, not necessarily the index
			Vector3 ref = L.get( support_end );
			
			// move the element at j to the front
			Vector3 p = L.remove( j );
			L.add( 0, p );
			
			support_end = getIndexFromRef( ref );
		}
		
		private MaxExcessData max_excess( int t, int i ) // double check
		{
			// TEMP
			//System.out.println( "\tmax_excess\n\t\tt=" + t + "\n\t\ti=" + i );
			
			Vector3 c = B.center();
			double sqr_r = B.squared_radius();
			double e = 0.0;
			double max_e = 0.0;
			int pivot = 0;
			
			// TEMP
			//System.out.println( "\t\tcenter=" + c );
			//System.out.println( "\t\tradiussq=" + sqr_r );
			
			for( int k=t; k!=i; ++k )
			{
				Vector3 p = L.get( k );
				
				e = -sqr_r
					+ ( p.x - c.x ) * ( p.x - c.x )
					+ ( p.y - c.y ) * ( p.y - c.y )
					+ ( p.z - c.z ) * ( p.z - c.z );
				
				if( e > max_e )
				{
					max_e = e;
					pivot = k;
				}
			}
			return new MaxExcessData( max_e, pivot );
		}
	}
	
	private class MiniballB
	{
		/**************************
		 *   Data Members
		 **************************/
		
		private int m;
		private double[] q0 = new double[d];
		private double[] z = new double[d+1];
		private double[] f = new double[d+1];
		private double[][] v = new double[d+1][d];
		private double[][] a = new double[d+1][d];
		private double[][] c = new double[d+1][d];
		private double[] sqr_r = new double[d+1];
		private int current_c;
		private double current_sqr_r;
		

		/**************************
		 *   Constructors
		 **************************/
		
		public MiniballB( )
		{
			reset();
		}
		
		
		/**************************
		 *   Accessors
		 **************************/
		
		public Vector3 center( )
		{
			return new Vector3( c[current_c][0], c[current_c][1], c[current_c][2] );
		}
		
		public double squared_radius( )
		{
			return current_sqr_r;
		}
		
		public int size( )
		{
			return m;
		}
		
		public double excess( Vector3 p ) // double check
		{
			Vector3 c = center();
			return -current_sqr_r
				+ ( p.x - c.x ) * ( p.x - c.x )
				+ ( p.y - c.y ) * ( p.y - c.y )
				+ ( p.z - c.z ) * ( p.z - c.z );
		}
		
		
		/**************************
		 *   Methods
		 **************************/
		
		public void reset( )
		{
			m = 0;
			
			// we misuse c[0] for the center of the empty sphere
			for( int j=0; j<d; ++j )
			{
				c[0][j]=0;
			}
			current_c = 0;
			current_sqr_r = -1;
		}
		
		public boolean push( Vector3 p )
		{
			// TEMP
			//System.out.println( "\tpush\n\t\tp=" + p );
			//System.out.println( "\t\tcenter=" + center() );
			//System.out.println( "\t\tradiussq=" + current_sqr_r );
			
			int i, j;
			double eps = 1e-32;
			
			if( m == 0 )
			{
				for( i=0; i<d; ++i )
				{
					q0[i] = p.get( i );
				}
				for( i=0; i<d; ++i )
				{
					c[0][i] = q0[i];
				}
				sqr_r[0] = 0;
			}
			else
			{
				// set v_m to Q_m
				for( i=0; i<d; ++i )
				{
					v[m][i] = p.get( i )-q0[i];
				}
			
				// compute the a_{m,i}, i< m
				for( i=1; i<m; ++i )
				{
					a[m][i] = 0;
					for( j=0; j<d; ++j )
					{
						a[m][i] += v[i][j] * v[m][j];
					}
					a[m][i]*=(2/z[i]);
				}
				
				// update v_m to Q_m-\bar{Q}_m
				for( i=1; i<m; ++i )
				{
					for( j=0; j<d; ++j )
					{
						v[m][j] -= a[m][i] * v[i][j];
					}
				}
				
				// compute z_m
				z[m]=0;
				for( j=0; j<d; ++j )
				{
					z[m] += v[m][j] * v[m][j];
				}
				z[m]*=2;
				
				// reject push if z_m too small
				if( z[m] < eps * current_sqr_r )
				{
					// TEMP
					//System.out.println( "\t\tcenter=" + center() );
					//System.out.println( "\t\tradiussq=" + current_sqr_r );
					//System.out.println( "\t\treturn false!" );
					
					return false;
				}
				
				// update c, sqr_r
				double e = -sqr_r[m-1];
				for( i=0; i<d; ++i )
				{
					e += ( p.get( i ) - c[m-1][i] ) * ( p.get( i ) - c[m-1][i] );
				}
				f[m]=e/z[m];
				
				for( i=0; i<d; ++i )
				{
					c[m][i] = c[m-1][i]+f[m]*v[m][i];
				}
				sqr_r[m] = sqr_r[m-1] + e*f[m]/2;
			}
			
			current_c = m;
			current_sqr_r = sqr_r[m];
			m++;
			
			return true;
		}
		
		public void pop( )
		{
			--m;
		}
	}
	
	
	/**************************
	 *   Methods
	 **************************/
	
	public Sphere solve( Vector3[] points )
	{
		// check in all our points
		Miniball miniball = new Miniball();
		for( int i=0; i<points.length; i++ )
		{
			miniball.check_in( points[i] );
		}
		
		// run the actual algorithm
		miniball.build();
		
		return new Sphere( miniball.center(), Math.sqrt( miniball.squared_radius() ) );
	}
}
