using System.Collections.Generic;
using System.Threading;
using ln.ethercat.controller.drives;
using ln.logging;

namespace ln.ethercat.controller
{

    public delegate void ControllerLogicDelegate(Controller controller);

    public class Controller
    {
        public event ControllerLogicDelegate ControllerLogic;


        List<DriveController> driveControllers = new List<DriveController>();
        public DriveController[] DriveControllers => driveControllers.ToArray();

        List<ControlLoop> controlLoops = new List<ControlLoop>();
        public ControlLoop[] ControlLoops => controlLoops.ToArray();

        public bool IsRunning => threadController?.IsAlive ?? false;

        public double ControllerLoopInterval { get; set; } = 0.1;
        public double ControllerLoopFrequency {
            get => 1.0 / ControllerLoopInterval;
            set => ControllerLoopInterval = 1.0 / value;
        }

        bool stopRequested;
        Thread threadController;

        public Controller()
        {
        }

        public void Start()
        {
            if (threadController?.IsAlive ?? false)
            {
                Logging.Log(LogLevel.WARNING, "Controller; Start(): already started");
            } else {
                stopRequested = false;
                threadController = new Thread(ControllerThread);
                threadController.Start();
            }
        }

        public void Stop()
        {
            if (threadController?.IsAlive ?? false)
            {
                stopRequested = true;
                threadController.Join();
                threadController = null;
            }
        }

        public void Add(DriveController driveController) => driveControllers.Add(driveController);
        public void Remove(DriveController driveController) => driveControllers.Remove(driveController);

        public void Add(ControlLoop controlLoop) => controlLoops.Add(controlLoop);
        public void Remove(ControlLoop controlLoop) => controlLoops.Remove(controlLoop);

        public DriveStates DrivesState {
            get {
                DriveStates lowestState = DriveStates.OPERATIONAL;
                foreach (DriveController driveController in driveControllers)
                {
                    DriveStates driveState = driveController.DriveState;
                    if (driveState < lowestState)
                        lowestState = driveState;
                }
                return lowestState;
            }
        }

        void ControllerThread()
        {
            while (!stopRequested)
            {
                foreach (DriveController driveController in driveControllers)
                    driveController.UpdateStates();

                foreach (ControlLoop controlLoop in controlLoops)
                    controlLoop.Loop();

                ControllerLogic?.Invoke(this);

                foreach (DriveController driveController in driveControllers)
                    driveController.UpdateDrive();

                Thread.Sleep((int)(1000.0 * ControllerLoopInterval));
            }
        }



    }
}