using System.Collections.Generic;
using System.Linq;
using UnityEngine;

public class Particle : MonoBehaviour
{
    public float Mass;
    public Vector3 Velocity;
    public Vector3 Force;
    public Vector3 Position
    {
        get { return transform.position; }
        set { transform.position = value; }
    }

    public bool IsMovable = true;

    public static float StructuralSpringConstant;
    public static float ShearSpringConstant;
    public static float BendSpringConstant;
    public static float DragDampingConstant;
    public static bool RK4 = true;
    public static bool showSphere = false;

    public List<GameObject> StructuralNeighbors;
    public List<GameObject> ShearNeighbors;
    public List<GameObject> BendPerpendicularNeighbors;
    public List<GameObject> BendDiagonalNeighbors;

    private void FixedUpdate()
    {
        if (IsMovable)
        {
            if (RK4)
            {
                OdeIntRK4();
            }
            else
            {
                OdeIntRK2();
            }
        }

        GetComponent<MeshRenderer>().enabled = showSphere;
    }

    public void OdeIntRK2()
    {
        Force = CalculateForce(Velocity, transform.position);

        var k1v = Time.fixedDeltaTime * Force / Mass;
        var k1x = Time.fixedDeltaTime * Velocity;

        Force = CalculateForce(Velocity + k1v / 2f, transform.position + k1x / 2f);

        var k2v = Time.fixedDeltaTime * Force / Mass;
        var k2x = Time.fixedDeltaTime * k1v / 2f;

        Velocity += k2v;
        Position += k2x;
    }

    public void OdeIntRK4()
    {
        Force = CalculateForce(Velocity, transform.position);

        var k1v = Time.fixedDeltaTime * Force / Mass;
        var k1x = Time.fixedDeltaTime * Velocity;

        Force = CalculateForce(Velocity + k1v / 2f, transform.position + k1x / 2f);

        var k2v = Time.fixedDeltaTime * Force / Mass;
        var k2x = Time.fixedDeltaTime * k1v / 2f;

        Force = CalculateForce(Velocity + k2v / 2f, transform.position + k2x / 2f);

        var k3v = Time.fixedDeltaTime * Force / Mass;
        var k3x = Time.fixedDeltaTime * k2v / 2f;

        Force = CalculateForce(Velocity + k3v, transform.position + k3x);

        var k4v = Time.fixedDeltaTime * Force / Mass;
        var k4x = Time.fixedDeltaTime * k3v;

        Velocity += (k1v + 2 * k2v + 2 * k3v + k4v) / 6f;
        Position += (k1x + 2 * k2x + 2 * k3x + k4x) / 6f;
    }

    public Vector3 CalculateForce(Vector3 velocity, Vector3 position)
    {
        var restDistancePerpendicularMagnitude = Flag.Distance;
        var restDistanceDiagonalMagnitude = Mathf.Sqrt(2) * restDistancePerpendicularMagnitude;

        var gravityForce = Environment.Gravity * Mass;
        var drag = -DragDampingConstant * velocity.normalized * velocity.sqrMagnitude;
        
        var constraintForce = new Vector3();
        foreach (var neighbor in StructuralNeighbors)
        {
            var distance = position - neighbor.transform.position;
            var correction = distance.normalized * (distance.magnitude - restDistancePerpendicularMagnitude);
            constraintForce -= StructuralSpringConstant * correction;
        }
        foreach (var neighbor in ShearNeighbors)
        {
            var distance = position - neighbor.transform.position;
            var correction = distance.normalized * (distance.magnitude - restDistanceDiagonalMagnitude);
            constraintForce -= ShearSpringConstant * correction;
        }
        foreach (var neighbor in BendPerpendicularNeighbors)
        {
            var distance = position - neighbor.transform.position;
            var correction = distance.normalized * (distance.magnitude - 2 * restDistancePerpendicularMagnitude);
            constraintForce -= BendSpringConstant * correction;
        }
        foreach (var neighbor in BendDiagonalNeighbors)
        {
            var distance = position - neighbor.transform.position;
            var correction = distance.normalized * (distance.magnitude - 2 * restDistanceDiagonalMagnitude);
            constraintForce -= BendSpringConstant * correction;
        }

        return gravityForce + constraintForce + WindForce() + drag;
    }

    public void SetRK4()
    {
        RK4 = true;
    }

    public void SetRK2()
    {
        RK4 = false;
    }

    public void ToggleSpheres()
    {
        showSphere = !showSphere;
    }

    public static Vector3 Normal(Vector3 a, Vector3 b, Vector3 c)
    {
        return Vector3.Cross(b - a, c - a);
    }

    private Vector3 WindForce()
    {
        var c = StructuralNeighbors.Count;
        var forces = new List<Vector3>();
        for (int i = 0; i < c; i++)
        {
            var normal = Normal(transform.position, StructuralNeighbors[i % c].transform.position, StructuralNeighbors[(i + 1) % c].transform.position);
            forces.Add(normal * Vector3.Dot(normal.normalized, Environment.WindForce));
        }
        return new Vector3(forces.Select(f => f.x).Average(), forces.Select(f => f.y).Average(), forces.Select(f => f.z).Average()) / 2f;
    }
}
