/*************************************************************
/* Created by @kingbladeDev ( on twitter - https://twitter.com/kingbladeDev )
/* Handles Unity kinematic physics for objects that are able to push one another and are effected by eachother's weight
/************************************************************/


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

public class PhysicsObject : MonoBehaviour
{
    private static Vector2 GRAVITY = new Vector2(0, -600);

    [Range(0.0001f, Mathf.Infinity)] public float weight = 1;

    public Vector2 velocity;
    protected ContactFilter2D contactFilter;
    protected RaycastHit2D[] hitBuffer = new RaycastHit2D[64];

    public float gravityModifier = 1;

    protected Rigidbody2D rb2d;
    protected BoxCollider2D box;

    private Vector2 weightFromLastPush = Vector2.zero;

    void OnEnable()
    {
        rb2d = GetComponent<Rigidbody2D>();
        box = GetComponent<BoxCollider2D>();
    }

    void Start()
    {
        contactFilter.useTriggers = false;
        contactFilter.SetLayerMask(Physics2D.GetLayerCollisionMask(gameObject.layer));
        contactFilter.useLayerMask = true;
    }

    Vector2 CalculateDeltaFrame()
    {
        // Apply gravity
        velocity += gravityModifier * GRAVITY * Time.fixedDeltaTime;

        // calculate the delta position that should be moved this update
        Vector2 deltaPosition = velocity * Time.fixedDeltaTime;

        return deltaPosition;
    }


    void FixedUpdate()
    {

        // get how much you should move
        Vector2 averageMovement = CalculateDeltaFrame() * weight;
        Vector2 deltaPosition = averageMovement / (weight * Vector2.one + weightFromLastPush);
        weightFromLastPush = Vector2.zero;

        // Handle X movement
        Vector2 move = Vector2.right * deltaPosition.x;
        Movement(move, false);

        // Handle Y movement
        move = Vector2.up * deltaPosition.y;
        Movement(move, true);
    }

    class PushInfo
    {
        public PhysicsObject obj;
        public Vector2 motionOriginalPosition;
        public float distanceInsideWall = 0;
    }

    PushInfo GetPushInfo(List<PushInfo> rootMotionInfluenced, PhysicsObject obj, bool createOnNotExist = false)
    {
        foreach (PushInfo info in rootMotionInfluenced)
        {
            if (info.obj == obj)
            {
                return info;
            }
        }

        if (createOnNotExist)
        {
            PushInfo info = new PushInfo();
            info.obj = obj;
            info.motionOriginalPosition = obj.rb2d.position;
            rootMotionInfluenced.Add(info);
            return info;
        }

        return null;
    }

    void Movement(Vector2 move, bool yMove, List<PushInfo> rootMotionInfluenced = null)
    {
        float distance = move.magnitude;

        // check if we are the root of the movement
        bool root = false;
        if(rootMotionInfluenced == null) {
            root = true;
            rootMotionInfluenced = new List<PushInfo>();
        }

        // calculate the data for box casting.
        // it should shave a little bit of the box from the side to no collide with objects like a nearby wall when falling or the floor when moving to the side
        Vector2 boxSize = box.size * transform.lossyScale - new Vector2(yMove ? 0.1f : 0, yMove ? 0 : 0.1f);

        // cast for collision
        int count = Physics2D.BoxCastNonAlloc(rb2d.position, boxSize, 0, move.normalized, hitBuffer, move.magnitude, contactFilter.layerMask);
        float distanceInWall = 0;

        // run on hits
        for (int i = 0; i < count; i++)
        {
            // colliding with yourself is not a thing
            if (hitBuffer[i].transform == transform)
            {
                continue;
            }

            // since we are calling this function once for x-movement and one for y-movement
            // we want to make sure that the collision is in the right direction, otherwise we might get blocked
            // by a collision that is to our left of us when we want to move to the right
            if (hitBuffer[i].normal * -1 != move.normalized)
            {
                continue;
            }

            // calculate potential push distance (with shell distance)
            float pushDistance = Mathf.Max(0, distance - hitBuffer[i].distance);

            // if possible, push the object
            PhysicsObject other = hitBuffer[i].rigidbody.GetComponent<PhysicsObject>();
            if (other != null)
            {
                // if the object was already pushed by this root motion, skip it
                if (GetPushInfo(rootMotionInfluenced, other) == null)
                {
                    // add to the 'moved' list
                    GetPushInfo(rootMotionInfluenced, other, true);

                    // push instead of being pushed :D
                    other.Push(pushDistance * move.normalized, yMove, rootMotionInfluenced);
                }
            }
            // if not possible, we are against a wall
            else
            {
                // calculate the maximum distance we are inside a wall
                distanceInWall = Mathf.Max(pushDistance, distanceInWall);
            }
        }

        // add to the pushing list
        PushInfo pushedEntry = GetPushInfo(rootMotionInfluenced, this, true);
        pushedEntry.distanceInsideWall = Mathf.Max(distanceInWall, pushedEntry.distanceInsideWall);

        // move the object
        rb2d.position = Round(rb2d.position + move); //.normalized * distance;

        // if we are the root, fix the collisions
        if (root)
        {
            float totalWeight = 0;
            float maxDistanceInsideWall = 0;
            foreach (PushInfo i in rootMotionInfluenced)
            {
                totalWeight += i.obj.weight;
                maxDistanceInsideWall = Mathf.Max(maxDistanceInsideWall, i.distanceInsideWall);

                // add wight we are pushing to the calculation for next frame
                if (i.obj != this)
                {
                    weightFromLastPush += i.obj.weight * new Vector2(yMove ? 0 : 1, yMove ? 1 : 0);
                }
            }

            // reposition the pushed objects
            foreach (PushInfo i in rootMotionInfluenced)
            {
                Vector2 objMoved = i.obj.rb2d.position - i.motionOriginalPosition;
                i.obj.rb2d.position = Round(i.motionOriginalPosition + objMoved);
                Vector2 setBackByWall = move.normalized * maxDistanceInsideWall;

                // set the objects back as they should
                Vector2 positionAfterSetback = i.obj.rb2d.position - setBackByWall;
                if ((positionAfterSetback - i.motionOriginalPosition).normalized != move.normalized)
                {
                    i.obj.rb2d.position = Round(i.motionOriginalPosition);
                }
                else
                {
                    i.obj.rb2d.position = Round(positionAfterSetback);
                }

                // set speed for the original object or it's connected objects
                if (i.obj.rb2d.position - i.motionOriginalPosition == Vector2.zero && i.obj == this)
                {
                    if (yMove)
                    {
                        i.obj.velocity.y = 0;
                    }
                    else
                    {
                        i.obj.velocity.x = 0;
                    }
                }
            }
        }
    }

    private void Push(Vector2 movement, bool yMove, List<PushInfo> rootMotionInfluenced)
    {
        // Handle X movement
        if (!yMove)
        {
            Movement(new Vector2(movement.x, 0), false, rootMotionInfluenced);
        }
        // Handle Y movement
        else
        {
            Movement(new Vector2(0, movement.y), true, rootMotionInfluenced);
        }
    }

    Vector2 Round(Vector2 v, float digits = 2)
    {
        float mul = Mathf.Pow(10, digits);
        return new Vector2(Mathf.Round(mul * v.x), Mathf.Round(mul * v.y)) / mul;
    }
}