using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;

public class FlatlandMovement : MonoBehaviour
{
    protected bool toggle = false;
    protected bool grabbed = false;

    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;

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

    bool isPatrol = false;

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

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

    protected void FixedUpdate()
    {
        if (isAutoMove)
        {

            Vector3 tmp = new Vector3(transform.position.x, 0, transform.position.z);
            if (SpaceLength(nowDest, transform.position) < 0.1f || SpaceInnerPorduct(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 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 SpaceInnerPorduct(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 = { ((acceleration.x / acceleration.magnitude)), 0.0, ((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 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)
    {
        var tmp = collision.GetContact(0).normal;
        tmp = new Vector3(tmp.x, 0, tmp.z);
        tmp = tmp.normalized;
        tmp *= 100;
        collisionforce = tmp;
        alpha += collisionforce;
    }
    /// <summary>
    /// 나갔을때 호출
    /// </summary>
    /// <param name="collision"></param>
    public virtual void OnCollisionExitchild(Collision collision)
    {
        alpha -= collisionforce;
        collisionforce = new Vector3(0, 0, 0);
    }
}
