OSCCollar/OSCCollar/OSCCollar.cs

using Newtonsoft.Json;
using GlaxOSC;

namespace VRC_Console;

public partial class OSCCollar
{
    private static readonly string ConfigFilePath = Path.Combine(Directory.GetCurrentDirectory(), "config.json");

    public static void Main(string[] args)
    {
        OSCCollar _;
        if (File.Exists(ConfigFilePath))
        {
            Config? config = JsonConvert.DeserializeObject<Config>(File.ReadAllText(ConfigFilePath));
            if(config is { })
            {
                _ = new OSCCollar(config);
            }
            else
            {
                _ = new OSCCollar();
            }
        }else
        {
            _ = new OSCCollar();
        }
    }

    private readonly Config _config;
    private readonly OSC _osc;
    
    private Task _consoleOutputTask;
    
    private string _avatarId = "";
    private float _leashStretch;
    private readonly Vector _unitVectorLeash = new(1, 0);
    private Vector _movementVector = new(0, 0);
    private bool _allowMoving = true;
    private DateTime _lastNilMessageSent = DateTime.UnixEpoch;
    private readonly DateTime _programStarted = DateTime.Now;
    private static readonly TimeSpan UpdateInterval = TimeSpan.FromMilliseconds(10);
    private static readonly TimeSpan MessageMinInterval = TimeSpan.FromMilliseconds(400);
    private GPS _gps1 = null!, _gps2 = null!, _gps3 = null!;
    private readonly Queue<ValueTuple<double, double>> _calibrationAverage = new();
    private double _constantA, _constantB, _constantC, _constantD, _constantE;

    private OSCCollar(Config config, bool skipSetup = true)
    {
        this._config = config;
        File.WriteAllText(ConfigFilePath, JsonConvert.SerializeObject(this._config, Formatting.Indented));

        this._osc = new OSC("OSCCollar", new Dictionary<string, Type>()
        {
            { "/avatar/parameters/GPS1", typeof(float) },
            { "/avatar/parameters/GPS2", typeof(float) },
            { "/avatar/parameters/GPS3", typeof(float) },
            { "/avatar/parameters/Leash_Stretch", typeof(float) },
            { "/avatar/parameters/Leash_Toggle", typeof(bool) },
        });
        this._osc.OnParameterChangeEvent += OnOscParameterChangeEvent;
        
        this.SetupGPSVars();
        if (!skipSetup)
        {
            Console.WriteLine(Resources.OSCCollar_OSCCollar_Position_your_GPS_receivers);
            Console.WriteLine(Resources.OSCCollar_OSCCollar_GPS_1_position, _gps1.X, _gps1.Y);
            Console.WriteLine(Resources.OSCCollar_OSCCollar_GPS_2_position, _gps2.X, _gps2.Y);
            Console.WriteLine(Resources.OSCCollar_OSCCollar_GPS_3_position, _gps3.X, _gps3.Y);
            Console.WriteLine(Resources.OSCCollar_OSCCollar_Radius_of_each_receiver, this._config.Radius * 2);
            Console.ReadKey();
        }
        
        Thread runningThread = new(RunningThread);
        runningThread.Start();
    }

    private void OnOscParameterChangeEvent(string endpoint, object? oldvalue, object? newvalue)
    {
        switch (endpoint)
        {
            case "/avatar/parameters/GPS1":
                this._gps1.Distance = 1 - (float)(newvalue ?? 1);
                break;
            case "/avatar/parameters/GPS2":
                this._gps2.Distance = 1 - (float)(newvalue ?? 1);
                break;
            case "/avatar/parameters/GPS3":
                this._gps3.Distance = 1 - (float)(newvalue ?? 1);
                break;
            case "/avatar/parameters/Leash_Stretch":
                this._leashStretch = (float)(newvalue ?? 0);
                break;
            case "/avatar/parameters/Leash_Toggle":
                this._allowMoving = (bool)(newvalue ?? false);
                break;
            case "/avatar/change":
                this._avatarId = (string)(newvalue ?? "");
                break;
        }
    }

    private OSCCollar(string ip = "127.0.0.1", int portSend = 9000, double radius = 100, double walkStretchDeadzone = 0.1, double runStretch = 0.4, int calibrationValues = 128,
        bool skipSetup = true) : this(new Config()
        {
            Ip = ip,
            PortSend = portSend,
            WalkStretchDeadzone = walkStretchDeadzone,
            RunStretch = runStretch,
            Radius = radius,
            CalibrationValues = calibrationValues
        }, skipSetup)
    {
        
    }

    #region Setup
    private void SetupGPSVars()
    {
        this._gps1 = new(0, -this._config.Radius);
        this._gps2 = new(-(this._config.Radius * 0.866), this._config.Radius / 2);
        this._gps3 = new(this._config.Radius * 0.866, this._config.Radius / 2);
        this._constantA = 2 * _gps2.X - 2 * _gps1.X;
        this._constantB = 2 * _gps2.Y - 2 * _gps1.Y;
        this._constantC = Math.Pow(_gps1.X, 2) + Math.Pow(_gps2.X, 2) - Math.Pow(_gps1.Y, 2) + Math.Pow(_gps2.Y, 2);
        this._constantD = 2 * _gps3.X - 2 * _gps2.X;
        this._constantE = Math.Pow(_gps2.X, 2) + Math.Pow(_gps3.X, 2) - Math.Pow(_gps2.Y, 2) + Math.Pow(_gps3.Y, 2);
    }
    #endregion

    private void CalculatePositionFromGPS()
    {
        if (this._gps1.Distance == 0 || this._gps2.Distance == 0 || this._gps3.Distance == 0)
            return;
        double gpsFactorA = Math.Pow(_gps1.Distance, 2) - Math.Pow(_gps2.Distance, 2) - _constantC;
        double gpsFactorB = Math.Pow(_gps2.Distance, 2) - Math.Pow(_gps3.Distance, 2) - _constantE;

        double gpsPosX = (gpsFactorB / _constantD);
        double gpsPosY = -1 * (gpsFactorA * _constantD - _constantA * gpsFactorB) / (_constantB * _constantD);

        if (this._leashStretch == 0)
            AddCalibrationValues(gpsPosX, gpsPosY);
        
        ValueTuple<double, double> calibrationValues = GetCalibrationValues();
        
        double posX = gpsPosX + calibrationValues.Item1;
        double posY = gpsPosY + calibrationValues.Item2;
        
        double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2));

        _unitVectorLeash.X = inverseLength is not Double.PositiveInfinity ? posX * inverseLength : 0;
        _unitVectorLeash.Y = inverseLength is not Double.PositiveInfinity ? posY * inverseLength : 0;
        
        if (_leashStretch < this._config.WalkStretchDeadzone) //Below Deadzone
            _movementVector = new();
        else if (_leashStretch < this._config.RunStretch) //Walk
            _movementVector = _unitVectorLeash.MultipliedWith(0.5);
        else //Run
            _movementVector = _unitVectorLeash.MultipliedWith(1);
    }

    private void RunningThread()
    {
        while (true)
        {
            CalculatePositionFromGPS();
            if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now)
            {
                this._osc.Client.Send("/input/Vertical", 0f);
                this._osc.Client.Send("/input/Horizontal", 0f);
                this._lastNilMessageSent = DateTime.Now;
            }else if (_allowMoving)
            {
                this._osc.Client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y));
                this._osc.Client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X));
            }
            
            PrintConsole();
            Thread.Sleep(UpdateInterval);
        }
    }

    private void AddCalibrationValues(double x, double y)
    {
        this._calibrationAverage.Enqueue(new ValueTuple<double, double>(x, y));
        while (this._calibrationAverage.Count > this._config.CalibrationValues)
            this._calibrationAverage.Dequeue();
    }

    private ValueTuple<double, double> GetCalibrationValues()
    {
        if (this._calibrationAverage.Count < 1)
            return new ValueTuple<double, double>(0, 0);
        double averageX = -this._calibrationAverage.Average(value => value.Item1);
        double averageY = -this._calibrationAverage.Average(value => value.Item2);
        return new ValueTuple<double, double>(averageX, averageY);
    }
}