using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace VSI
{
    public class MotionGenerator
    {
        double[,] trajectoryBuffer;
        double[,] downloadBuffer;

        int vectorDownloadCount = 0;
        int currentDownloadCount = 0;
        
        int motionMilliseconds = 0;
        int currentMillisecond = 0;

        int targetFillLevel = 0;
        int currentFillLevel = 0;
        int maxFillLevel = 0;

        bool downloadQueued = false;
        volatile bool downloadEnabled = false;



        public int CurrentVectorIndex
        {
            get { return currentMillisecond; }
        }

        public bool MotionActive
        {
            get { return (currentMillisecond != 0) && (currentMillisecond != motionMilliseconds); }
        }

        public bool DownloadEnabled
        {
            get { return downloadEnabled; }
            set { downloadEnabled = value; }
        }

        public int VectorDownloadCount
        {
            get { return vectorDownloadCount; }
            set 
            {
                if (MotionActive)
                    return;

                vectorDownloadCount = value;
                downloadBuffer = new double[vectorDownloadCount, HiCONDLL.MAX_AXIS];
            }
        }

        public int TargetFillLevel
        {
            get { return targetFillLevel; }
            set { targetFillLevel = value; }
        }



        /// <summary>
        /// 
        /// </summary>
        /// <param name="vectorDownloadCount"></param>
        /// <param name="targetFillLevel"></param>
        public MotionGenerator(int vectorDownloadCount, int targetFillLevel)
        {
            this.VectorDownloadCount = vectorDownloadCount;
            this.TargetFillLevel = targetFillLevel;
        }


        /// <summary>
        /// 
        /// </summary>
        /// <param name="newBuffer"></param>
        public void SetBuffer(double[,] newBuffer)
        {
            if (MotionActive)
                return;

            trajectoryBuffer = newBuffer;
            Reset();
        }

        public void Reset()
        {
            if (trajectoryBuffer == null)
            {
                motionMilliseconds = 0;
            }
            else
            {
                motionMilliseconds = trajectoryBuffer.GetLength(0);
            }

            currentMillisecond = 0;
            downloadQueued = false;
            downloadEnabled = false;
        }


        /// <summary>
        /// 
        /// </summary>
        public void Update()
        {
            if (!downloadEnabled)
                return;

            if (currentMillisecond < motionMilliseconds)
            {
                if (!downloadQueued)
                {
                    //format download buffer
                    for (int vector = 0; vector < vectorDownloadCount; vector++)
                    {
                        for (int axis = 0; axis < HiCONDLL.MAX_AXIS; axis++)
                            downloadBuffer[vector, axis] = trajectoryBuffer[currentMillisecond, axis];

                        currentMillisecond++;
                        currentDownloadCount++;
                    }

                    downloadQueued = true;
                }


                HiCONDLL.vsiStatusGetMotionBufferFillLevel(ref currentFillLevel, ref maxFillLevel);

                if (currentFillLevel <= TargetFillLevel)
                {
                    HiCONDLL.ERROR error = HiCONDLL.vsiCmdLoadBufferedMotion(downloadBuffer, currentDownloadCount);

                    if (error != HiCONDLL.ERROR.NONE) //an error has occured, and we cannot continue with the motion
                        downloadEnabled = false;
                    else
                        downloadQueued = false;
                }
            }
        }




    }
}
