OHV/Dev/OHV/VehicleControlSystem/ControlLayer/Steering.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using GSG.NET.Concurrent;
using GSG.NET.Logging;
using GSG.NET.Utils;
using OHV.Common.Shareds;
using OHV.SqliteDAL;
using Prism.Events;
using VehicleControlSystem.ControlLayer.IO;
using VehicleControlSystem.ControlLayer.MQ;

namespace VehicleControlSystem.ControlLayer
{
    public class Steering : ControlObjectBase, IDisposable
    {
        static Logger logger = Logger.GetLogger();
        static Logger SteerLogger = Logger.GetLogger("SteerLogger");

        ThreadCancel threadCancel = new ThreadCancel();
        TsQueue<eSteerCommand> qReqSteer = new TsQueue<eSteerCommand>();

        EzIO iO = null;
        SqliteManager sql = null;
        IEventAggregator eventAggregator = null;
        ZmqManager zmqManager = null;

        public event EventHandler<int> OnSteeringError;

        private eSteeringState steeringState;

        public eSteeringState SteeringState
        {
            get { return steeringState; }
            set { if (SetField(ref this.steeringState, value)) { /*this.zmqManager.SetCurrentSteeringState(value);*/ } }
        }

        public Steering() { }

        public void Dispose()
        {
            this.threadCancel.Cancel();
        }

        public void AssignRefObject(IIO io, SqliteManager sql, IEventAggregator ea, ZmqManager zm)
        {
            this.iO = io as EzIO;
            this.iO.OnChangedIO += IO_OnChangedIO;

            this.sql = sql;
            this.eventAggregator = ea;
            this.zmqManager = zm;

            this.threadCancel.AddGo(_DoControlSteer);
        }

        #region Thread Method
        void _DoControlSteer()
        {
            while (!this.threadCancel.Canceled)
            {
                var q = this.qReqSteer.Dequeue();
                //this.ControlSteerNWaitDone( q );
                this.ExecuteCommandSteer(q);
            }
        }

        void ControlSteerNWaitDone(eSteeringState requestState)
        {
            logger.D($"Steer - Start {requestState} -------------------------------------------------------");

            if (requestState == eSteeringState.Left)
            {
                this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", false);
                this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", true);
            }
            else
            {
                this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", true);
                this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", false);
            }

            //Hight Voltage On
            this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
            this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);

            logger.D($"Steer - OutPut On");

            var sT = SwUtils.CurrentTimeMillis;
            bool isCheckedOutput = false;
            var currentDirection = eSteeringState.None;
            long eT = 0;
            while (true)
            {
                LockUtils.Wait(10);

                currentDirection = GetSteerDirection();
                if (requestState == currentDirection)
                    break;

                eT = SwUtils.Elapsed(sT);

                if (eT > 700 && !isCheckedOutput)
                {
                    this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
                    this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
                    isCheckedOutput = true;
                    logger.D($"Steer - Output Off {eT}ms");
                }

                if (eT > ConstUtils.ONE_SECOND * 2)
                {
                    logger.D($"Steer - Wait Time Out {eT}ms");
                    this.SteeringState = eSteeringState.None;

                    if (!isCheckedOutput)
                    {
                        this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
                        this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
                        logger.D($"Steer - Output Off {SwUtils.Elapsed(sT)}ms");
                    }
                    return;
                }
            }

            if (!isCheckedOutput) //위에서 Off 를 안했을 경우가 생길 수 있음. (700ms 이내에 조향 바뀌었을때)
            {
                this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
                this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
                logger.D($"Steer - Output Off {SwUtils.Elapsed(sT)}ms");
            }

            logger.D($"Steer - Drive Request Start");
            this.SteeringState = currentDirection;
            logger.D($"Steer - Contorl End {SwUtils.Elapsed(sT)}ms -------------------------------------------------------");

            SteerLogger.I(SwUtils.Elapsed(sT));
        }

        void ExecuteCommandSteer(eSteerCommand command)
        {
            logger.D($"Execute Steer Command - {command}");
            var cmdSt = SwUtils.CurrentTimeMillis;

            Action<bool> actionSteerFront = (isLeft) =>
            {
                if (isLeft)
                    this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", false);
                else
                    this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", true);
            };

            Action<bool> actionSteerRear = (isLeft) =>
            {
                if (isLeft)
                    this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", true);
                else
                    this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", false);
            };

            Action<bool> actionOnFrontHightVolteOn = (isOn) =>
            {
                if (isOn)
                    this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
                else
                    this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
            };

            Action<bool> actionOnRearHightVolteOn = (isOn) =>
            {
                if (isOn)
                    this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
                else
                    this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
            };

            Action<eSteerCommand> actionSteer = (cmd) =>
            {
                List<Func<bool>> inputList = new List<Func<bool>>();
                List<Action<bool>> outputList = new List<Action<bool>>();

                switch (cmd)
                {
                    case eSteerCommand.LL:
                    case eSteerCommand.ManualLL:
                        inputList.Add(IsFrontLeft);
                        inputList.Add(IsRearLeft);
                        this.iO.WriteOutputIO("OUT_F_STEERING_OFF", true);
                        this.iO.WriteOutputIO("OUT_R_STEERING_OFF", true);
                        actionSteerFront(true);
                        actionSteerRear(true);
                        outputList.Add(actionOnRearHightVolteOn);
                        outputList.Add(actionOnFrontHightVolteOn);
                        break;
                    case eSteerCommand.LX:
                        inputList.Add(IsFrontLeft);
                        this.iO.WriteOutputIO("OUT_F_STEERING_OFF", true);
                        actionSteerFront(true);
                        outputList.Add(actionOnFrontHightVolteOn);
                        break;
                    case eSteerCommand.XL:
                        inputList.Add(IsRearLeft);
                        this.iO.WriteOutputIO("OUT_R_STEERING_OFF", true);
                        actionSteerRear(true);
                        outputList.Add(actionOnRearHightVolteOn);
                        break;
                    case eSteerCommand.RR:
                    case eSteerCommand.ManualRR:
                        inputList.Add(IsFrontRight);
                        inputList.Add(IsRearRight);
                        this.iO.WriteOutputIO("OUT_F_STEERING_OFF", true);
                        this.iO.WriteOutputIO("OUT_R_STEERING_OFF", true);
                        actionSteerFront(false);
                        actionSteerRear(false);
                        outputList.Add(actionOnRearHightVolteOn);
                        outputList.Add(actionOnFrontHightVolteOn);
                        break;
                    case eSteerCommand.RX:
                        inputList.Add(IsFrontRight);
                        this.iO.WriteOutputIO("OUT_F_STEERING_OFF", true);
                        actionSteerFront(false);
                        outputList.Add(actionOnFrontHightVolteOn);
                        break;
                    case eSteerCommand.XR:
                        inputList.Add(IsRearRight);
                        this.iO.WriteOutputIO("OUT_R_STEERING_OFF", true);
                        actionSteerRear(false);

                        outputList.Add(actionOnRearHightVolteOn);
                        break;
                    default:
                        break;
                }

                outputList.ForEach(x => x.Invoke(true));

                var sT = SwUtils.CurrentTimeMillis;
                while (true)
                {
                    LockUtils.Wait(10);

                    if (inputList.All(x => x.Invoke()))
                    {
                        logger.D($"Execute Steer - {cmd} - Sensor On {SwUtils.Elapsed(sT)}mm");
                        break;
                    }

                    if (SwUtils.Elapsed(sT) > 700)
                    {
                        logger.D($"Execute Steer Time out - {cmd} - {SwUtils.Elapsed(sT)}mm");
                        break;
                    }
                }

                outputList.ForEach(x => x.Invoke(false));
            };

            switch (command)
            {
                case eSteerCommand.LL:
                case eSteerCommand.ManualLL:
                case eSteerCommand.LX:
                case eSteerCommand.XL:
                case eSteerCommand.RR:
                case eSteerCommand.ManualRR:
                case eSteerCommand.RX:
                case eSteerCommand.XR:
                    actionSteer(command);
                    break;
                case eSteerCommand.OffOff:
                    //Todo: 전후방 전원 Off
                    this.iO.WriteOutputIO("OUT_F_STEERING_OFF", false);
                    this.iO.WriteOutputIO("OUT_R_STEERING_OFF", false);
                    break;
                case eSteerCommand.OnOn:
                    this.iO.WriteOutputIO("OUT_F_STEERING_OFF", true);
                    this.iO.WriteOutputIO("OUT_R_STEERING_OFF", true);
                    break;
                case eSteerCommand.FrontOn:
                    this.iO.WriteOutputIO("OUT_F_STEERING_OFF", true);
                    break;
                case eSteerCommand.RearOn:
                    this.iO.WriteOutputIO("OUT_R_STEERING_OFF", true);
                    break;
                case eSteerCommand.FrontOff:
                    this.iO.WriteOutputIO("OUT_F_STEERING_OFF", false);
                    break;
                case eSteerCommand.RearOff:
                    this.iO.WriteOutputIO("OUT_R_STEERING_OFF", false);
                    break;
                default:
                    break;
            }

            if ( command == eSteerCommand.ManualLL || command == eSteerCommand.ManualRR)
            {
                this.SteeringState = GetSteerDirection();
            }

            logger.D($"Complete Steer Command - {command} / {SwUtils.Elapsed(cmdSt)}ms");
        }

        eSteeringState GetSteerDirection()
        {
            if (this.IsLeft())
                return eSteeringState.Left;
            if (this.IsRight())
                return eSteeringState.Right;

            return eSteeringState.None;
        }

        public void RequestControl(eSteeringState state)
        {
            //this.qReqSteer.Enqueue( state );
            switch (state)
            {
                case eSteeringState.None:
                    break;
                case eSteeringState.Left:
                    this.RequestCommand(eSteerCommand.ManualLL);
                    break;
                case eSteeringState.Right:
                    this.RequestCommand(eSteerCommand.ManualRR);
                    break;
                default:
                    break;
            }
        }

        public void RequestCommand(eSteerCommand command) => this.qReqSteer.Enqueue(command);


        #endregion

        private void IO_OnChangedIO(BitBlock bit)
        {
            //if ( bit.Tag.Equals( "IN_F_STEERING_DETECT_LEFT" ) || bit.Tag.Equals( "IN_F_STEERING_DETECT_RIGHT" ) )
            //    this.GetSteeringState();

            //if ( bit.Tag.Equals( "IN_R_STEERING_DETECT_LEFT" ) || bit.Tag.Equals( "IN_R_STEERING_DETECT_RIGHT" ) )
            //    this.GetSteeringState();
        }

        public eSteeringState GetSteeringState()
        {
            eSteeringState frontState = eSteeringState.None;
            if (this.IsFrontLeft() && this.IsFrontRight())
                frontState = eSteeringState.None;
            if (!this.IsFrontLeft() && !this.IsFrontRight())
                frontState = eSteeringState.None;

            if (this.IsFrontRight())
                frontState = eSteeringState.Right;
            else if (this.IsFrontLeft())
                frontState = eSteeringState.Left;
            else
                frontState = eSteeringState.None;

            eSteeringState rearState = eSteeringState.None;
            if (this.IsRearLeft() && this.IsRearRight())
                rearState = eSteeringState.None;
            if (!this.IsRearLeft() && !this.IsRearRight())
                rearState = eSteeringState.None;

            if (this.IsRearRight())
                rearState = eSteeringState.Right;
            else if (this.IsRearLeft())
                rearState = eSteeringState.Left;
            else
                rearState = eSteeringState.None;

            eSteeringState totalState = eSteeringState.None;
            if (frontState == eSteeringState.Left && rearState == eSteeringState.Left)
                totalState = eSteeringState.Left;
            else if (frontState == eSteeringState.Right && rearState == eSteeringState.Right)
                totalState = eSteeringState.Right;
            else
                totalState = eSteeringState.None;

            return totalState;
        }

        readonly object lockObject = new object();
        bool isExecuteSteering = false;
        public int ControlSteering(bool isLeft = false)
        {
            if (this.isExecuteSteering)
                return 0;
            this.isExecuteSteering = true;

            var sT = SwUtils.CurrentTimeMillis;

            lock (lockObject)
            {
                if (isLeft) //Left IO 가 직진 이라고 생각하자.
                {
                    this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", false);
                    this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", true);
                    this.SetHightVolt();
                    if (this.SteeringState == eSteeringState.Left)
                    {
                        this.isExecuteSteering = false;
                        return 0;
                    }
                }
                else
                {
                    this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", true);
                    this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", false);
                    this.SetHightVolt();
                    if (this.SteeringState == eSteeringState.Right)
                    {
                        this.isExecuteSteering = false;
                        return 0;
                    }
                }

                var task = Task<bool>.Run(() =>
                {
                    long sTime = SwUtils.CurrentTimeMillis;
                    while (true)
                    {
                        LockUtils.Wait(5);

                        if (SwUtils.Gt(sTime, 2 * ConstUtils.ONE_SECOND)) //Wait 3Sec
                            return false;

                        if (isLeft)
                        {
                            if (this.IsLeft())
                                break;
                        }
                        else
                        {
                            if (this.IsRight())
                                break;
                        }
                    }
                    return true;
                }).ContinueWith(t =>
                    {
                        if (!t.Result)
                            this.OnSteeringError?.BeginInvoke(this, 33, null, null);
                        else
                            this.OnSteeringError?.BeginInvoke(this, 0, null, null);

                        logger.D($"[Steer Release Lock] - {SwUtils.Elapsed(sT)} ms");

                        LockUtils.Wait(200);
                        this.SteeringState = GetSteeringState();
                        this.isExecuteSteering = false;

                    });
            }

            return 0;
        }

        /// <summary>
        /// Error 를 만들지 않고 동작 만 한다.
        /// </summary>
        /// <param name="isLeft"></param>
        public void ControlSteeringNotResult(bool isLeft = false)
        {
            if (this.isExecuteSteering) return;

            this.isExecuteSteering = true;

            var sT = SwUtils.CurrentTimeMillis;

            if (isLeft) //Left IO 가 직진 이라고 생각하자.
            {
                this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", false);
                this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", true);
                this.SetHightVolt();
                if (this.SteeringState == eSteeringState.Left)
                {
                    this.isExecuteSteering = false;
                    return;
                }
            }
            else
            {
                this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", true);
                this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", false);
                this.SetHightVolt();
                if (this.SteeringState == eSteeringState.Right)
                {
                    this.isExecuteSteering = false;
                    return;
                }
            }

            var task = Task<bool>.Run(() =>
           {
               long sTime = SwUtils.CurrentTimeMillis;
               while (true)
               {
                   LockUtils.Wait(5);

                   if (SwUtils.Gt(sTime, 2 * ConstUtils.ONE_SECOND)) //Wait 3Sec
                       return false;

                   if (isLeft)
                   {
                       if (this.IsLeft())
                           break;
                   }
                   else
                   {
                       if (this.IsRight())
                           break;
                   }
               }
               return true;
           }).ContinueWith(t =>
              {
                  var eT = SwUtils.Elapsed(sT);
                  logger.D($"[Steer Release Lock] - {eT} ms");

                  if (!t.Result)
                      this.SteeringState = eSteeringState.None;
                  else
                  {
                      LockUtils.Wait(200);
                      this.SteeringState = GetSteeringState();
                  }

                  this.isExecuteSteering = false;
              });
        }

        /// <summary>
        /// Test Control
        /// </summary>
        /// <param name="isLeft"></param>
        public void TestSteerControl(bool isLeft = false)
        {
            if (this.isExecuteSteering) return;

            this.isExecuteSteering = true;

            logger.D($"[Steer Lock] ----------------------------------------------------------------");

            var sT = SwUtils.CurrentTimeMillis;

            Task.Run(() =>
            {
                if (isLeft) //Left IO 가 직진 이라고 생각하자.
                {
                    this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", false);
                    this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", true);

                    this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
                    this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
                    LockUtils.Wait(700);
                    this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
                    this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
                }
                else
                {
                    this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", true);
                    this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", false);

                    this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
                    this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
                    LockUtils.Wait(700);
                    this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
                    this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
                }
            });

            Task.Run(() =>
            {
                long sTime = SwUtils.CurrentTimeMillis;
                while (true)
                {
                    LockUtils.Wait(5);

                    if (SwUtils.Gt(sTime, 2 * ConstUtils.ONE_SECOND))
                    {
                        this.SteeringState = eSteeringState.None;
                        this.isExecuteSteering = false;
                        logger.D($"Steer Control Fail {SwUtils.Elapsed(sT)}");
                        return;
                    }

                    if (isLeft)
                    {
                        if (this.IsLeft())
                            break;
                    }
                    else
                    {
                        if (this.IsRight())
                            break;
                    }
                }
                LockUtils.Wait(200); //안정화 시간.

                logger.D($"Request Steer State Start");
                this.SteeringState = GetSteeringState();
                logger.D($"Request Steer State End");

                var eT = SwUtils.Elapsed(sT);
                logger.D($"[Steer Release Lock] - {eT} ms ----------------------------------------------------------");
                SteerLogger.I(eT);
            });

            this.isExecuteSteering = false;
        }

        void SetHightVolt()
        {
            this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
            this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
            this.iO.WriteOutputIO("OUT_F_STEERING_DA", false, 400);
            this.iO.WriteOutputIO("OUT_R_STEERING_DA", false, 700);
        }

        bool IsFrontLeft() => this.iO.IsOn("IN_F_STEERING_DETECT_LEFT");
        bool IsFrontRight() => this.iO.IsOn("IN_F_STEERING_DETECT_RIGHT");
        bool IsRearLeft() => this.iO.IsOn("IN_R_STEERING_DETECT_LEFT");
        bool IsRearRight() => this.iO.IsOn("IN_R_STEERING_DETECT_RIGHT");

        public bool IsLeft() => this.IsFrontLeft() && this.IsRearLeft();
        public bool IsRight() => this.IsFrontRight() && this.IsRearRight();
    }
}