using System;

using Godot;

​

public partial class BodySolver : Node

{

​

}

​

public abstract partial class BodyPartSolver : Node

{

    public abstract void Update(BodySolver Solver);

}

public struct Vector3 

{

    public float X;

    public float Y;

    public float Z;

    public Vector3(float _x, float _y, float _z)

    {

        X = _x;

        Y = _y;

        Z = _z;

    }

}

​

public struct Basis

{

}

​

public struct Quaternion

{

    public float X;

    public float Y;

    public float Z;

    public float W;

    public Quaternion(float _x, float _y, float _z, float _w)

    {

        X = _x;

        Y = _y;

        Z = _z;

        W = _w;

    }

}

​

//central bone chain

interface IEyesSolver

{

    public Vector3 GetEyesPos();

    public Basis GetEyesBas();

}

interface INeckSolver

{

    public Vector3 GetNeckPos();

    public Basis GetNeckBas();

}

interface IChestSolver

{

    public Vector3 GetChestPos();

    public Basis GetChestBas();

}

interface ISpineSolver

{

    public Vector3 GetSpinePos();

    public Basis GetSpineBas();

}

interface ILHipsSolver

{

    public Vector3 GetLHipsPos();

    public Basis GetLHipsBas();

}

interface IRHipsSolver

{

    public Vector3 GetRHipsPos();

    public Basis GetRHipsBas();

}

​

//shoulders

interface ILShoulderSolver

{

    public Vector3 GetLShoulderPos();

    public Basis GetLShoulderBas();

}

interface IRShoulderSolver

{

    public Vector3 GetRShoulderPos();

    public Basis GetRShoulderBas();

}

//elbows

interface ILElbowSolver

{

    public Vector3 GetLElbowPos();

    public Basis GetLElbowBas();

}

interface IRElbowSolver

{

    public Vector3 GetRElbowPos();

    public Basis GetRElbowBas();

}

//wrists

interface ILWristSolver

{

    public Vector3 GetLWristPos();

    public Basis GetLWristBas();

}

interface IRWristSolver

{

    public Vector3 GetRWristPos();

    public Basis GetRWristBas();

}

​

//knees

interface ILKneeSolver

{

    public Vector3 GetLKneePos();

    public Basis GetLKneeBas();

}

interface IRKneeSolver

{

    public Vector3 GetRKneePos();

    public Basis GetRKneeBas();

}

​

//ankles

interface ILAnkleSolver

{

    public Vector3 GetLAnklePos();

    public Basis GetLAnkleBas();

}

interface IRAnkleSolver

{

    public Vector3 GetRAnklePos();

    public Basis GetRAnkleBas();

}

​

//toes

interface ILToeSolver

{

    public Vector3 GetLToePos();

    public Basis GetLToeBas();

}

interface IRToeSolver

{

    public Vector3 GetRToePos();

    public Basis GetRToeBas();

}

​

interface IFullbodySolver :

    IEyesSolver,

    INeckSolver,

    IChestSolver,

    ISpineSolver,

​

    ILShoulderSolver,

    ILElbowSolver,

    ILWristSolver,

​

    IRShoulderSolver,

    IRElbowSolver,

    IRWristSolver,

​

    ILHipsSolver,

    ILKneeSolver,

    ILAnkleSolver,

    ILToeSolver,

​

    IRHipsSolver,

    IRKneeSolver,

    IRAnkleSolver,

    IRToeSolver

{ }

​

public struct TrackedPoint 

{

    public Vector3 Pos = default;

    public Vector3 Vel = default;

    public Vector3 Acc = default;

    public Basis Bas = default;

    public Quaternion AngVel = default;

    public Quaternion AngAcc = default;

    public TrackedPoint()

    {

    }

}

​

public partial class WebSolver : BodyPartSolver, IFullbodySolver

{

    [Export] public string Address = "127.0.0.1";

    [Export] public int Port = 8080;

​

    //core chain

    private TrackedPoint _Eyes = default;

    private TrackedPoint _Neck = default;

    private TrackedPoint _Chest = default;

    private TrackedPoint _Spine = default;

    //left arm

    private TrackedPoint _LShoulder = default;

    private TrackedPoint _LElbow = default;

    private TrackedPoint _LWrist = default;

    //right arm

    private TrackedPoint _RShoulder = default;

    private TrackedPoint _RElbow = default;

    private TrackedPoint _RWrist = default;

    //left leg

    private TrackedPoint _LHips = default;

    private TrackedPoint _LKnee = default;

    private TrackedPoint _LAnkle = default;

    private TrackedPoint _LToe = default;

    //right leg

    private TrackedPoint _RHips = default;

    private TrackedPoint _RKnee = default;

    private TrackedPoint _RAnkle = default;

    private TrackedPoint _RToe = default;

    public override void Update(BodySolver Solver)

    {

        //extrapolate old data using the calculated velocity and acceleration

        //if request in progress

        //    use old data, return

        //if request failed

        //    use old data, return

        //if request finished

        //    prepare new points, calculate point data

    }

    public void GetPoseRequestFinished(long result, long responseCode, string[] headers, byte[] body)

    {

        if (result != (long)HTTPRequest.Result.Success)

        {

            GD.PushError("Couldn't get pose from server {0}:{1} ", Address, Port.ToString());

        }

        GD.Print("Recieved pose from server {0}:{1}", Address, Port.ToString());

        var json = new Json();

        json.Parse(body.GetStringFromUtf8());

        var response = json.GetData().AsGodotDictionary();

    }

​

    #region getters

    //core chain

    public Vector3 GetEyesPos()

    {

        return _Eyes.Pos;

    }

    public Vector3 GetNeckPos()

    {

        return _Neck.Pos;

    }

    public Vector3 GetChestPos()

    {

        return _Chest.Pos;

    }

    public Vector3 GetSpinePos()

    {

        return _Spine.Pos;

    }

    public Basis GetEyesBas()

    {

        return _Eyes.Bas;

    }

    public Basis GetNeckBas()

    {

        return _Neck.Bas;

    }

    public Basis GetChestBas()

    {

        return _Chest.Bas;

    }

    public Basis GetSpineBas()

    {

        return _Spine.Bas;

    }

​

    //left arm

    public Vector3 GetLShoulderPos()

    {

        return _LShoulder.Pos;

    }

    public Vector3 GetLElbowPos()

    {

        return _LElbow.Pos;

    }

    public Vector3 GetLWristPos()

    {

        return _LWrist.Pos;

    }

    public Basis GetLShoulderBas()

    {

        return _LShoulder.Bas;

    }

    public Basis GetLElbowBas()

    {

        return _LElbow.Bas;

    }

    public Basis GetLWristBas()

    {

        return _LWrist.Bas;

    }

​

    //right arm

    public Vector3 GetRShoulderPos()

    {

        return _RShoulder.Pos;

    }

    public Vector3 GetRElbowPos()

    {

        return _RElbow.Pos;

    }

    public Vector3 GetRWristPos()

    {

        return _RWrist.Pos;

    }

    public Basis GetRShoulderBas()

    {

        return _RShoulder.Bas;

    }

    public Basis GetRElbowBas()

    {

        return _RElbow.Bas;

    }

    public Basis GetRWristBas()

    {

        return _RWrist.Bas;

    }

​

    //left leg

    public Vector3 GetLHipsPos()

    {

        return _LHips.Pos;

    }

    public Vector3 GetLKneePos()

    {

        return _LKnee.Pos;

    }

    public Vector3 GetLAnklePos()

    {

        return _LAnkle.Pos;

    }

    public Vector3 GetLToePos()

    {

        return _LToe.Pos;

    }

    public Basis GetLHipsBas()

    {

        return _LHips.Bas;

    }

    public Basis GetLKneeBas()

    {

        return _LKnee.Bas;

    }

    public Basis GetLAnkleBas()

    {

        return _LAnkle.Bas;

    }

    public Basis GetLToeBas()

    {

        return _LToe.Bas;

    }

​

    //right leg

    public Vector3 GetRHipsPos()

    {

        return _RHips.Pos;

    }

    public Vector3 GetRKneePos()

    {

        return _RKnee.Pos;

    }

    public Vector3 GetRAnklePos()

    {

        return _RAnkle.Pos;

    }

    public Vector3 GetRToePos()

    {

        return _RToe.Pos;

    }

    public Basis GetRHipsBas()

    {

        return _RHips.Bas;

    }

    public Basis GetRKneeBas()

    {

        return _RKnee.Bas;

    }

    public Basis GetRAnkleBas()

    {

        return _RAnkle.Bas;

    }

    public Basis GetRToeBas()

    {

        return _RToe.Bas;

    }

    #endregion

}

​

public class Program

{

    public static void Main()

    {

        Console.WriteLine("Hello World");

    }

}