using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;
using PixelCrushers;
using UnityEngine.AI;
using Random = UnityEngine.Random;
using UnityEngine.Events;

namespace Beyond {

    public class AnimSequencer : Saver
    {
        [Serializable]
        public class SaveData
        {
            public State state;
            public int currentPoint;
            public int selectedIndex;
            public int targetIndex;
            public float timer;
            public Vector3 position;
            public Quaternion rotation;
        }
        
        public const float SEG_LENGTH = 10f;
        public enum State { IDLE, MOVING };
        public State mState = State.IDLE;
        [HideInInspector]
        public float mSeqTimer = 0f;
        [HideInInspector]
        public float mSpeed = 0.0f;
        public float mAcc = 1f;
        public float mMaxSpeed = 15f;
        
        float mSegmentDist = 0f;
       // public int points = 20;

        public float m_randomMotionFreq = 1.5f;
        public float m_randomMotionScale = 0.2f;
        public float m_followRate = 1.5f;
        // public float m_maxPlayerDist = 5f;
        // public float m_minPlayerDist = 2f;
        public float m_minTargetDistFromPlayer = 1f;
        public bool m_WaitForPlayer = true;
        public float m_maxDistanceFromPlayer = 10f;

        Vector3 m_randShift = Vector3.zero;

        public Transform m_objectToMove;
        bool m_forward = true;
        private Transform m_PlayerTransform;

        //[HideInInspector]
        //public Point[] mPoints;
        [HideInInspector]
        public SeqPoint[] mSeqPoints;

        [HideInInspector]
        public bool autoStart;
        public float mTargetDistanceTh = 0.1f;
        float mTimer = 0f;
//        Vector3 mDestPos;
        private int mSelectedIndex = -1;
        private bool m_IsInfrontOfPlayer = true;
        public int mTargetIndex = -1;
        int mCurrentPoint = -1;
        const int MAX_SEG = 100;
        const int MIN_SEG = 10;
        public float mMaxPlayerDistance = 100f;

        [Serializable]
        public class SeqPoint
        {
            public enum PointType { MOVING, STOP};

            public SeqPoint(Vector3 pos)
            {
                speed = 5f;
                acc = 3f;
                angularSpeed = 10f;
                animTime = 2f;

                this.pos = pos;
                length = -1.0f;
                totalLength = -1.0f;
                numSegments = 0;
                rot = Quaternion.identity;
            }


            public SeqPoint(SeqPoint point, Vector3 shift)
            {
                speed = 5f;
                acc = 3f;
                angularSpeed = 10f;
                animTime = 2f;

                pos = point.pos + shift;
                rot = point.rot;
                length = point.length;
                totalLength = point.totalLength;
                numSegments = point.numSegments;

            }

            public void OnReached()
            {
                if (!eventFired)
                {
                    eventFired = true;
                    onReached?.Invoke();
                }
            }

            [SerializeField]
            public float speed;
            [SerializeField]
            public float acc;
            [SerializeField]
            public float angularSpeed;
            [SerializeField]
            public float animTime;
            //[SerializeField]
            //public int targetPointIdx;
            public UnityEvent onReached;
            public PointType type = PointType.MOVING;
            public Vector3 pos;
            public Quaternion rot;
            public float length; //length of segment starting at this point
            public float totalLength; //total length excluding this segment
            public int numSegments;
            private bool eventFired = false;
        }

        IEnumerator GenerateRandomShift()
        {
            while (true)
            {
                m_randShift.x = Random.Range(-m_randomMotionScale, m_randomMotionScale);
                m_randShift.y = Random.Range(-m_randomMotionScale, m_randomMotionScale);
                m_randShift.z = Random.Range(-m_randomMotionScale, m_randomMotionScale);

                yield return new WaitForSeconds(m_randomMotionFreq);
            }
        }

        private void OnDisable()
        {
            Debug.Log("Disabling AnimSequencer.");
        }

        public override string RecordData()
        {
            SaveData d = new SaveData();
            d.position = transform.position;
            d.rotation = transform.rotation;
            d.state = mState;
            d.timer = mTimer;
            d.currentPoint = mCurrentPoint;
            d.selectedIndex = mSelectedIndex;
            d.targetIndex = mTargetIndex;
            return SaveSystem.Serialize(d);

        }

        public override void ApplyData(string s)
        {
            if (String.IsNullOrEmpty(s))
                return;
            SaveData d = SaveSystem.Deserialize<SaveData>(s);
            if (d == null)
                return;
            transform.position = d.position;
            transform.rotation = d.rotation;
            mState = d.state;
            mTimer = d.timer;
            mCurrentPoint = d.currentPoint;
            mSelectedIndex = d.selectedIndex;
            mTargetIndex = d.targetIndex;
        }

        void MoveTowardsRandom()
        {
            m_objectToMove.localPosition = Vector3.Lerp(m_randShift, m_objectToMove.localPosition, Mathf.Exp(-Time.deltaTime * m_followRate));

        }

        public int selectedIndex
        {
            get { return mSelectedIndex; }
            set { mSelectedIndex = value; }
        }

        // Use this for initialization
        void Reset()
        {
           // mPoints = new Point[2];
           // mPoints[0] = new Point(transform.position);
           // mPoints[1] = new Point(transform.position + new Vector3(0f, 0f, 1f));
            mSeqPoints = new SeqPoint[1];
            mSeqPoints[0] = new SeqPoint(transform.position);
            mSelectedIndex = -1;
        }
        void SetState(State state)
        {
            if (mState == state)
                return;
            mTimer = 0f;
            switch (state)
            {
                case State.IDLE:
                    break;
                case State.MOVING:
                    break;
            };
            mState = state;
        }

        IEnumerator FindClosestPointToPlayer()
        {
            int idx = -1;
            float minDist = -1f;
            float dist = 0f;
                
            while (true)
            {
                if (m_PlayerTransform == null)
                    break;
                yield return new WaitForSeconds(0.5f);
                minDist = float.PositiveInfinity;
                Vector3 pos = m_PlayerTransform.position;
                for (int i=0; i<mSeqPoints.Length; i++)
                {
                    dist = (pos - mSeqPoints[i].pos).magnitude;
                    if (dist > m_minTargetDistFromPlayer && minDist > dist)
                    {
                        mTargetIndex = i;
                        minDist = dist;
                    }
                }

            }
            yield return null;
        }

        private float CalcDistToPoint(int id)
        {
            Vector3 vec = m_PlayerTransform.position - mSeqPoints[id].pos;
            vec.y = 0f;
            return vec.magnitude;
        }

        private int FindClosestStopPoint(int currPoint, out float dist)
        {
            float minDist = float.PositiveInfinity; ;
            dist = 0f;
            int id = 0;
            for (int i = 0; i < mSeqPoints.Length; i++)
            {
                if (mSeqPoints[i].type != SeqPoint.PointType.STOP)
                    continue;
                if (i > currPoint + 1)
                    break;
                if (i < currPoint - 1)
                    continue;
                dist = CalcDistToPoint(i);
                if (minDist > dist)
                {
                    minDist = dist;
                    id = i;
                }
            }
            return id;
        }


        IEnumerator FindNextClosestPointToPlayer()
        {
            float[] dists = { float.PositiveInfinity , float.PositiveInfinity , float.PositiveInfinity };
            int[] ids = {0,0,0};
            Vector3[] dirVecs = { Vector3.zero, Vector3.zero, Vector3.zero };
            //int closestID = 0;
            //float closestDist = 0f;
            while (true)
            {
                if (m_PlayerTransform == null)
                    break;
                yield return new WaitForSeconds(0.5f);


                ids[1] = mTargetIndex;
                if (ids[1] < 0)
                    ids[1] = 0;

                //closestID = FindClosestStopPoint(ids[1], out closestDist);

                //find previous and next point to current point
                if (ids[1] > 0)
                {
                    for (int i = ids[1] - 1; i >= 0; i--)
                        if (mSeqPoints[i].type == SeqPoint.PointType.STOP)
                        {
                            ids[0] = i;
                            break;
                        }
                }
                else
                {
                    ids[0] = 0;
                }

                if (ids[1] < mSeqPoints.Length - 1)
                {
                    for (int i = ids[1] + 1; i < mSeqPoints.Length; i++)
                        if (mSeqPoints[i].type == SeqPoint.PointType.STOP)
                        {
                            ids[2] = i;
                            break;
                        }
                }
                else
                {
                    ids[2] = ids[1];
                }
                for (int i = 0; i < 3; i++)
                {
                    dirVecs[i] = mSeqPoints[ids[i]].pos - m_PlayerTransform.position;
                    dists[i] = dirVecs[i].magnitude;
                }
                //check if player if infront or behind me
                var fromPlayer = transform.position - m_PlayerTransform.position;
                var forward = mSeqPoints[ids[1]].pos - mSeqPoints[ids[0]].pos;
               // var forward = mSeqPoints[ids[2]].pos - mSeqPoints[ids[1]].pos;
                m_IsInfrontOfPlayer = Vector3.Dot(fromPlayer, forward) > 0f;

                //check if previous point meets conditions, if yes, set it as a target
                if (dists[0] < dists[1] && dists[0] > m_minTargetDistFromPlayer && Vector3.Dot(dirVecs[0], dirVecs[1]) > 0f)
                {
                    //move to the previous point
                    mTargetIndex = ids[0];
                    continue;
                }
                //check if next point meets conditions, if yes, set it as a target
                //if current & next points are infront of the player...
                if (Vector3.Dot(dirVecs[1], dirVecs[2]) > 0f)
                {
                    //if player is too close, move to the next point
                    if (dists[1] < m_minTargetDistFromPlayer || dists[2] < dists[1])
                    {
                        mTargetIndex = ids[2];
                        continue;
                    }
                }
                else //jump to the next
                {
                    mTargetIndex = ids[2];
                    continue;
                    //Debug.Log("FindNextClosestPointToPlayer: need breakpoint");
                }

            }
            yield return null;
        }

        IEnumerator FindNextPointToPlayer()
        {
            int idx = -1;
            float minDist = -1f;
            float dist = 0f;

            while (true)
            {
                if (m_PlayerTransform == null)
                    break;
                yield return new WaitForSeconds(0.5f);
                minDist = float.PositiveInfinity;
                Vector3 pos = m_PlayerTransform.position;
                Vector3 pForward = m_PlayerTransform.forward;
                for (int i = 0; i < mSeqPoints.Length; i++)
                {
                    if (mSeqPoints[i].type != SeqPoint.PointType.STOP)
                        continue;
                    Vector3 vec = mSeqPoints[i].pos - pos;
                    vec.y = 0f;
                    if (Vector3.Dot(vec, pForward) < 0f)
                        continue;
                    dist = (vec).magnitude;
                    if (dist > m_minTargetDistFromPlayer && minDist > dist)
                    {
                        mTargetIndex = i;
                        minDist = dist;
                    }
                }
                Debug.LogError("Target point: "+mTargetIndex);


            }
            yield return null;
        }
        void Start()
        {
            var p = Player.Instance;
            if (p)
            {
                m_PlayerTransform = p.transform;
                //StartCoroutine(FindClosestPointToPlayer());
//                StartCoroutine(FindNextPointToPlayer());
                StartCoroutine(FindNextClosestPointToPlayer());
            }
            else
            {
                Debug.LogError("Player not found!");
            }
            UpdatePointsList();
            if (mSeqPoints.Length > 1)
            {
                transform.position = mSeqPoints[0].pos;
                //selectedIndex = 0;
            }
            //if (mState == State.MOVING)
            {
                NextSequencePoint();
            }
            if (m_objectToMove)
                StartCoroutine(GenerateRandomShift());
        }

        public int Count
        {
            get
            {
                return mSeqPoints.Length;
            }
        }

        public SeqPoint this[int i]
        {
            get
            {
                return mSeqPoints[i];
            }
            set
            {
                mSeqPoints[i] = value;
            }
        }


        public Vector3 GetInterpolated(int idx, float t)
        {
            Vector3 p0, p3;
            if (idx == 0)
                p0 = mSeqPoints[idx].pos;
            else
                p0 = mSeqPoints[idx - 1].pos;

            if (idx + 2 < mSeqPoints.Length)
                p3 = mSeqPoints[idx + 2].pos;
            else
                p3 = mSeqPoints[idx + 1].pos;
            //return transform.TransformPoint(CatmullRom(p0, mPoints[idx].pos, mPoints[idx + 1].pos, p3, t));
            return CatmullRom(p0, mSeqPoints[idx].pos, mSeqPoints[idx + 1].pos, p3, t);
        }
        public float GetLength(int startIdx, float t0, float t1)
        {
            const float maxDist = .5f;
            Vector3 p0 = GetInterpolated(startIdx, t0);
            Vector3 p1 = GetInterpolated(startIdx, t1);
            float d = (p1 - p0).magnitude;
            if (d < maxDist)
                return d;
            else
                return GetLength(startIdx, t0, (t0 + t1) * 0.5f) + GetLength(startIdx, (t0 + t1) * 0.5f, t1);
        }

        public void UpdatePointsList()
        {
            int numPoints = mSeqPoints.Length;

            int cnt = 0;
            mSeqPoints[cnt].pos = transform.position;

            for (int i = 0; i < numPoints - 1; i++)
            {
                mSeqPoints[i].length = GetLength(i, 0f, 1f);
                mSeqPoints[i].totalLength = i == 0 ? 0f : mSeqPoints[i - 1].totalLength + mSeqPoints[i - 1].length;
            }
        }

        public void SetPointType(int selectedIndex, SeqPoint.PointType pointType)
        {
            mSeqPoints[selectedIndex].type = pointType;
        }

        public Quaternion GetInterpolatedRot(int idx, float t)
        {
            Quaternion p0, p3;
            if (idx == 0)
                p0 = mSeqPoints[idx].rot;
            else
                p0 = mSeqPoints[idx - 1].rot;

            if (idx + 2 < mSeqPoints.Length)
                p3 = mSeqPoints[idx + 2].rot;
            else
                p3 = mSeqPoints[idx].rot;
            return CatmullRom(p0, mSeqPoints[idx].rot, mSeqPoints[idx + 1].rot, p3, t);
        }


        public void AddSeqPoint()
        {
            if (mSelectedIndex < 0)
                mSelectedIndex = mSeqPoints.Length - 1;
            SeqPoint[] arr = mSeqPoints;
            mSeqPoints = new SeqPoint[arr.Length + 1];
            for (int i = 0; i <= mSelectedIndex; i++)
            {
                mSeqPoints[i] = arr[i];
            }


            Vector3 prevPoint = arr.Length > 0 ? arr[mSelectedIndex].pos : transform.position;

            if (mSelectedIndex == arr.Length - 1) //this is the last point
            {
                mSeqPoints[mSelectedIndex + 1] = new SeqPoint(prevPoint + transform.forward);
            }
            else
            {
                mSeqPoints[mSelectedIndex + 1] = new SeqPoint((prevPoint + arr[mSelectedIndex+1].pos)*0.5f);
                for (int i= mSelectedIndex + 2; i<mSeqPoints.Length; i++)
                {
                    mSeqPoints[i] = arr[i - 1];
                }
            }
            mSelectedIndex++;
        }

        public void RemoveSelected()
        {
            if (mSelectedIndex > -1)
            {
                SeqPoint[] arr = mSeqPoints;
                int cnt = 0;
                mSeqPoints = new SeqPoint[arr.Length - 1];
                for (int i = 0; i < arr.Length; i++)
                {
                    if (i != mSelectedIndex)
                    {
                        mSeqPoints[cnt++] = arr[i];
                    }
                }

                if (mSeqPoints.Length == mSelectedIndex)
                {
                    mSelectedIndex--;
                }
            }

            UpdatePointsList();
        }


        public void MoveSelectedUp()
        {

            if (mSelectedIndex > 0)
            {
                SeqPoint p = mSeqPoints[mSelectedIndex];
                mSeqPoints[mSelectedIndex] = mSeqPoints[mSelectedIndex - 1];
                mSeqPoints[mSelectedIndex - 1] = p;
            }
            UpdatePointsList();
        }

        public void MoveSelectedDown()
        {
            if (mSelectedIndex < mSeqPoints.Length - 1)
            {
                SeqPoint p = mSeqPoints[mSelectedIndex];
                mSeqPoints[mSelectedIndex] = mSeqPoints[mSelectedIndex + 1];
                mSeqPoints[mSelectedIndex + 1] = p;
            }
            UpdatePointsList();
        }

        Vector3 CatmullRom(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4, float s)
        {
            Vector3 res;
            float s2, s3;
            s2 = s * s;
            s3 = s2 * s;
            res = p1 * (-s3 + 2.0f * s2 - s) + p2 * (3.0f * s3 - 5.0f * s2 + 2.0f) + p3 * (-3.0f * s3 + 4.0f * s2 + s) + p4 * (s3 - s2);
            res /= 2.0f;
            return res;
        }

        Quaternion CatmullRom(Quaternion p1, Quaternion p2, Quaternion p3, Quaternion p4, float s)
        {
            Quaternion res;
            float s2, s3;
            s2 = s * s;
            s3 = s2 * s;
            res = p1.Mul(-s3 + 2.0f * s2 - s).Add(p2.Mul(3.0f * s3 - 5.0f * s2 + 2.0f).Add(p3.Mul(-3.0f * s3 + 4.0f * s2 + s).Add(p4.Mul(s3 - s2))));
            res.Mul(0.5f);
            return res;
        }

        public Vector3 GetWorldPos(int idx)
        {
            //        return transform.TransformPoint(mPoints[idx].pos);
            return mSeqPoints[idx].pos;
        }

        void NextSequencePoint()
        {
            mTimer = 0f;
            mSelectedIndex++;
            if (mSelectedIndex >= mSeqPoints.Length-1)
            {
                //SetState(State.IDLE);
                return;
            }
            SeqPoint point = mSeqPoints[mSelectedIndex];
            //mDestPos = point.pos;
            mSegmentDist = 0f;
        }

        void PrevSequencePoint()
        {
            mTimer = 0f;
            mSelectedIndex--;
            if (mSelectedIndex < 0)
            {
                //SetState(State.IDLE);
                return;
            }
            SeqPoint point = mSeqPoints[mSelectedIndex];
            //mDestPos = point.pos;
            mSegmentDist = 1f;
        }
        void OnGUI()
        {

        }

        // Project vector on plane
        Vector3 ProjectVectorOnPlane(Vector3 planeNormal, Vector3 vector)
        {
            return vector - (Vector3.Dot(vector, planeNormal) * planeNormal);
        }

        // Get signed vector angle
        float SignedVectorAngle(Vector3 referenceVector, Vector3 otherVector, Vector3 normal)
        {
            Vector3 perpVector;
            float angle;

            perpVector = Vector3.Cross(normal, referenceVector);
            angle = Vector3.Angle(referenceVector, otherVector);
            angle *= Mathf.Sign(Vector3.Dot(perpVector, otherVector));

            return angle;
        }


        float DistanceLeft(int pointID)
        {
            float dist = 0f;
            if (m_forward)
            {
                if (mSelectedIndex < mSeqPoints.Length - 1 && mSelectedIndex >= 0 && pointID > mSelectedIndex)
                {
                    dist = (1f - mSegmentDist) * mSeqPoints[mSelectedIndex].length;
                    for (int i = mSelectedIndex + 1; i < pointID; i++)
                    {
                        dist += mSeqPoints[i].length;
                    }
                }
            }
            else
            {
                if (mSelectedIndex >= 0 && mSelectedIndex < mSeqPoints.Length - 1 && pointID <= mSelectedIndex)
                {
                    dist = mSeqPoints[mSelectedIndex].length * mSegmentDist;
                    for (int i = pointID; i < mSelectedIndex; i++)
                    {
                        dist += mSeqPoints[i].length;
                    }
                }
            }
            return dist;
        }


        void MoveBy(float val)
        {
            const float th = 0.005f;
            float shift = Mathf.Abs(val);
            float direction = Mathf.Sign(val);
            int itter = 0;
            do
            {
                itter++;
                Vector3 currPos = transform.position;
                SeqPoint point;
                point = mSeqPoints[mSelectedIndex];

                mSegmentDist += direction * shift / point.length;

                if (mSegmentDist > 1f)
                {
                    float d = mSegmentDist - 1f;
                    var tmp = d * point.length;
                    NextSequencePoint();
                    mSegmentDist += tmp / point.length;
                }
                else if (mSegmentDist < 0f)
                {
                    float d = mSegmentDist;
                    var tmp = d * point.length;
                    PrevSequencePoint();
                    mSegmentDist -= tmp / point.length;
                }

                currPos = GetInterpolated(mSelectedIndex, mSegmentDist);
                float dist = (currPos - transform.position).magnitude;
                if (dist < shift)
                {
                    shift -= dist;
                }
                else
                {
                    shift = dist - shift;
                    direction *= -1f;
                }

                transform.position = currPos;
                //transform.position += (currPos - transform.position).normalized * Mathf.Abs(val);
            } while (shift > th && itter < 3);
            //Debug.Log("MoveBy iterrs: "+itter+" final shift: "+shift);
        }
        void Update()
        {
            if (mState != State.MOVING && autoStart)
            {
                SetState(State.MOVING);
                autoStart = false;
            }


            switch (mState)
            {
                case State.MOVING:
                   if (mSelectedIndex < mTargetIndex)
                    {
                        m_forward = true;
                    }
                   else
                    {
                        m_forward = false;
                    }
                    var dist = DistanceLeft(mTargetIndex);
                    //Debug.Log("AniSeq, moving, dist: "+dist+" target point: "+mTargetIndex+" sel idx: "+mSelectedIndex+" player: "+m_PlayerTransform.ToString());
                    if (dist < mTargetDistanceTh)
                    {
                        mCurrentPoint = mTargetIndex;
                        mSeqPoints[mCurrentPoint].OnReached();
                        SetState(State.IDLE);
                        break;
                    }

                    var breakingDist = mSpeed * mSpeed / (2f * mAcc);


                    if (dist > breakingDist)
                    {
                        if (mSpeed < mMaxSpeed)
                        {
                            mSpeed += mAcc * Time.deltaTime;
                            mSpeed = Mathf.Clamp(mSpeed, 0f, mMaxSpeed);
                        }
                        //slow down and wait for player
                        if (m_WaitForPlayer && m_IsInfrontOfPlayer && m_forward && m_PlayerTransform != null)
                        {
                            var d = (transform.position - m_PlayerTransform.position).magnitude;
                            float a = (d - m_minTargetDistFromPlayer) / m_maxDistanceFromPlayer;
                            mSpeed *= 1f - Mathf.Clamp01(a);
                        }
                    }
                    else
                    {
                        mSpeed -= mAcc * Time.deltaTime;
                        mSpeed = Mathf.Clamp(mSpeed, 0f, mMaxSpeed);
                    }
                    float shift = mSpeed * Time.deltaTime;
                    if (!m_forward)
                        shift *= -1f;
                    MoveBy(shift);


                    break;
                case State.IDLE:
                    if (mCurrentPoint >= 0 & mCurrentPoint < mSeqPoints.Length)
                        transform.position = Vector3.Lerp(transform.position, mSeqPoints[mCurrentPoint].pos, Mathf.Exp(-Time.deltaTime * m_followRate));
                    if (mCurrentPoint != mTargetIndex && mTimer > 3f)
                    {
                        SetState(State.MOVING);
                    }
                    break;
            }
            mTimer += Time.deltaTime;
            MoveTowardsRandom();

        }

    }
  
}