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;
    protected double beta = 0.5f; // v/c

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

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

    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 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)
    {
        isAutoMove = false;
        isPatrol = true;
        return SetVelecityToAbPosition(dest,v);
    }

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