////////////////////////////////////////////////////////////////////////////////
//																			  //
// AUTHOR:      Tevfik Metin Sezgin	                        				  //
//				Massachusetts Institute of Technology						  //
//				Department of Electrical Engineering and Computer Science	  //
//				Artificial Intelligence Laboratory							  //
//																			  //
// E-MAIL:		mtsezgin@ai.mit.edu, mtsezgin@mit.edu						  //
//																			  //
// COPYRIGHT:   Tevfik Metin Sezgin                    						  //
//              All rights reserved. This code can not be copied, modified,   //
//              or distributed in whole or partially without the written 	  //
//				permission of the author. Also see the COPYRIGHT file.		  //
//																			  //
////////////////////////////////////////////////////////////////////////////////

/**
  *
  * See the end of the file for the log of changes.
  *
  * $Author: mtsezgin $
  * $Date: 2000/06/08 03:21:34 $
  * $Revision: 1.1 $
  * $Headers$
  * $Id: LoadStoreModule.java,v 1.1 2000/06/08 03:21:34 mtsezgin Exp $
  * $Name:  $
  * $Locker:  $
  * $Source: /mit/mtsezgin/CVS/Repository/REC/LoadStoreModule.java,v $
  *
  **/


/**
  *
  * This class has useful functions for linear line matching.
  * Some adapted from numerical recipes book.
  *
  **/


class LinearFit
{
    /**
    * Constant corresponding to deriving direction using simple
	* tangents.
    **/
	public static final int SIMPLE_TANGENTS_METHOD = 0;
	

    /**
    * Constant corresponding to deriving direction using 
	* sliding window and the ODR package.
    **/
	public static final int SWODR_METHOD = 1;
	

    /**
    * Constant corresponding to deriving direction using 
	* sliding window and rotation mehod.
    **/
	public static final int ROTATION_METHOD = 2;
	

    /**
    *
    * Do a linear fit. Return a and b in an array. (y = ax+b)
    *
    **/
    public static double[] 
	fit( double x[], double y[] )
	{
		int    i;
		double t;
		double sxoss;
		double ss;
		
    	int    ndata = x.length;
		double a     = 0.0;
		double b     = 0.0;
		double sx    = 0.0;
		double sy    = 0.0;
		double st2   = 0.0;
		
		for ( i=0; i<ndata; i++ ) {
			sx += x[i];
			sy += y[i];
		}

		ss    = ndata;
		sxoss = sx/ss;
		
		for ( i=0; i<ndata; i++ ) {
			t    = x[i] - sxoss;
			st2 += t*t;
			a   += t*y[i];
		}

		a /= st2;
		b  = (sy-sx*a)/ss;
		
		double result[] = new double[2];
		
		result[0] = a;
		result[1] = b;
		return result;
	}

    /**
    *
    * Return the angle -- in radians -- that best approximates
    * the slope of the line that best fits x, y data points.
	*
    **/
    public static double
	findAngle( double x[], double y[] )
	{
		int center_x = 0;
		int center_y = 0;
		int int_x[]  = new int[x.length];
		int int_y[]  = new int[y.length];
		for ( int i=0; i<x.length; i++ ) {
			center_x += x[i];
			center_y += y[i];
		}
		center_x /= x.length;
		center_y /= y.length;
		for ( int i=0; i<x.length; i++ ) {
			int_x[i] = (int)(x[i]-center_x);
			int_y[i] = (int)(y[i]-center_y);
		}
		
		Polygon polygon = new Polygon( int_x, int_y, x.length );
		
		
		int    partitions = 360; // # of partitions we divide a quadrant
		double min_angle  = -Math.PI/2;
		double error      = 0.0;
		double min_error  = Double.MAX_VALUE;
		double distance   = 0.0;
		double y_average  = 0.0;

		Polygon rotated_polygon;
		for ( int i = -partitions; i<partitions; i++ ) {
			rotated_polygon = new Polygon( polygon );
			// Go from -PI/2 to  +PI/2
			rotated_polygon.rotate( Math.PI/((double)partitions*2.0)*i );
							
			error     = 0.0;												 
			y_average = 0.0;															 
			for ( int j=0; j<rotated_polygon.npoints; j++ ) {
				y_average += rotated_polygon.ypoints[j];
			}
			y_average /= rotated_polygon.npoints;
			for ( int j=0; j<rotated_polygon.npoints; j++ ) {
				distance = y_average - rotated_polygon.ypoints[j];
				error += distance*distance;
			}
			if ( error < min_error ) {
				min_error = error;
				min_angle = -Math.PI/((double)partitions*2.0)*i;
			}
		}

		return min_angle;
	}
	

    /**
    *
    * Do a linear fit. Return a and b in an array. (y = ax+b)
    *
    **/
    public static double[] 
	fit2( double x[], double y[] )
	{
		double sxx = 0.0;
		double syy = 0.0;
		double sxy = 0.0;
		double xa  = 0.0;
		double ya  = 0.0;
		
		for ( int i=0; i<x.length; i++ ) {
			xa += x[i];
			ya += y[i];
		}
		xa /= x.length;
		ya /= y.length;
		
		for ( int i=0; i<x.length; i++ ) {
			sxx += (x[i]-xa)*(x[i]-xa);
			syy += (y[i]-ya)*(y[i]-ya);
			sxy += (x[i]-xa)*(y[i]-ya);
		}

		double result[] = new double[2];
		
		result[0] = sxy/sxx;
		result[1] = ya-result[0]*xa;
		return result;
	}
	

    /**
    *
    * Do a linear fit. Return then angle that the fitted line makes
	* with the x axis. 
    *
    **/
    public static double
	findAngleViaODR( double x[], double y[] )
	{
		return Math.atan( 
					(OrthogonalDistanceRegression.doODR( "parameters",
														 "result",
														 x,
														 y ))[0] );
	}
	

    /**
    *
    * Return a String giving info about the fit method.
    *
    **/
    public static String
	fitMethodToString( int fit_method )
	{
		switch( fit_method ) {
			case LinearFit.SIMPLE_TANGENTS_METHOD :
				return "simple tangents method";
				
			case LinearFit.SWODR_METHOD :
				return "sliding window ODR method";
				
			case LinearFit.ROTATION_METHOD :
				return "rotational ODR method";
				
			default :
				return "unknown fit method";
		}
	}
}
