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) {
            float totalDist = 0;
            
            //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) = rp.updateDistanceJoints();
                    totalDist += dist.distance;
                    //Debug.Log(go.name+": left dist: "+leftSurfaceDist+" right Dist: "+rightSurfaceDist);
                }
            }
            
            Debug.Log("Total distance for cable: "+totalDist);
        }
    }

    public static Vector2 TangentPointCircle(GameObject fixedPointObject, GameObject circleObject) {
        var roller = circleObject.GetComponent<RollerProperties>();
        var fixedTransform = fixedPointObject.GetComponent<RollerProperties>().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);

            if (roller.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");
    }
}