/*-
 *  Copyright 2020 Beuth Hochschule für Technik Berlin, Hochschule für Technik Stuttgart
 * 
 *  This file is part of CityDoctor2.
 *
 *  CityDoctor2 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 3 of the License, or
 *  (at your option) any later version.
 *
 *  CityDoctor2 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 CityDoctor2.  If not, see <https://www.gnu.org/licenses/>.
 */
package de.hft.stuttgart.citydoctor2.math;

import java.util.List;

/**
 * Utility class for calculating covariance matrices
 * 
 * @author Matthias Betz
 *
 */
public class CovarianceMatrix {

	private CovarianceMatrix() {

	}

	/**
	 * Calculates the covariance matrix of the given points, with the given expected
	 * values.
	 * 
	 * @param vertices the vertices for which the matrix is calculated
	 * @param expected the expected values
	 * @return the covariance 3 x 3 matrix
	 */
	public static double[][] calculateCovarianceMatrix(List<? extends Vector3d> vertices, double[] expected) {
		if (expected.length != 3) {
			throw new IllegalArgumentException("for 3D points 3 expected values have to be provided");
		}
		double[][] covValues = new double[3][3];
		for (Vector3d v : vertices) {
			double xdiff = v.getX() - expected[0];
			double ydiff = v.getY() - expected[1];
			double zdiff = v.getZ() - expected[2];
			covValues[0][0] += xdiff * xdiff;
			covValues[0][1] += xdiff * ydiff;
			covValues[0][2] += xdiff * zdiff;
			covValues[1][1] += ydiff * ydiff;
			covValues[1][2] += ydiff * zdiff;
			covValues[2][2] += zdiff * zdiff;
		}
		covValues[0][0] /= vertices.size();
		covValues[0][1] /= vertices.size();
		covValues[0][2] /= vertices.size();
		covValues[1][1] /= vertices.size();
		covValues[1][2] /= vertices.size();
		covValues[2][2] /= vertices.size();
		// the covariance matrix is symmetric, so we can fill in the remaining values
		covValues[1][0] = covValues[0][1];
		covValues[2][0] = covValues[0][2];
		covValues[2][1] = covValues[1][2];
		return covValues;
	}

	/**
	 * see {@link CovarianceMatrix#calculateCovarianceMatrix(List, double[])}
	 * 
	 * @param vertices the vertices for which the matrix is calculated
	 * @param expected the expected values as a vector
	 * @return the covariance 3 x 3 matrix
	 */
	public static double[][] calculateCovarianceMatrix(List<? extends Vector3d> vertices, Vector3d expected) {
		return calculateCovarianceMatrix(vertices, expected.getCoordinates());
	}

	public static Vector3d getCentroid(List<? extends Vector3d> vertices) {
		if (vertices.isEmpty()) {
			throw new IllegalArgumentException("Vertices may not be empty to calculate centroid");
		}
		// Move vertices to origin to avoid numeric instabilities
		Vector3d start = vertices.get(0);
		// get the center of all points
		double meanX = 0.0;
		double meanY = 0.0;
		double meanZ = 0.0;
		for (Vector3d v : vertices) {
			meanX += v.getX() - start.getX();
			meanY += v.getY() - start.getY();
			meanZ += v.getZ() - start.getZ();
		}
		meanX = meanX / vertices.size() + start.getX();
		meanY = meanY / vertices.size() + start.getY();
		meanZ = meanZ / vertices.size() + start.getZ();
		return new Vector3d(meanX, meanY, meanZ);
	}

}