using System;
using GameDev.UDP;
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 byte Watchdog { get; set; }
    [field: SerializeField] private byte States { get; set; }
    [field: SerializeField] private int M1SpeedCmd { get; set; }
    [field: SerializeField] private int M2SpeedCmd { get; set; }
    [field: SerializeField] private float AccelerometerPitch { get; set; }
    [field: SerializeField] private float FilteredPitch { get; set; }
    [field: SerializeField] private float PitchRate { get; set; }
    [field: SerializeField] private float RemSpeedRef { get; set; }
    [field: SerializeField] private float RemTurnSpeedRef { get; set; }
    [field: SerializeField] private float ScContOut { get; set; }
    [field: SerializeField] private float TcContOut { get; set; }
    [field: SerializeField] private float OlContOut { get; set; }
    [field: SerializeField] private float RefIL { get; set; }
    [field: SerializeField] private float ActIL { get; set; }
    [field: SerializeField] private float ErrorIL { get; set; }
    [field: SerializeField] private float ILContOut { get; set; }
    [field: SerializeField] private float IerrorIL { get; set; }
    [field: SerializeField] private float ILAntiWindup { get; set; }
    [field: SerializeField] private float SpeedCmd1 { get; set; }
    [field: SerializeField] private float SpeedCmd2 { get; set; }
    [field: SerializeField] private float BotLinVel { get; set; }
    [field: SerializeField] private float BotAngVel { get; set; }
    [field: SerializeField] private float LeftMotorAngVel { get; set; }
    [field: SerializeField] private float RightMotorAngVel { get; set; }
    [field: SerializeField] private int M1Raw { get; set; }
    [field: SerializeField] private int M2Raw { 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 = ReceivedData[i];
        States = ReceivedData[i += 1];
        M1SpeedCmd = BitConverter.ToInt16(ReceivedData, i += 1);
        M2SpeedCmd = BitConverter.ToInt16(ReceivedData, i += 2);
        AccelerometerPitch = BitConverter.ToSingle(ReceivedData, i += 2);
        FilteredPitch = BitConverter.ToSingle(ReceivedData, i += 4);
        PitchRate = BitConverter.ToSingle(ReceivedData, i += 4);
        RemSpeedRef = BitConverter.ToSingle(ReceivedData, i += 4);
        RemTurnSpeedRef = BitConverter.ToSingle(ReceivedData, i += 4);
        ScContOut = BitConverter.ToSingle(ReceivedData, i += 4);
        TcContOut = BitConverter.ToSingle(ReceivedData, i += 4);
        OlContOut = BitConverter.ToSingle(ReceivedData, i += 4);
        RefIL = BitConverter.ToSingle(ReceivedData, i += 4);
        ActIL = BitConverter.ToSingle(ReceivedData, i += 4);
        ErrorIL = BitConverter.ToSingle(ReceivedData, i += 4);
        ILContOut = BitConverter.ToSingle(ReceivedData, i += 4);
        IerrorIL = BitConverter.ToSingle(ReceivedData, i += 4);
        ILAntiWindup = BitConverter.ToSingle(ReceivedData, i += 4);
        SpeedCmd1 = BitConverter.ToSingle(ReceivedData, i += 4);
        SpeedCmd2 = BitConverter.ToSingle(ReceivedData, i += 4);
        BotLinVel = BitConverter.ToSingle(ReceivedData, i += 4);
        BotAngVel = BitConverter.ToSingle(ReceivedData, i += 4);
        LeftMotorAngVel = BitConverter.ToSingle(ReceivedData, i += 4);
        RightMotorAngVel = BitConverter.ToSingle(ReceivedData, i += 4);
        M1Raw = BitConverter.ToInt32(ReceivedData, i += 4);
        M2Raw = 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;
        }
    }
}