using Fluctio.FluctioSim.Common.Configuration;
using Fluctio.FluctioSim.Core.Components.MachineLearning.NumberProviders;
using Fluctio.FluctioSim.Core.Components.Prefabs.Primitives.Base;
using Mujoco;
using UnityEngine;

namespace Fluctio.FluctioSim.Core.Components.Prefabs.Complex.RotaryPendulum
{
    public class PendulumBalanceTimer : AgentNumberProvider
    {
        [SerializeField] protected PrimitiveJoint barJoint;
        [SerializeField, Range(Config.MinFloat, 180)] protected float maxBarTilt = 45;

        private MjHingeJoint _internalBarJoint;
        private int _balanceHoldingTimer = 0;

        public void Start()
        {
            _internalBarJoint = barJoint.GetJointComponent<MjHingeJoint>();
        }

        private void OnEnable()
        {
            Agent.UnityAgent.EpisodeInitializing += OnEpisodeInitializing;
        }

        private void OnDisable()
        {
            Agent.UnityAgent.EpisodeInitializing -= OnEpisodeInitializing;
        }

        private void OnEpisodeInitializing()
        {
            _balanceHoldingTimer = 0;
        }

        private void FixedUpdate()
        {
            var distanceFromCenterInDegrees = Mathf.Abs(Mathf.Abs(_internalBarJoint.Configuration) - 180);
            if (distanceFromCenterInDegrees <= maxBarTilt)
            {
                _balanceHoldingTimer += 1;
            }
            else {
                _balanceHoldingTimer = 0;
            }
        }

        public override float GetNumber() => _balanceHoldingTimer * Time.fixedDeltaTime;
    }
}