///
/// Nanotec Nanolib example
/// Copyright (C) Nanotec GmbH & Co. KG - All Rights Reserved
///
/// This product includes software developed by the
/// Nanotec GmbH & Co. KG (http://www.nanotec.com/).
///
/// The Nanolib interface headers and the examples source code provided are 
/// licensed under the Creative Commons Attribution 4.0 Internaltional License. 
/// To view a copy of this license, 
/// visit https://creativecommons.org/licenses/by/4.0/ or send a letter to 
/// Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
///
/// The parts of the library provided in binary format are licensed under 
/// the Creative Commons Attribution-NoDerivatives 4.0 International License. 
/// To view a copy of this license, 
/// visit http://creativecommons.org/licenses/by-nd/4.0/ or send a letter to 
/// Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
///
/// This program is distributed in the hope that it will be useful,
/// but WITHOUT ANY WARRANTY; without even the implied warranty of
/// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 
///
/// <file_name>MotorFunctionsExample.cs</file_name>
///
/// <summary>Definition of motor specific functions</summary>
///
/// <date>29-10-2024</date>
///
/// <author>Michael Milbradt<author>
///

using MenuUtils;
using Nlc;

namespace MotorFunctionsExample
{
    public class MotorFunctions
    {
        /// <summary>
        /// Determine motor parameters and store them on the device.
        /// </summary>
        /// <param name="ctx">Menu context.</param>
        public static void MotorAutoSetup(Context ctx)
        {
            ctx.WaitForUserConfirmation = true;

            if (ctx.ActiveDevice.Equals(new DeviceHandle()))
            {
                StringUtils.HandleErrorMessage(ctx, "", "No active device set. Select an active device first.");
                return;
            }

            Console.WriteLine();
            Console.WriteLine(ctx.LightYellow);
            Console.WriteLine("Please note the following requirements for performing the auto-setup: ");
            Console.WriteLine("- The motor must be unloaded.");
            Console.WriteLine("- The motor must not be touched.");
            Console.WriteLine("- The motor must be able to rotate freely in any direction.");
            Console.WriteLine("- No NanoJ program may be running.");
            Console.WriteLine(ctx.Def);

            Console.Write("Do you want to continue? ");
            string result = ReadLine("[y/n] ", "y");
            if (result != "y")
            {
                return;
            }

            // Stop a possibly running NanoJ program
            ResultVoid writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x00, OdIndices.OdNanoJControl, 32);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during MotorAutoSetup: ", writeResult.getError());
                return;
            }

            // Switch the state machine to "voltage enabled"
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during MotorAutoSetup: ", writeResult.getError());
                return;
            }

            // Set mode of operation to auto-setup
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0xFE, OdIndices.OdModeOfOperation, 8);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during MotorAutoSetup: ", writeResult.getError());
                return;
            }

            // Switch on
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x07, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during MotorAutoSetup: ", writeResult.getError());
                return;
            }

            // Switch the state machine to "enable operation"
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x0F, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during MotorAutoSetup: ", writeResult.getError());
                return;
            }

            // Run auto setup
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x1F, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during MotorAutoSetup: ", writeResult.getError());
                return;
            }

            Console.WriteLine("Motor auto setup is running, please wait ...");

            // Wait until auto setup is finished, check status word
            while (true)
            {
                ResultInt readNumberResult = ctx.NanolibAccessor.readNumber(ctx.ActiveDevice, OdIndices.OdStatusWord);
                if (readNumberResult.hasError())
                {
                    StringUtils.HandleErrorMessage(ctx, "Error during MotorAutoSetup: ", readNumberResult.getError());
                    return;
                }

                // Finish if bit 12, 9, 5, 4, 2, 1, 0 set
                if ((readNumberResult.getResult() & 0x1237) == 0x1237)
                {
                    break;
                }
            }

            // Reboot current active device
            Console.WriteLine("Rebooting ...");
            ResultVoid rebootResult = ctx.NanolibAccessor.rebootDevice(ctx.ActiveDevice);
            if (rebootResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during MotorAutoSetup: ", rebootResult.getError());
                return;
            }
            Console.WriteLine("Motor auto setup finished.");
        }

        /// <summary>
        /// Function to demonstrate how to move a motor in profile velocity mode.
        /// </summary>
        /// <param name="ctx">Menu context.</param>
        public static void ExecuteProfileVelocityMode(Context ctx)
        {
            ctx.WaitForUserConfirmation = true;

            if (ctx.ActiveDevice.Equals(new DeviceHandle()))
            {
                StringUtils.HandleErrorMessage(ctx, "", "No active device set. Select an active device first.");
                return;
            }

            Console.WriteLine("This example lets the motor run in Profile Velocity mode ...");

            // Stop a possibly running NanoJ program
            ResultVoid writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x00, OdIndices.OdNanoJControl, 32);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }

            // Choose Profile Velocity mode
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x03, OdIndices.OdModeOfOperation, 8);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }

            // Set the desired speed in rpm (60)
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x3C, OdIndices.OdTargetVelocity, 32);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }

            // Switch the state machine to "operation enabled"
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x07, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x0F, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }
            Console.WriteLine("Motor is running clockwise ...");

            // Let the motor run for 3 seconds
            Thread.Sleep(3000);

            // Stop the motor
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }

            // Set the desired speed in rpm (60), now counterclockwise
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, -0x3C, OdIndices.OdTargetVelocity, 32);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }

            // Start the motor
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x0F, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }
            Console.WriteLine("Motor is running counterclockwise ...");

            // Let the motor run for 3 seconds
            Thread.Sleep(3000);

            // Stop the motor
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecuteProfileVelocityMode: ", writeResult.getError());
                return;
            }
        }

        /// <summary>
        /// Function to demonstrate how to move a motor in positioning mode.
        /// </summary>
        /// <param name="ctx">Menu context.</param>
        public static void ExecutePositioningMode(Context ctx)
        {
            ctx.WaitForUserConfirmation = true;

            if (ctx.ActiveDevice.Equals(new DeviceHandle()))
            {
                StringUtils.HandleErrorMessage(ctx, "", "No active device set. Select an active device first.");
                return;
            }

            Console.WriteLine("This example lets the motor run in Profile Position mode ...");

            // Stop a possibly running NanoJ program
            ResultVoid writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x00, OdIndices.OdNanoJControl, 32);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }

            // Choose Profile Position mode
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x01, OdIndices.OdModeOfOperation, 8);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }

            // Set the desired speed in rpm (60)
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x3C, OdIndices.OdProfileVelocity, 32);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }

            // Set the desired target position (36000)
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x8CA0, OdIndices.OdTargetPosition, 32);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }

            // Switch the state machine to "operation enabled"
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x07, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x0F, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }

            // Move the motor to the desired target position relatively
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x5F, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }
            Console.WriteLine("Motor is running clockwise until position is reached ...");
            while (true)
            {
                var readResult = ctx.NanolibAccessor.readNumber(ctx.ActiveDevice, OdIndices.OdStatusWord);
                if (readResult.hasError())
                {
                    StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", readResult.getError());
                    // Try to stop motor
                    ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
                    break;
                }

                if ((readResult.getResult() & 0x1400) == 0x1400)
                {
                    break;
                }
            }

            // Stop the motor
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }

            // Set the desired target position (-36000)
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, -0x8CA0, OdIndices.OdTargetPosition, 32);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }

            // State machine operation enabled
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x0F, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }

            // Move the motor to the desired target position relatively
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x5F, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }
            Console.WriteLine("Motor is running counterclockwise until position is reached ...");
            while (true)
            {
                var readResult = ctx.NanolibAccessor.readNumber(ctx.ActiveDevice, OdIndices.OdStatusWord);
                if (readResult.hasError())
                {
                    StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", readResult.getError());
                    // Try to stop motor
                    ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
                    break;
                }

                if ((readResult.getResult() & 0x1400) == 0x1400)
                {
                    break;
                }
            }

            // Stop the motor
            writeResult = ctx.NanolibAccessor.writeNumber(ctx.ActiveDevice, 0x06, OdIndices.OdControlWord, 16);
            if (writeResult.hasError())
            {
                StringUtils.HandleErrorMessage(ctx, "Error during ExecutePositioningMode: ", writeResult.getError());
                return;
            }
        }

        /// <summary>
        /// Helper function for MotorAutoSetup
        /// </summary>
        /// <param name="prompt">The prompt as string</param>
        /// <param name="defaultValue">The default value to take</param>
        /// <returns>Default value or input as string</returns>
        private static string ReadLine(string prompt, string defaultValue)
        {
            Console.Write(prompt);
            var input = Console.ReadLine();
            return string.IsNullOrEmpty(input) ? defaultValue : input.Trim();
        }
    }
}


