OSCCollar/OSCCollar/OSCCollar.cs

using System.Net;
using BuildSoft.OscCore;
using Newtonsoft.Json;
using VRC.OSCQuery;
using Extensions = VRC.OSCQuery.Extensions;

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 Config _config;
    private readonly int _portReceive;

    private OSCQueryService? OscQueryService { get; set; }
    private OscServer _server = null!;
    private OscClient _client = null!;
    private Task _consoleOutputTask;
    
    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 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.SetupGPSVars();
        if (!skipSetup)
        {
            Console.WriteLine("Position your GPS receivers:");
            Console.WriteLine($"GPS 1 x: {_gps1.X} y: {_gps1.Y}");
            Console.WriteLine($"GPS 2 x: {_gps2.X} y: {_gps2.Y}");
            Console.WriteLine($"GPS 3 x: {_gps3.X} y: {_gps3.Y}");
            Console.WriteLine($"Radius of each receiver (sphere): {this._config.Radius * 2}");
            Console.ReadKey();
        }
        
        this._portReceive = Extensions.GetAvailableUdpPort();
        this.SetupOSCServer();
        this.SetupOSCQuery();
        this._consoleOutputTask = this.SetupConsoleOutput();
    }

    private OSCCollar(string ip = "127.0.0.1", int portSend = 9000, double radius = 100, double calibrationX = 0,
        double calibrationY = 0, double walkStretchDeadzone = 0.1, double runStretch = 0.4,
        bool skipSetup = true) : this(new Config()
        {
            Ip = ip,
            PortSend = portSend,
            WalkStretchDeadzone = walkStretchDeadzone,
            RunStretch = runStretch,
            Radius = radius,
            CalibrationX = calibrationX,
            CalibrationY = calibrationY
        }, 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);
    }

    private void SetupOSCServer()
    {
        this._server = new OscServer(this._portReceive);
        this._client = new OscClient(this._config.Ip, this._config.PortSend);
        this._server.TryAddMethod("/avatar/parameters/GPS1", GPS1Handle);
        this._server.TryAddMethod("/avatar/parameters/GPS2", GPS2Handle);
        this._server.TryAddMethod("/avatar/parameters/GPS3", GPS3Handle);
        this._server.TryAddMethod("/avatar/parameters/Leash_Stretch", CollarStretchHandle);
        this._server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle);
        this._server.Start();

        Thread runningThread = new Thread(RunningThread);
        runningThread.Start();
    }
    
    private void SetupOSCQuery()
    {
        var tcpPort = Extensions.GetAvailableTcpPort();
        this.OscQueryService = new OSCQueryServiceBuilder()
            .WithHostIP(IPAddress.Parse(this._config.Ip))
            .WithOscIP(IPAddress.Parse(this._config.Ip))
            .WithDiscovery(new MeaModDiscovery())
            .WithTcpPort(tcpPort)
            .WithUdpPort(this._portReceive)
            .WithServiceName("Collar")
            .StartHttpServer()
            .AdvertiseOSC()
            .AdvertiseOSCQuery()
            .Build();
        this.OscQueryService.AddEndpoint<float>("/avatar/parameters/GPS1", Attributes.AccessValues.WriteOnly);
        this.OscQueryService.AddEndpoint<float>("/avatar/parameters/GPS2", Attributes.AccessValues.WriteOnly);
        this.OscQueryService.AddEndpoint<float>("/avatar/parameters/GPS3", Attributes.AccessValues.WriteOnly);
        this.OscQueryService.AddEndpoint<float>("/avatar/parameters/Leash_Stretch", Attributes.AccessValues.WriteOnly);
        this.OscQueryService.AddEndpoint<bool>("/avatar/parameters/Leash_Toggle", Attributes.AccessValues.WriteOnly);
        this.OscQueryService.RefreshServices();
    }
    #endregion

    #region Handle OSC-Messages
    private void AllowMovingHandle(OscMessageValues messageValues)
    {
        this._allowMoving = messageValues.ReadBooleanElement(0);
    }

    private void GPS1Handle(OscMessageValues messageValues)
    {
        this._gps1.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
    }
    
    private void GPS2Handle(OscMessageValues messageValues)
    {
        this._gps2.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
    }
    
    private void GPS3Handle(OscMessageValues messageValues)
    {
        this._gps3.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
    }
    
    private void CollarStretchHandle(OscMessageValues messageValues)
    {
        this._leashStretch = messageValues.ReadFloatElement(0);
    }
    #endregion

    private void CalculatePositionFromGPS()
    {
        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 = (gpsFactorA * _constantD - _constantA * gpsFactorB) / (_constantB * _constantD);

        AddCalibrationValues(gpsPosX, gpsPosY);
        
        double posX = gpsPosX + this._config.CalibrationX;
        double posY = gpsPosY + this._config.CalibrationY;
        
        double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2));

        _unitVectorLeash.X = posX * inverseLength * -1;
        _unitVectorLeash.Y = posY * inverseLength;
        
        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)
        {
            this._server.Update();
            CalculatePositionFromGPS();
            if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now)
            {
                this._client.Send("/input/Vertical", 0f);
                this._client.Send("/input/Horizontal", 0f);
                this._lastNilMessageSent = DateTime.Now;
            }else if (_allowMoving)
            {
                this._client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y));
                this._client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X));
            }
            
            Thread.Sleep(UpdateInterval);
        }
    }

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

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