// 
//     Kerbal Engineer Redux
// 
//     Copyright (C) 2014 CYBUTEK
// 
//     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 3 of the License, 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, see <http://www.gnu.org/licenses/>.
// 

using System;
using System.Collections.Generic;
using KerbalEngineer.VesselSimulator;

namespace KerbalEngineer
{
    // a (force, application point) tuple
    public class AppliedForce
    {
        private static readonly Pool<AppliedForce> pool = new Pool<AppliedForce>(Create, Reset);

        public Vector3d vector;
        public Vector3d applicationPoint;

        static private AppliedForce Create()
        {
            return new AppliedForce();
        }

        static private void Reset(AppliedForce appliedForce) { }

        static public AppliedForce New(Vector3d vector, Vector3d applicationPoint)
        {
            AppliedForce force = pool.Borrow();
            force.vector = vector;
            force.applicationPoint = applicationPoint;
            return force;
        }

        public void Release()
        {
            pool.Release(this);
        }


    }

	// This class was mostly adapted from FARCenterQuery, part of FAR, by ferram4, GPLv3
	// https://github.com/ferram4/Ferram-Aerospace-Research/blob/master/FerramAerospaceResearch/FARCenterQuery.cs
    // Also see https://en.wikipedia.org/wiki/Resultant_force

	// It accumulates forces and their points of applications, and provides methods for
    // calculating the effective torque at any position, as well as the minimum-torque net force application point.
    //
    // The latter is a non-trivial issue; there is a 1-dimensional line of physically-equivalent solutions parallel
    // to the resulting force vector; the solution closest to the weighted average of force positions is chosen.
	// In the case of non-parallel forces, there usually is an infinite number of such lines, all of which have
	// some amount of residual torque. The line with the least amount of residual torque is chosen.
	public class ForceAccumulator
	{
	    // Total force.
		private Vector3d totalForce = Vector3d.zero;
		// Torque needed to compensate if force were applied at origin.
		private Vector3d totalZeroOriginTorque = Vector3d.zero;

		// Weighted average of force application points.
		private WeightedVectorAverager avgApplicationPoint = new WeightedVectorAverager();

		// Feed an force to the accumulator.
		public void AddForce(Vector3d applicationPoint, Vector3d force)
		{
			totalForce += force;
			totalZeroOriginTorque += Vector3d.Cross(applicationPoint, force);
			avgApplicationPoint.Add(applicationPoint, force.magnitude);
		}

        public Vector3d GetAverageForceApplicationPoint() {
            return avgApplicationPoint.Get();
        }

        public void AddForce(AppliedForce force) {
            AddForce(force.applicationPoint, force.vector);
        }

		// Residual torque for given force application point.
		public Vector3d TorqueAt(Vector3d origin)
		{
			return totalZeroOriginTorque - Vector3d.Cross(origin, totalForce);
		}

        // Total force vector.
        public Vector3d GetTotalForce()
        {
            return totalForce;
        }

        // Returns the minimum-residual-torque force application point that is closest to origin.
        // Note that TorqueAt(GetMinTorquePos()) is always parallel to totalForce.
        public Vector3d GetMinTorqueForceApplicationPoint(Vector3d origin)
        {
            double fmag = totalForce.sqrMagnitude;
            if (fmag <= 0) {
                return origin;
            }

            return origin + Vector3d.Cross(totalForce, TorqueAt(origin)) / fmag;
        }

        public Vector3d GetMinTorqueForceApplicationPoint()
        {
            return GetMinTorqueForceApplicationPoint(avgApplicationPoint.Get());
        }

        public void Reset()
        {
            totalForce = Vector3d.zero;
            totalZeroOriginTorque = Vector3d.zero;
            avgApplicationPoint.Reset();
        }
	}
}