using System;
using GameDev.UDP;
using ScriptableObjectArchitecture;
using UnityEngine;
using UnityEngine.Serialization;

[RequireComponent(typeof(UDPManager))]
public class ReceiveData : MonoBehaviour
{
    [Header("Connections")]
    [SerializeField] private UDPManager _udpManager;

    [Header("Config")]
    [SerializeField] private float _sampleTimeSetting;

    [Header("State")]
    [SerializeField] private float _updateRateHz;
    [SerializeField] private ulong _packagesReceived;
    private byte[] ReceivedData { get; set; }

    private int _newPackages;
    private float _sampleTime;

    [field: Header("ReceivedData")]
    [field: SerializeField] private ByteReference Watchdog { get; set; }
    [field: SerializeField] private ByteReference States { get; set; }
    [field: SerializeField] private IntReference M1SpeedCmd { get; set; }
    [field: SerializeField] private IntReference M2SpeedCmd { get; set; }
    [field: SerializeField] private FloatReference AccelerometerPitch { get; set; }
    [field: SerializeField] private FloatReference FilteredPitch { get; set; }
    [field: SerializeField] private FloatReference PitchRate { get; set; }
    [field: SerializeField] private FloatReference RemSpeedRef { get; set; }
    [field: SerializeField] private FloatReference RemTurnSpeedRef { get; set; }
    [field: SerializeField] private FloatReference ScContOut { get; set; }
    [field: SerializeField] private FloatReference TcContOut { get; set; }
    [field: SerializeField] private FloatReference OlContOut { get; set; }
    [field: SerializeField] private FloatReference RefIL { get; set; }
    [field: SerializeField] private FloatReference ActIL { get; set; }
    [field: SerializeField] private FloatReference ErrorIL { get; set; }
    [field: SerializeField] private FloatReference ILContOut { get; set; }
    [field: SerializeField] private FloatReference IerrorIL { get; set; }
    [field: SerializeField] private FloatReference ILAntiWindup { get; set; }
    [field: SerializeField] private FloatReference SpeedCmd1 { get; set; }
    [field: SerializeField] private FloatReference SpeedCmd2 { get; set; }
    [field: SerializeField] private FloatReference BotLinVel { get; set; }
    [field: SerializeField] private FloatReference BotAngVel { get; set; }
    [field: SerializeField] private FloatReference LeftMotorAngVel { get; set; }
    [field: SerializeField] private FloatReference RightMotorAngVel { get; set; }
    [field: SerializeField] private IntReference M1Raw { get; set; }
    [field: SerializeField] private IntReference M2Raw { get; set; }
    [field: SerializeField] private IntReference Millis { get; set; }

    private void OnEnable()
    {
        _sampleTime = _sampleTimeSetting;
        _udpManager.ReceivedNewUdpData += ProcessData;
    }

    private void OnDisable()
    {
        _udpManager.ReceivedNewUdpData -= ProcessData;
    }

    private void ProcessData()
    {
        _packagesReceived++;
        _newPackages++;
        ReceivedData = _udpManager.Data;
        var i = 0;
        Watchdog.Value = ReceivedData[i];
        States.Value = ReceivedData[i += 1];
        M1SpeedCmd.Value = BitConverter.ToInt16(ReceivedData, i += 1);
        M2SpeedCmd.Value = BitConverter.ToInt16(ReceivedData, i += 2);
        AccelerometerPitch.Value = BitConverter.ToSingle(ReceivedData, i += 2);
        FilteredPitch.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        PitchRate.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        RemSpeedRef.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        RemTurnSpeedRef.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        ScContOut.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        TcContOut.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        OlContOut.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        RefIL.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        ActIL.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        ErrorIL.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        ILContOut.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        IerrorIL.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        ILAntiWindup.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        SpeedCmd1.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        SpeedCmd2.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        BotLinVel.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        BotAngVel.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        LeftMotorAngVel.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        RightMotorAngVel.Value = BitConverter.ToSingle(ReceivedData, i += 4);
        M1Raw.Value = BitConverter.ToInt32(ReceivedData, i += 4);
        M2Raw.Value = BitConverter.ToInt32(ReceivedData, i += 4);
        Millis.Value = BitConverter.ToInt32(ReceivedData, i += 4);
    }

    private void FixedUpdate()
    {
        CalculateReceiveRate();
    }

    private void CalculateReceiveRate()
    {
        if (_sampleTime < 0)
        {
            _updateRateHz = _newPackages;
            _newPackages = 0;
            _sampleTime = _sampleTimeSetting;
        }
        else
        {
            _sampleTime -= Time.fixedDeltaTime;
        }
    }
}