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


namespace VSI
{
    class MotionSequence
    {
        bool complete = false;
        public bool Complete
        {
            get { return complete; }
        }

        HiCON.ERROR result = HiCON.ERROR.NONE;
        public HiCON.ERROR Error
        {
            get { return result; }
        }


        public void Execute()
        {
            Action action = new Action(ExecuteSequence);
            action.BeginInvoke(null, null);
        }


        void ExecuteSequence()
        {
            double[] axisPositions = new double[3];

            axisPositions[0] = 2;
            axisPositions[1] = 2;
            axisPositions[2] = 0;
            result = HiCON.vsiCmdExecuteLinearMove(0x07, axisPositions, 120, 10, HiCON.MoveType.ABSOLUTE);
            if (result != HiCON.ERROR.NONE)
            {
                complete = true;
                return;
            }
            WaitForMotionDone();


            axisPositions[0] = -2;
            axisPositions[1] = 2;
            axisPositions[2] = 0;
            result = HiCON.vsiCmdExecuteArcMoveRadius(2, axisPositions, 2, 120, HiCON.CLOCKWISE);
            if (result != HiCON.ERROR.NONE)
            {
                complete = true;
                return;
            }
            WaitForMotionDone();


            axisPositions[0] = 2;
            axisPositions[1] = 2;
            axisPositions[2] = 0;
            result = HiCON.vsiCmdExecuteArcMoveRadius(2, axisPositions, 2, 120, HiCON.CLOCKWISE);
            if (result != HiCON.ERROR.NONE)
            {
                complete = true;
                return;
            }
            WaitForMotionDone();


            axisPositions[0] = -2;
            axisPositions[1] = 2;
            axisPositions[2] = 0;
            result = HiCON.vsiCmdExecuteArcMoveAngle(1, axisPositions, 180, 120, HiCON.CLOCKWISE);
            if (result != HiCON.ERROR.NONE)
            {
                complete = true;
                return;
            }
            WaitForMotionDone();


            axisPositions[0] = 2;
            axisPositions[1] = 2;
            axisPositions[2] = 0;
            result = HiCON.vsiCmdExecuteArcMoveAngle(1, axisPositions, 180, 120, HiCON.CLOCKWISE);
            if (result != HiCON.ERROR.NONE)
            {
                complete = true;
                return;
            }
            WaitForMotionDone();


            axisPositions[0] = 0;
            axisPositions[1] = 4;
            axisPositions[2] = 0;
            result = HiCON.vsiCmdExecuteArcMoveCenter(2, axisPositions, -2, 0, 120, HiCON.CLOCKWISE);
            if (result != HiCON.ERROR.NONE)
            {
                complete = true;
                return;
            }
            WaitForMotionDone();


            axisPositions[0] = 0;
            axisPositions[1] = 4;
            axisPositions[2] = 0;
            result = HiCON.vsiCmdExecuteArcMoveCenter(0, axisPositions, -2, 0, 120, HiCON.CLOCKWISE);
            if (result != HiCON.ERROR.NONE)
            {
                complete = true;
                return;
            }
            WaitForMotionDone();


            axisPositions[0] = 2;
            axisPositions[1] = 2;
            axisPositions[2] = 0;
            result = HiCON.vsiCmdExecuteArcMoveCenter(2, axisPositions, 0, -2, 120, HiCON.COUNTER_CLOCKWISE);
            if (result != HiCON.ERROR.NONE)
            {
                complete = true;
                return;
            }
            WaitForMotionDone();



            WaitForMotionDone();
            complete = true;
        }


        void WaitForMotionDone()
        {
            bool isMoving = true;
            while (true)
            {
                System.Threading.Thread.Sleep(100);
                if (HiCON.vsiStatusIsMoving(-1, ref isMoving) != HiCON.ERROR.NONE)
                    continue;

                if (!isMoving)
                    break;
            }
        }


    }
}
