using System;
using MrEModuleSdk.Devices.Mre3Ois;
using MrEModuleSdk.Devices.Registers;
using MrEModuleSdk.Dto;

namespace Mre3OisExample
{
    internal class Mre3OisExample
    {
        static void Main()
        {
            // Mre3OisSdkComDeviceController is the main customer entry point for MR-E-3 OIS devices.
            var controller = new Mre3OisSdkComDeviceController();

            try
            {
                Console.WriteLine("Starting MR-E-3 OIS example...");
                Console.Write("Enter the COM port number of the MR-E-3 OIS device: ");

                var comPortText = Console.ReadLine();
                if (!int.TryParse(comPortText, out var comPort))
                {
                    Console.WriteLine("Invalid COM port number.");
                    return;
                }

                if (!controller.Connect(comPort))
                {
                    throw new InvalidOperationException("Connection failed.");
                }

                Console.WriteLine("Connection successful.");
                Console.WriteLine();

                PrintBoardInformation(controller);
                EnsureDevicePowered(controller);
                PrintDeviceInformation(controller);
                RunOpenLoopExample(controller);
                RunClosedLoopStaticSetPointExample(controller);
                RunSignalGeneratorExample(controller);
                RunOpticalStabilizationExample(controller);
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Example failed: {ex.Message}");
            }
            finally
            {
                StopControlAndClearCurrents(controller);
                controller.Disconnect(true);
                Console.WriteLine();
                Console.WriteLine("Example finished. Press Enter to close.");
                Console.ReadLine();
            }
        }

        private static void PrintBoardInformation(Mre3OisSdkComDeviceController controller)
        {
            Console.WriteLine(
                $"Driver FW ID = {controller.Status.GetBoardFirmwareId()}\n" +
                $"SN = {controller.GetElectronicSerialNumber()}");
            Console.WriteLine($"{controller.Status.GetBoardFirmwareMajorVersion()}." +
                $"{controller.Status.GetBoardFirmwareMinorVersion()}." +
                $"{controller.Status.GetBoardFirmwareBuildVersion()} " +
                $"({controller.Status.GetBoardFirmwareId() / 100}-" +
                $"{controller.Status.GetBoardFirmwareBranch():D2}-" +
                $"{controller.Status.GetBoardFirmwareType()})");
        }

        private static void EnsureDevicePowered(Mre3OisSdkComDeviceController controller)
        {
            if (controller.BoardEeprom.GetDeviceDetectChannelMask() == 0)
            {
                // In OQC mode the device is not initialized automatically.
                controller.MiscellaneousFeatures.SetDevPower(1, EAxis.X);
                controller.MiscellaneousFeatures.SetDevPower(1, EAxis.Y);
            }
        }

        private static void PrintDeviceInformation(Mre3OisSdkComDeviceController controller)
        {
            var deviceModel = ReadDeviceModel(controller);

            Console.WriteLine($"Device connected = {deviceModel}");
            Console.WriteLine($"Product serial number = {controller.GetProductSerialNumber()}");
            Console.WriteLine($"Device temperature = {controller.TemperatureManager.ReadDeviceTemperature():F2} [°C]");
            Console.WriteLine(
                $"Device EEPROM version = {controller.DeviceEeprom.GetEepromVersion()}." +
                $"{controller.DeviceEeprom.GetEepromSubVersion()}");

            var errorFlags = controller.GetDeviceStatus();
            Console.WriteLine($"Device status entries = {errorFlags[0].ErrorType}");
            Console.WriteLine();
        }

        private static EDeviceModel ReadDeviceModel(Mre3OisSdkComDeviceController controller)
        {
            var registerBytes = controller.GetValue((int)EmrE3Registers.MiscFeaturesDeviceModel);
            return (EDeviceModel)registerBytes[3];
        }

        private static void RunOpenLoopExample(Mre3OisSdkComDeviceController controller)
        {
            Console.WriteLine("Open-loop control example");

            controller.OpticalStabilizationControl.RunControl(false);
            controller.SignalFlowManager.InputStage.StaticValue.WriteCurrent(EChannel.Channel0, 0.1f);
            controller.SignalFlowManager.InputStage.StaticValue.WriteCurrent(EChannel.Channel1, 0.1f);

            var x = controller.OpticalStabilizationControl.ReadFeedbackX();
            var y = controller.OpticalStabilizationControl.ReadFeedbackY();
            Console.WriteLine($"Open-loop angle position x={x:F3} deg, y={y:F3} deg");

            controller.SignalFlowManager.InputStage.StaticValue.WriteCurrent(EChannel.Channel0, 0.0f);
            controller.SignalFlowManager.InputStage.StaticValue.WriteCurrent(EChannel.Channel1, 0.0f);
            Console.WriteLine();
        }

        private static void RunClosedLoopStaticSetPointExample(Mre3OisSdkComDeviceController controller)
        {
            Console.WriteLine("Closed-loop static setpoint example");

            controller.OpticalStabilizationControl.RunControl(true);
            controller.OpticalStabilizationControl.SetInputSource(EOpticalStabilizationInputSource.StaticSetPoint);
            controller.OpticalStabilizationControl.WriteStaticSetPointX(0.5f);
            controller.OpticalStabilizationControl.WriteStaticSetPointY(0.5f);

            Console.WriteLine("Static setpoints applied: X=0.5 deg, Y=0.5 deg");
            Console.WriteLine();
        }

        private static void RunSignalGeneratorExample(Mre3OisSdkComDeviceController controller)
        {
            Console.WriteLine("Signal generator example");

            controller.OpticalStabilizationControl.SetInputSource(EOpticalStabilizationInputSource.SignalGenerator);

            foreach (var axis in new[] { EAxis.X, EAxis.Y })
            {
                controller.SignalFlowManager.InputStage.SignalGenerator.WriteAmplitude(axis, 0.25f);
                controller.SignalFlowManager.InputStage.SignalGenerator.WriteFrequency(axis, 1.0f);
                controller.SignalFlowManager.InputStage.SignalGenerator.WriteCycles(axis, -1);
                controller.SignalFlowManager.InputStage.SignalGenerator.SwitchRunning(axis, true);
            }

            controller.SignalFlowManager.InputStage.SignalGenerator.WritePhaseDelay(EAxis.Y, (float)(Math.PI / 2.0));

            Console.WriteLine("Signal generator started on X and Y axes.");
            Console.WriteLine();
        }

        private static void RunOpticalStabilizationExample(Mre3OisSdkComDeviceController controller)
        {
            Console.WriteLine("Optical stabilization tuning example");

            controller.OpticalStabilizationControl.SetInputSource(EOpticalStabilizationInputSource.Imu);
            controller.OpticalStabilizationControl.RunControl(true);

            SetTp1725HallPidVector(controller, EAxis.X, 4.5f, 1.0f, 0.5f);
            SetTp1725HallPidVector(controller, EAxis.Y, 4.5f, 1.0f, 0.5f);

            controller.OpticalStabilizationControl.FeedForwardKParX(0.5f);
            controller.OpticalStabilizationControl.FeedForwardTParX(-0.04f);
            controller.OpticalStabilizationControl.FeedForwardKParY(0.5f);
            controller.OpticalStabilizationControl.FeedForwardTParY(0.04f);

            Console.WriteLine("PID and feed-forward parameters updated.");
            Console.WriteLine();
        }

        private static void SetTp1725HallPidVector(
            Mre3OisSdkComDeviceController controller,
            EAxis axis,
            float pGain,
            float iGain,
            float dGain)
        {
            var register = axis == EAxis.X ? EMrE3VectorMap.AxisPidCoefX : EMrE3VectorMap.AxisPidCoefY;
            var values = new[] { pGain, iGain, dGain };
            var bytes = new byte[values.Length * sizeof(float)];

            for (var index = 0; index < values.Length; index++)
            {
                WriteSingleToBigEndianBytes(bytes, index * sizeof(float), values[index]);
            }

            if (!controller.SetVector((int)register, 0, bytes))
            {
                throw new InvalidOperationException($"Failed to write PID vector for axis {axis}.");
            }
        }

        private static void WriteSingleToBigEndianBytes(byte[] target, int offset, float value)
        {
            var raw = BitConverter.GetBytes(value);
            Array.Reverse(raw);
            Array.Copy(raw, 0, target, offset, sizeof(float));
        }

        private static void StopControlAndClearCurrents(Mre3OisSdkComDeviceController controller)
        {
            controller.OpticalStabilizationControl.RunControl(false);
            controller.SignalFlowManager.InputStage.SignalGenerator.SwitchRunning(EAxis.X, false);
            controller.SignalFlowManager.InputStage.SignalGenerator.SwitchRunning(EAxis.Y, false);
            controller.SignalFlowManager.InputStage.StaticValue.WriteCurrent(EChannel.Channel0, 0.0f);
            controller.SignalFlowManager.InputStage.StaticValue.WriteCurrent(EChannel.Channel1, 0.0f);
        }
    }
}
