using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using Assets.Scripts.Collision;
using System;

public class FlatlandMovement : MonoBehaviour
{
    //충돌처리
    [SerializeField]
    protected ObjectType objectType;
    [SerializeField]
    protected PresetType collisionPreset;

    protected CollisionType[] howToCollison;

    protected int cnt = 0;

    [SerializeField]
    protected double beta = 0.0f; // v/c

    protected MatrixBuilder<double> M = Matrix<double>.Build;
    protected VectorBuilder<double> V = Vector<double>.Build;

    public Vector3 alpha = new Vector3(0.0f, 0.0f, 0.0f); // proper acceleration
    public Vector3 v;
    public Vector3 orientation;
    Vector3 collisionforce;
    protected Vector3 startScale;

    public LevelManager levelManager;
    public double gamma = 1.0f;


    public double time = 0.0f;

    public GameObject theobject;

    public float mass = 10.0f;

    // 마우스로 이동할때 하용하는 변수들.
    bool isAutoMove = false;

    bool isPatrol = false;

    Vector3 nowDest;
    /// <summary>
    /// 앞으로 남은 목적지들.
    /// </summary>
    Queue<Vector3> dests = new Queue<Vector3>();
    /// <summary>
    /// 앞으로 남은 목적지 속력
    /// </summary>
    Queue<float> pathVelocitys = new Queue<float>();

    List<Hash128> collisions = new List<Hash128>();

    public ObjectType ObjectType { get { return objectType; } }

    protected virtual void Start()
    {
        Physics.IgnoreLayerCollision(0, 1);
        startScale = theobject.transform.localScale;

        switch (collisionPreset)
        {
            case PresetType.player:
                howToCollison = CollisionPreset.Player;
                break;
            case PresetType.normalobject:
                howToCollison = CollisionPreset.NormalObject;
                break;
            case PresetType.interactive:
                howToCollison = CollisionPreset.Interactive;
                break;
            case PresetType.wall:
                howToCollison = CollisionPreset.Wall;
                break;
            case PresetType.clock:
                howToCollison = CollisionPreset.Clock;
                break;
            case PresetType.etc:
                break;
            default:
                break;
        }
    }

    protected virtual void FixedUpdate()
    {
        if (isAutoMove)
        {

            Vector3 tmp = new Vector3(transform.position.x, 0, transform.position.z);
            if (SpaceLength(nowDest, transform.position) < 0.1f || SpaceInnerProduct(nowDest - tmp, v) < 0)
            {
                //목적지 근접 or 넘어가면
                //Debug.Log("dest" + nowDest);
                //Debug.Log("position" + transform.position);

                //넘어간만큼 보정.
                //TODO : 좀더 엄밀하게 수정 필요.
                //Vector3 nowpos = transform.position;
                //nowpos.x = nowDest.x;
                //nowpos.z = nowDest.z;

                //transform.position = nowpos;


                if (dests.Count >= 1)
                {
                    // 다음 목적지가 있을떄.
                    NextMove(); //이동한다.
                }
                else
                {
                    //다음목적지 없을때
                    v = Vector3.zero;
                    nowDest = Vector3.zero;
                    isAutoMove = false;
                }

            }
        }
    }

    protected virtual void Update()
    {
        
    }
    /// <summary>
    /// 시간축 y을 무시한
    /// 거리 
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <returns></returns>
    protected float SpaceLength(Vector3 a, Vector3 b)
    {
        return Mathf.Sqrt((a.x - b.x) * (a.x - b.x) + (a.z - b.z) * (a.z - b.z));
    }
    /// <summary>
    /// 시간축 y을 무시한
    /// 내적
    /// </summary>
    /// <param name="a">xy가 공간</param>
    /// <param name="b">xz가 공간</param>
    /// <returns></returns>
    protected float SpaceInnerProduct(Vector3 a, Vector3 b)
    {
        return a.x * b.x + a.z * b.z;
    }

    /// <summary>
    /// 여러 위치를 거쳐서 이동합니다.
    /// </summary>
    /// <param name="path">상대좌표 x z 가 공간</param>
    /// <param name="v"> 경로별 속력 0번은 0.0f </param>
    /// <param isPatrol="isPatrol"> 왔다갔다 여부 </param>
    public bool MoveToRetivePosition(List<Vector3> path, List<float> v,bool isPatrol = false)
    {
        if(!SetDestsAndVelecitysAsRelPositions(path, v))
        {
            return false;
        }
        // patrol
        this.isPatrol = isPatrol;

        //start move
        NextMove();

        return true;
    }

    /// <summary>
    /// 목적지로 바로 이동합니다.
    /// </summary>
    /// <param name="path">목적지 x z 가 공간</param>
    /// <param name="v">속력 0<v<1</param>
    public bool MoveToAbPosition(Vector3 dest, float v)
    {
        bool result = SetVelecityToAbPosition(dest, v);
        if (!result)
        {
            return false;
        }
        nowDest = dest;
        isAutoMove = true;
        return true;
    }
    /// <summary>
    /// 절대좌표 방향으로 무한히 날아갑니다
    /// </summary>
    /// <param name="dest"></param>
    /// <param name="v"></param>
    /// <returns></returns>
    public bool MoveInfinitelyToAbPosition(Vector3 dest, float v)
    {
        StartCoroutine(_AcceleratetoConstantVelocity(dest, v));
        return true;
    }

    public IEnumerator _AcceleratetoConstantVelocity(Vector3 dest, float v)
    {
        Vector3 acceleration = new Vector3(dest.x, 0f, dest.y);
        var tinterval = Constants.alphatinterval;

        acceleration = acceleration.normalized;

        var deltat = Time.fixedDeltaTime * tinterval;

        acceleration = (float)((Constants.c / deltat) * MathNet.Numerics.Trig.Asinh(v * Constants.Gamma(v * Constants.c))) * acceleration;
        // acceleration required to accelerate to v in deltat seconds: see https://en.wikiversity.org/wiki/Theory_of_relativity/Rindler_coordinates

        var startv = this.v;

        this.alpha = acceleration;

        for(var j = 0; j < tinterval; ++j)
        {
            yield return new WaitForFixedUpdate();
        }
        this.alpha = new Vector3(0, 0, 0);

        var atmp = (float)(v * Constants.c);

        double[] vtmp = { atmp * ((acceleration.x / acceleration.magnitude)), 0.0, atmp * ((acceleration.z / acceleration.magnitude)) };
        var deltavnaive = V.DenseOfArray(vtmp);

        double[] tmp = {Constants.c * Constants.Gamma(deltavnaive.L2Norm()),
                      deltavnaive[0] * Constants.Gamma(deltavnaive.L2Norm()),
                      deltavnaive[2] * Constants.Gamma(deltavnaive.L2Norm())};

        var deltav = V.DenseOfArray(tmp);

        if (startv.magnitude > 0.0f)
        {
            deltav = Constants.BoostMatrix(-startv) * deltav;
        }
        
        var tt = deltav[0] / Constants.c;

        Vector3 finaldeltav = new Vector3((float)(deltav[1] / tt), 0.0f, (float)(deltav[2] / tt));

        this.v = finaldeltav;


    }

    public IEnumerator _AcceleratetoConstantVelocityWithoutCorrection(Vector3 dest, float v)
    {
        Vector3 acceleration = new Vector3(dest.x, 0f, dest.z);
        var tinterval = Constants.alphatinterval;

        acceleration = acceleration.normalized;

        var deltat = Time.fixedDeltaTime * tinterval;

        acceleration = (float)((Constants.c / deltat) * MathNet.Numerics.Trig.Asinh(v * Constants.Gamma(v * Constants.c))) * acceleration;
        // acceleration required to accelerate to v in deltat seconds: see https://en.wikiversity.org/wiki/Theory_of_relativity/Rindler_coordinates

        this.alpha += acceleration;

        for (var j = 0; j < tinterval; ++j)
        {
            yield return new WaitForFixedUpdate();
        }
        this.alpha -= acceleration;

    }

        public bool SetVelecityToAbPosition(Vector3 dest, float v)
    {
        dest.y = 0;
        //속도와 목적지 설정.
        Vector3 tmp = dest - transform.position;
        tmp.y = 0;
        this.v = tmp.normalized * v * (float)Constants.c;
        return true;
    }

    private bool SetDestsAndVelecitysAsRelPositions(List<Vector3> path, List<float> v)
    {
        //Debug.Log("aa");
        foreach (float a in v)
        {
            if (a <= 0.0001f)
            {
                return false;
            }
        }
        if (path.Count >= 1)
        {
            dests = new Queue<Vector3>();
            //상대좌표 -> 절대좌표
            foreach (var a in path)
            {
                dests.Enqueue(new Vector3(transform.position.x + a.x, 0, transform.position.z + a.z));
            }

            pathVelocitys = new Queue<float>(v);
        }
        return true;
    }

    private void NextMove()
    {
        //Debug.Log("a"+dests.Count);
        var dest = dests.Dequeue();
        var vel = pathVelocitys.Dequeue();
        MoveToAbPosition(dest, vel);
        //Debug.Log("b"+dests.Count);
        if (isPatrol)
        {
            dests.Enqueue(dest);
            pathVelocitys.Enqueue(vel);
        }
    }

    /// <summary>
    /// 겹쳐지면 모든 프레임 호출
    /// </summary>
    /// <param name="collision"></param>
    public virtual void OnCollisionStaychild(Collision collision)
    {
        
    }
    /// <summary>
    /// 들어왔을때 호출
    /// </summary>
    /// <param name="collision"></param>
    public virtual void OnCollisionEnterchild(Collision collision)
    {

        if(collision.gameObject.GetComponent<ExtrudedMesh>() == null)
        {
            return;
        }

        if(collisions.IndexOf(collision.gameObject.GetComponent<ExtrudedMesh>().hash) != -1)
        {
            return;
        }

        //howToCollison

        collisions.Add(collision.gameObject.GetComponent<ExtrudedMesh>().hash);

        FlatlandMovement other = collision.transform.parent.GetComponent<FlatlandMovement>();

        CollisionType type = Array.Find(howToCollison, (x) => { return x.other == other.ObjectType; });

        if (!type.isCollision)
        {
            //충돌하지 않을시 충돌하지 않음.
            return;
        }

        Vector3 col = (other.v - this.v);

        var totalThreeMomentum = Constants.BoostMatrix(v) * other.GetThreeMomentum();
        
        var tmp = collision.GetContact(0).normal;
        tmp = new Vector3(tmp.x, 0, tmp.z);

        tmp = tmp.normalized;
        tmp = -tmp;

        Vector3 otherv = other.v;

        double[] tmpx = {0,tmp.x, tmp.z };
        double[] othertmpx = { 0, -tmp.x, -tmp.z };
        double[] tmpv = { Constants.Gamma(v.magnitude) * Constants.c, Constants.Gamma(v.magnitude) * v.x, Constants.Gamma(v.magnitude) * v.z };

        var direction = V.DenseOfArray(tmpx);
        var realdirection = V.DenseOfArray(othertmpx);
        var collisionvelocity = V.DenseOfArray(tmpv);

        if(otherv.magnitude > 0)
        {
            direction = Constants.BoostMatrix(otherv) * direction;
            collisionvelocity = Constants.BoostMatrix(otherv) * collisionvelocity;
        }
        if(v.magnitude > 0)
        {
            realdirection = Constants.BoostMatrix(v) * realdirection;
        }

        Vector3 realfinalforcedirection = new Vector3((float)realdirection[1], 0, (float)realdirection[2]);
        Vector3 finalcollisionforcedirection = new Vector3((float)direction[1], 0, (float)direction[2]);
        var finalcollisionvelocitygamma = collisionvelocity[0] / Constants.c;
        Vector3 finalcollisionvelocity = new Vector3((float)(collisionvelocity[1] / finalcollisionvelocitygamma), 0, (float)(collisionvelocity[2] / finalcollisionvelocitygamma));

        finalcollisionforcedirection = finalcollisionforcedirection.normalized;
        realfinalforcedirection = realfinalforcedirection.normalized;

        var cosine = SpaceInnerProduct(finalcollisionvelocity.normalized,finalcollisionforcedirection);
        var totalenergy = Constants.Gamma(finalcollisionvelocity.magnitude) * mass * Constants.c * Constants.c + other.mass * Constants.c * Constants.c;
        var initialmomentum = finalcollisionvelocitygamma * mass * finalcollisionvelocity.magnitude;

        var a = 4 * (
            (initialmomentum * Constants.c * cosine) * (initialmomentum * Constants.c * cosine)
             - totalenergy * totalenergy);
        var b = 4 * (
            (totalenergy * totalenergy * totalenergy)
            - (initialmomentum * Constants.c) * (initialmomentum * Constants.c) * totalenergy
            + (mass * mass * Constants.c * Constants.c * Constants.c * Constants.c) * totalenergy
            - (other.mass * Constants.c * Constants.c) * (other.mass * Constants.c * Constants.c) * totalenergy
            );
        var c = (2 * initialmomentum * initialmomentum * Constants.c * Constants.c * totalenergy * totalenergy)
            - (2 * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c * totalenergy * totalenergy)
            + (2 * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c * initialmomentum * initialmomentum * Constants.c * Constants.c)
            + (2 * other.mass * other.mass * Constants.c * Constants.c * Constants.c * Constants.c * totalenergy * totalenergy)
            - (2 * other.mass * other.mass * Constants.c * Constants.c * Constants.c * Constants.c * initialmomentum * initialmomentum * Constants.c * Constants.c)
            + (2 * other.mass * other.mass * Constants.c * Constants.c * Constants.c * Constants.c * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c)
            - (totalenergy * totalenergy * totalenergy * totalenergy)
            - (initialmomentum * initialmomentum * initialmomentum * initialmomentum * Constants.c * Constants.c * Constants.c * Constants.c)
            - (mass * mass * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c)
            - (other.mass * other.mass * other.mass * other.mass * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c)
            - (4 * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c * initialmomentum * initialmomentum * Constants.c * Constants.c * cosine * cosine);

        var resultEnergy = (-b + Math.Sqrt((b * b) - 4*(a)*(c)))/(2*a);

        if(resultEnergy < 0)
        {
            resultEnergy = (-b - Math.Sqrt((b * b) - 4 * (a) * (c))) / (2 * a); ;
        }

        resultEnergy = totalenergy - resultEnergy;

        var resultvgamma = resultEnergy / (mass * Constants.c * Constants.c);

        var resultvbetamagnitude = Math.Sqrt(1.0 - (1.0/resultvgamma));

        StartCoroutine(_AcceleratetoConstantVelocityWithoutCorrection(realfinalforcedirection,(float)resultvbetamagnitude));

        //Debug.Log(collisions.Count);
    }
    /// <summary>
    /// 나갔을때 호출
    /// </summary>
    /// <param name="collision"></param>
    public virtual void OnCollisionExitchild(Collision collision)
    {
        if (collision.gameObject.GetComponent<ExtrudedMesh>() == null)
        {
            return;
        }

        if (collisions.IndexOf(collision.gameObject.GetComponent<ExtrudedMesh>().hash) == -1)
        {
            return;
        }

        collisions.Remove(collision.gameObject.GetComponent<ExtrudedMesh>().hash);

        //alpha -= collisionforce;
        //collisionforce = new Vector3(0, 0, 0);
    }

    public double GetEnergy()
    {
        return Constants.Gamma(v.magnitude) * mass * Constants.c * Constants.c;
    }

    public Vector<double> GetThreeMomentum()
    {
        var twop = GetTwoMomentum();

        double[] tmp = { GetEnergy() / Constants.c, twop[0], twop[1] };

        var result = V.DenseOfArray(tmp);
        return result;
    }

    public Vector<double> GetTwoMomentum()
    {
        double[] tmp = {Constants.Gamma(v.magnitude) * mass * v.x, Constants.Gamma(v.magnitude) * mass * v.z};
        var result = V.DenseOfArray(tmp);

        return result;
    }
}
