using System;
using System.Collections.Generic;
using UnityEngine;
using static System.Math;

/** Cable Joints algorithm based on Paper by Matthias Müller, Nuttapong Chentanez, Stefan Jeschke and Miles Macklin*/
public static class CableJointsAlgorithm {
    public static void TimeStep(List<GameObject> cableStartingPoints) {
        //handle each cable individually
        foreach (var cable in cableStartingPoints) {
            float totalDist = 0;
            
            //calculate new attachment points for all joints
            for (var go = cable; go != null; go = go.GetComponent<RollerProperties>().linkTo) {
                if (go.GetComponent<RollerProperties>().linkTo != null) {
                    var dist = go.GetComponent<DistanceJoint2D>();
                    var rp = go.GetComponent<RollerProperties>();
                    var (left, right) = rp.updateDistanceJoints();
                    rp.actualDistance = dist.ActualDistanceInWorld();

                    if (left.HasValue) {
                        dist.distance -= left.Value;
                    }
                    if (right.HasValue) {
                        dist.distance += right.Value;
                    }
                    
                    totalDist += dist.distance;
                    
                    //Debug.Log(go.name+": l: +"+left+" r: -"+right);
                }
            }
            
            //Debug.Log("Total distance for cable: "+totalDist);
        }
    }

    public static Vector2 TangentPointCircle(GameObject fixedPointObject, GameObject circleObject) {
        var fixedObject = fixedPointObject.GetComponent<RollerProperties>();
        var roller = circleObject.GetComponent<RollerProperties>();
        var fixedTransform = fixedObject.transform;
        var rollerTransform = roller.transform;

        //assume round rollers only
        var rollerRadius = rollerTransform.lossyScale.x / 2;

        Vector2 d = rollerTransform.Position2d() - fixedTransform.Position2d();

        var dLen = d.magnitude;
        if (dLen > rollerRadius) {
            float alpha;
            if (d.x >= 0) {
                alpha = (float)Asin(d.y / dLen);
            }
            else {
                alpha = (float)(PI - Asin(d.y / dLen));
            }

            float phi = (float)Asin(rollerRadius / dLen);

            //TODO: check if should be roller.clockwise
            if (fixedObject.clockwise) {
                alpha = (float)(alpha - PI / 2 - phi);
            }
            else {
                alpha = (float)(alpha + PI / 2 + phi);
            }

            var p1 = rollerTransform.Position2d() + rollerRadius * new Vector2((float)Cos(alpha), (float)Sin(alpha));

            return rollerTransform.InverseTransformPoint(p1);
        }

        throw new Exception("Overlapping Rollers Error");
    }

    public static (Vector2, Vector2) TangentCircleCircle(GameObject g1, GameObject g2) {
        var roller1 = g1.GetComponent<RollerProperties>();
        var roller2 = g2.GetComponent<RollerProperties>();
        var t1 = g1.transform;
        var t2 = g2.transform;

        //assume round rollers only
        var r1 = t1.lossyScale.x / 2;
        var r2 = t2.lossyScale.x / 2;

        Vector2 d = t2.Position2d() - t1.Position2d();
        float r = r1 + r2;
        if (roller1.clockwise == roller2.clockwise) {
            r = r2 - r1;
        }

        var dLen = d.magnitude;
        if (dLen > r) {
            float alpha;
            if (d.x >= 0) {
                alpha = (float)Asin(d.y / dLen);
            }
            else {
                alpha = (float)(PI - Asin(d.y / dLen));
            }

            float phi = (float)Asin(r / dLen); //TODO verify, paper says "|c|"

            float alpha1, alpha2;
            if (roller1.clockwise == roller2.clockwise) {
                if (roller1.clockwise) {
                    alpha1 = (float)(alpha - PI / 2 - phi);
                    alpha2 = (float)(alpha - PI / 2 + phi);
                }
                else {
                    alpha1 = (float)(alpha + PI / 2 + phi);
                    alpha2 = (float)(alpha + PI / 2 + phi);
                }
            }
            else {
                if (roller1.clockwise) {
                    alpha1 = (float)(alpha - PI / 2 + phi);
                    alpha2 = (float)(alpha + PI / 2 + phi);
                }
                else {
                    alpha1 = (float)(alpha + PI / 2 - phi);
                    alpha2 = (float)(alpha - PI / 2 - phi);
                }
            }

            var p1 = t1.Position2d() + r1 * new Vector2((float)Cos(alpha1), (float)Sin(alpha1));
            var p2 = t2.Position2d() + r2 * new Vector2((float)Cos(alpha2), (float)Sin(alpha2));

            return (t1.InverseTransformPoint(p1), t2.InverseTransformPoint(p2));
        }

        throw new Exception("Overlapping Rollers Error");
    }

    public static float CirclePointDistance(Vector2 p1, Vector2 p2, GameObject c) {
        Vector2 p1diff = c.transform.Position2d() - p1, p2diff = c.transform.Position2d() - p2;
        var circumference = c.transform.lossyScale.x;
        var angle1 = Math.Atan2(p1diff.y, p1diff.x);
        var angle2 = Math.Atan2(p2diff.y, p2diff.x);

        if (angle2 < -PI/2 && angle1 > PI/2) {
            angle2 += 2 * PI;
        }
        else if (angle1 < -PI/2 && angle2 > PI/2) {
            angle1 += 2 * PI;
        }
        
        var delta = angle2 - angle1;
        

        var dist = circumference * delta;

        if (!c.GetComponent<RollerProperties>().clockwise) {
            dist *= -1;
        }

        //Debug.Log(c.name+": angle1: "+angle1+" angle2: "+angle2+" -> delta: "+delta+"  circumferecne: "+circumference+" dist: "+dist);
        return (float) dist;
    }
}