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

public static class CableJointsAlgorithm {
    public static void TimeStep(List<Cable> cables) {
        //handle each cable individually
        foreach (var cable in cables) {
            //calculate new attachment points for all joints
            for(var go = cable.firstRoller; 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) = TangentCircleCircle(go, rp.linkTo);

                    //TODO: assumes circles are flat for sufficiently small timesteps
                    var leftSurfaceDist =
                        (go.transform.TransformPoint(dist.anchor) - go.transform.TransformPoint(left)).magnitude;
                    var rightSurfaceDist = (dist.attachedRigidbody.transform.TransformPoint(dist.connectedAnchor) -
                                            dist.attachedRigidbody.transform.TransformPoint(right)).magnitude;
                    
                    Debug.Log(go.name+": left dist: "+leftSurfaceDist+" right Dist: "+rightSurfaceDist);

                    dist.anchor = left;
                    dist.connectedAnchor = right;
                }
            }
        }
    }


    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");
    }
}