using MrEModuleSdk.Devices.Mre3;
using MrEModuleSdk.Dto;
using System.Collections.Generic;
using System;
using GeneralModuleCore.Dtos;

namespace Mre3Example
{
    public partial class Mre3Example
    {
        private static void RunVectorPatternTriggerExample(IMrE3SdkComDeviceController controller)
        {
            const int xPoints = 4;
            const int yPoints = 4;
            const double xyFactor = 40.0;
            const double offsetX = -0.2;
            const double offsetY = -0.2;

            Console.WriteLine("Vector pattern unit trigger example");
            Console.WriteLine("Builds a 4x4 grid in XY coordinates and loads it into the arbitrary vector memory.");
            Console.WriteLine();

            var gridPoints = BuildGrid(xPoints, yPoints);
            var xyPoints = ConvertGridToXy(gridPoints, xyFactor, offsetX, offsetY);
            var vectorToVpuMemory = BuildVectorPatternMemory(xyPoints);

            Console.WriteLine("Generated XY points:");
            foreach (var point in xyPoints)
            {
                Console.WriteLine($"[{point.Item1:F3}, {point.Item2:F3}]");
            }

            Console.WriteLine();
            Console.WriteLine("Vector payload:");
            Console.WriteLine(string.Join(", ", vectorToVpuMemory));
            Console.WriteLine();

            controller.SignalFlowManager.ControlModeStage.ChangeActiveSystem(EMrEControlModeSystems.ClosedLoopAngle, EAxis.X);
            controller.SignalFlowManager.ControlModeStage.ChangeActiveSystem(EMrEControlModeSystems.ClosedLoopAngle, EAxis.Y);

            controller.SignalFlowManager.InputStage.ArbitraryVector.SetUnitType(EAxis.X, EUnitType.XY);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetUnitType(EAxis.Y, EUnitType.XY);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetCycles(EAxis.X, 1);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetCycles(EAxis.Y, 1);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetExternalTrigger(EAxis.X, EExternalTriggerType.RisingEdge);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetExternalTrigger(EAxis.Y, EExternalTriggerType.RisingEdge);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetExternalTriggerGpio(EAxis.X, EExternalTriggerGpiox.Input);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetExternalTriggerGpio(EAxis.Y, EExternalTriggerGpiox.Input);
            controller.SignalFlowManager.InputStage.ChangeActiveSystem(EAxis.X, EmrEInputStageSystems.ArbitraryVector);
            controller.SignalFlowManager.InputStage.ChangeActiveSystem(EAxis.Y, EmrEInputStageSystems.ArbitraryVector);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetVectorValues(EAxis.X, vectorToVpuMemory.ToArray());
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetVectorValues(EAxis.Y, vectorToVpuMemory.ToArray());
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetStartIndex(EAxis.X, 0);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetEndIndex(EAxis.X, xPoints * yPoints + 1);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetStartIndex(EAxis.Y, xPoints * yPoints + 1);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetEndIndex(EAxis.Y, 2 * xPoints * yPoints + 2);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetSampleRate(EAxis.X, 5);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SetSampleRate(EAxis.Y, 5);

            controller.SignalFlowManager.InputStage.ArbitraryVector.SwitchArbitraryVector(EAxis.X, true);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SwitchArbitraryVector(EAxis.Y, true);

            WaitForEnter("Arbitrary vector is armed. Apply the external trigger, then press Enter to stop the example.");

            controller.SignalFlowManager.InputStage.ArbitraryVector.SwitchArbitraryVector(EAxis.X, false);
            controller.SignalFlowManager.InputStage.ArbitraryVector.SwitchArbitraryVector(EAxis.Y, false);
            Console.WriteLine("Arbitrary vector stopped.");
        }

        private static List<Tuple<int, int>> BuildGrid(int xPoints, int yPoints)
        {
            var matrixVector = new List<Tuple<int, int>>();

            for (var i = 0; i < xPoints; i++)
            {
                for (var j = 0; j < yPoints; j++)
                {
                    matrixVector.Add(Tuple.Create(i, j));
                }
            }

            return matrixVector;
        }

        private static List<Tuple<double, double>> ConvertGridToXy(
            IEnumerable<Tuple<int, int>> gridPoints,
            double xyFactor,
            double offsetX,
            double offsetY)
        {
            var xyPoints = new List<Tuple<double, double>>();

            foreach (var point in gridPoints)
            {
                xyPoints.Add(Tuple.Create(point.Item1 / xyFactor + offsetX, point.Item2 / xyFactor + offsetY));
            }

            xyPoints.Insert(0, xyPoints[0]);
            return xyPoints;
        }

        private static List<float> BuildVectorPatternMemory(IReadOnlyList<Tuple<double, double>> xyPoints)
        {
            var vectorToVpuMemory = new List<float>();

            for (var coordinateIndex = 0; coordinateIndex < 2; coordinateIndex++)
            {
                for (var pointIndex = 0; pointIndex < xyPoints.Count; pointIndex++)
                {
                    vectorToVpuMemory.Add((float)(coordinateIndex == 0 ? xyPoints[pointIndex].Item1 : xyPoints[pointIndex].Item2));
                }
            }

            return vectorToVpuMemory;
        }
    }
}
