using AutomaticController;
using CommunityToolkit.Mvvm.ComponentModel;
using CommunityToolkit.Mvvm.Input;
using LiveCharts;
using LiveCharts.Wpf;
using Simulator.Models;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Runtime.Intrinsics.X86;
using System.Text;
using System.Threading.Tasks;

namespace Simulator.ViewModels
{
    internal partial class MovingSetpointPIDViewModel : ObservableObject
    {
        [ObservableProperty]
        private MovingPIDSetpointModel _setpointModel;

        [ObservableProperty]
        private ObservableCollection<double> _processHistoryCollection;
        [ObservableProperty]
        private ObservableCollection<double> _setpointHistoryCollection;

        [ObservableProperty]
        private SeriesCollection _series;

        private List<double> _processHistoryList;
        private List<double> _setpointHistoryList;


        [ObservableProperty]
        private double _kp;
        [ObservableProperty]
        private double _ki;
        [ObservableProperty]
        private double _kd;

        private readonly int SIMULATION_STEP = 1000;


        public MovingSetpointPIDViewModel()
        {
            Kp = 0.1;
            Ki = 0.0;
            Kd = 0.0;

            SetpointModel = new MovingPIDSetpointModel()
            {
                Controller = new PIDController(Kp, Kd, Ki),
                Setpoints = new List<double>()
            };

            // Generate setpoints
            for (int i = 0; i < SIMULATION_STEP; i++)
            {
                double cos = Math.Cos(0.5 / (2 * Math.PI) * i);
                SetpointModel.Setpoints.Add(cos);
            }

            _processHistoryList = new List<double>();
            _setpointHistoryList = new List<double>();

            ProcessHistoryCollection = new ObservableCollection<double>();
            SetpointHistoryCollection = new ObservableCollection<double>();
            Series = new SeriesCollection()
            {
                new LineSeries()
                {
                    Title = "Actual",
                    Values = new ChartValues<double>(),
                    PointGeometrySize = 5
                },
                new LineSeries()
                {
                    Title = "Setpoint",
                    Values = new ChartValues<double>(),
                    PointGeometrySize = 5
                },
            };
#if DEBUG
            Simulate();
#endif
        }

        [RelayCommand]
        public void Simulate()
        {
            SetpointModel.Controller.Kp = Kp;
            SetpointModel.Controller.Ki = Ki;
            SetpointModel.Controller.Kd = Kd;

            _processHistoryList.Clear();
            _setpointHistoryList.Clear();

            InsertData(SetpointModel.Setpoints[0], 0d);

            for (int i = 1; i < SIMULATION_STEP; i++)
            {
                double current = _processHistoryList.LastOrDefault();
                double output = SetpointModel.Controller.Compute(SetpointModel.Setpoints[i], current);
                double actual = current + output;

                if (!double.IsNormal(SetpointModel.Setpoints[i]) || !double.IsNormal(actual))
                    continue;

                InsertData(SetpointModel.Setpoints[i], actual);
            }

            Series[0].Values.Clear();
            Series[1].Values.Clear();

            Series[0].Values = new ChartValues<double>(_processHistoryList);
            Series[1].Values = new ChartValues<double>(_setpointHistoryList);
        }

        private void InsertData(double setpoint, double actual)
        {
            if (double.IsNaN(setpoint))
                return;
            if (double.IsNaN(actual))
                return;

            _setpointHistoryList.Add(setpoint);
            _processHistoryList.Add(actual);
        }
    }
}