C#MonoGame+Retro2D : Class MobilUnit

    public class MobilUnit : Node
    {
        Queue<Vector2> _queueGoal = new Queue<Vector2>();

        float _animY = 0;
        float _vY = -0.5f;
        float _upY = -4;

        float _speed = 1;
        Vector2 _currentGoal;
        Vector2 _currentVector;

        bool _onMove = false;
        bool _isMove = false;
        bool _onGoal = false;
        bool _isGoal = false;


        public bool IsQueueGoal()
        {
            return _queueGoal.Count > 0;
        }
        public bool OnMove()
        {
            return _onMove;
        }
        public bool OnGoal()
        {
            return _onGoal;
        }
        public bool EndOfMove()
        {
            return (_onGoal && !IsQueueGoal());
        }

        public Vector2 GetMoveVector(Vector2 start, Vector2 goal, float speed)
        {
            float vecX = goal.X - start.X;
            float vecY = goal.Y - start.Y;

            Vector2 moveVector = new Vector2(0,0);

            moveVector.X = (float)Math.Cos(-Math.Atan2(vecX, vecY) + 1.5708) * speed;
            moveVector.Y = (float)Math.Sin(-Math.Atan2(vecX, vecY) + 1.5708) * speed;

            return moveVector;
        }
        public void Init(float x, float y, float speed = 1)
        {
            _x = x;
            _y = y;
            _speed = speed;
        }
        public void AddGoal(float x, float y)
        {
            _queueGoal.Enqueue(new Vector2(x, y));
        }
        public void ClearAllGoal()
        {
            _queueGoal.Clear();
        }

        public MobilUnit()
        {

        }

        public void Move()
        {
            _x += _currentVector.X;
            _y += _currentVector.Y;

            if (Math.Abs(_x - _currentGoal.X) < 1 &&
                Math.Abs(_y - _currentGoal.Y) < 1)
            {
                _isGoal = true;
                _onGoal = true;
                _isMove = false;

                _queueGoal.Dequeue();
            }
        }
        public Position GetDirection(float vxMin = 0.3f)
        {
            float vx = _currentVector.X;
            float vy = _currentVector.Y;

            if (vx >  vxMin) return Position.E;
            if (vx < -vxMin) return Position.W;

            if (vy > 0) return Position.S;
            if (vy < 0) return Position.N;

            return Position.CENTER;
        }

        public override Node Update()
        {
            _onMove = false;
            _onGoal = false;

            if (IsQueueGoal() && !_isMove)
            {
                _isGoal = false;
                _isMove = true;
                _onMove = true;

                _currentGoal = _queueGoal.Peek();
                _currentVector = GetMoveVector(new Vector2(_x, _y), _currentGoal, _speed);

            }

            if (_isMove) Move();

            _animY += _vY;

            if (_animY < _upY) _vY = 0.5f;
            if (_animY > 0) _vY = -0.5f;

            return this;
        }

        public override Node Render(SpriteBatch batch)
        {
            //if (IsQueueGoal())
            //{
            //    Vector2 prevVec = new Vector2(_x,_y);

            //    foreach  (Vector2 vec in _queueGoal)
            //    {
            //        batch.DrawLine(prevVec, vec, Color.Lavender * .6f, 1);
            //        batch.DrawPoint(vec, Color.GreenYellow, 1);
            //        prevVec = vec;
            //    }
            //}

            batch.DrawCircle(AbsX(),AbsY() + _animY, 8,16, Color.Coral,2);

            if (GetDirection() == Position.N) batch.DrawCircle(AbsX(), AbsY()- 8 + _animY, 4, 8, Color.Goldenrod, 2);
            if (GetDirection() == Position.S) batch.DrawCircle(AbsX(), AbsY()+ 8 + _animY, 4, 8, Color.Goldenrod, 2);
            if (GetDirection() == Position.W) batch.DrawCircle(AbsX()-8, AbsY() + _animY, 4, 8, Color.Goldenrod, 2);
            if (GetDirection() == Position.E) batch.DrawCircle(AbsX()+8, AbsY() + _animY, 4, 8, Color.Goldenrod, 2);


            return this;
        }