using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using ElmoMotionControlComponents.Drive.EASComponents;

namespace DriveDotNetRecording
{
    class Utils
    {
        public static bool MotorEnable(IDriveCommunication communication, bool enable)
        {
            string response;
            int desiredMO = (enable ? 1 : 0);
            string moCommand = string.Format("MO={0}", desiredMO);
            if (!SendCommand(communication, "MO", out response))
                return false;
            int moVal = int.Parse(response);
            if (moVal != desiredMO)
            {
                Console.WriteLine("Turning motor " + (enable ? "on" : "off"));
                if (!SendCommand(communication, moCommand, out response))
                    return false;
            }
            for (int i = 0; i < 20; i++) // Wait for the motor to enable (max. 10 secs)
            {
                if (!SendCommand(communication, "SO", out response))
                    return false;
                int soVal = int.Parse(response);
                if (soVal == desiredMO)
                    break;
                Thread.Sleep(500); // Wait 0.5 sec
            }

            return true;
        }

        public static bool SendCommand(IDriveCommunication communication, string command, out string response)
        {
            IDriveErrorObject errorObj;
            if (!communication.SendCommandAnalyzeError(command, out response, out errorObj, 500))
            {
                Console.WriteLine(string.Format("Error sending '{0}' command: {1}", command, GetErrorDesc(errorObj)));
                return false;
            }

            return true;
        }

        public static string GetErrorDesc(IDriveErrorObject errorObj)
        {
            if (errorObj.ErrorCode != 0)
                return string.Format("Drive error {0} ({1})", errorObj.ErrorCode, errorObj.ErrorDescription);
            else
                return string.Format("Library error {0}: ({1})", errorObj.LibraryErrorCode, errorObj.LibraryErrorDescription);
        }
    }
}