/*
 * Copyright (C) 2011-2015, Peter Abeles. All Rights Reserved.
 *
 * This file is part of Geometric Regression Library (GeoRegression).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package georegression.fitting.line;


import georegression.struct.line.LinePolar2D_F32;
import georegression.struct.point.Point2D_F32;

import java.util.List;

/**
 * Finds the best file line given a set of observations.
 *
 * @author Peter Abeles
 */
public class FitLine_F32 {

	/**
	 * 

	 * Computes the unweighted best fit line to the set of points using the polar line equation. The solution
	 * is optimal in the Euclidean sense, see [1] for more details.
	 * 
	 *
	 * 

	 * [1] K. Arras, R. Siegwart, "Feature Extraction and Scene Interpretation for Map-Based Navigation
	 * and Map Building" Proc. SPIE, Mobile Robotics XIII, Vol. 3210, 1997
	 * 
	 *
	 * @param points Set of points on the line.
	 * @param ret Storage for the line.  If null a new line will be declared.
	 * @return Best fit line
	 */
	public static LinePolar2D_F32 polar( List points , LinePolar2D_F32 ret ) {
		if( ret == null )
			ret = new LinePolar2D_F32();

		float meanX = 0;
		float meanY = 0;

		final int N = points.size();
		for( int i = 0; i < N; i++ ) {
			Point2D_F32 p = points.get(i);
			meanX += p.x;
			meanY += p.y;
		}
		meanX /= N;
		meanY /= N;

		float top = 0;
		float bottom = 0;

		for( int i = 0; i < N; i++ ) {
			Point2D_F32 p = points.get(i);
			float dx = meanX - p.x;
			float dy = meanY - p.y;

			top += dx*dy;
			bottom += dy*dy - dx*dx;
		}

		ret.angle = (float)Math.atan2(-2.0f*top , bottom)/2.0f;
		ret.distance = (float)( meanX*Math.cos(ret.angle) + meanY*Math.sin(ret.angle));

		return ret;
	}

	/**
	 * 

	 * Computes the weighted best fit line to the set of points using the polar line equation. The solution
	 * is optimal in the Euclidean sense, see [1] for more details.
	 * 
	 *
	 * 

	 * [1] K. Arras, R. Siegwart, "Feature Extraction and Scene Interpretation for Map-Based Navigation
	 * and Map Building" Proc. SPIE, Mobile Robotics XIII, Vol. 3210, 1997
	 * 
	 *
	 * @param points Set of points on the line.
	 * @param weights Weight for each point.  weights[i] ≥ 0
	 * @param ret Storage for the line.  If null a new line will be declared.
	 * @return Best fit line. or null if sum of weights is zero
	 */
	public static LinePolar2D_F32 polar( List points , float weights[] , LinePolar2D_F32 ret ) {

		final int N = points.size();
		float totalWeight = 0;
		for( int i = 0; i < N; i++ ) {
			totalWeight += weights[i];
		}

		if( totalWeight == 0 )
			return null;

		if( ret == null )
			ret = new LinePolar2D_F32();

		float meanX = 0;
		float meanY = 0;

		for( int i = 0; i < N; i++ ) {
			Point2D_F32 p = points.get(i);
			float w = weights[i];
			meanX += w*p.x;
			meanY += w*p.y;
		}
		meanX /= totalWeight;
		meanY /= totalWeight;

		float top = 0;
		float bottom = 0;

		for( int i = 0; i < N; i++ ) {
			Point2D_F32 p = points.get(i);
			float w = weights[i];
			float dx = meanX - p.x;
			float dy = meanY - p.y;

			top += w*dx*dy;
			bottom += w*(dy*dy - dx*dx);
		}
		top /= totalWeight;
		bottom /= totalWeight;

		ret.angle = (float)Math.atan2(-2.0f*top , bottom)/2.0f;
		ret.distance = (float)( meanX*Math.cos(ret.angle) + meanY*Math.sin(ret.angle));

		return ret;
	}
}