using Newtonsoft.Json;
using System.Data;
using System.Net.WebSockets;

using System.Text;
using System.Text.Json;
using System.Text.Json.Nodes;


Console.Write("Enter Jibo IP: ");
var jiboIp = (Console.ReadLine() ?? "").Trim();


if (string.IsNullOrWhiteSpace(jiboIp))
{
    Console.WriteLine("No IP entered.");
    return;
}

var wsAxisState = new Uri($"ws://{jiboIp}:8282/axis_state");
var wsAxisCommand = new Uri($"ws://{jiboIp}:8282/axis_command");

using var cts = new CancellationTokenSource();

Console.WriteLine($"Connecting to Jibo at {jiboIp}...");
Console.WriteLine("Press Ctrl+C to quit.");

Console.CancelKeyPress += (_, e) =>
{
    e.Cancel = true;
    cts.Cancel();
};


while (!cts.IsCancellationRequested)
{
    Console.Write("Enter Jibo new Pose Body <Neck,Torso,Pelvis> in degress: 0-359, 0-359, 0-359 ");
    var positionString = (Console.ReadLine() ?? "").Trim();
    float[] position = [.. positionString.Split(',').Select(s => float.TryParse(s.Trim(), out var val) ? val : 0)];

    if (string.IsNullOrWhiteSpace(positionString))
    {
        Console.WriteLine("Invalid Jibo Position entered.");
        continue;
    }

    JiboBodyControl js = new();

    try
    {
        js.PositionRobot(position[0], position[1], position[2]);
        js.Start(jiboIp);
    }
    catch (Exception ex)
    {
        Console.WriteLine($"Error: {ex.Message}");
    }


    Console.ReadLine();
    js?.StopAsync().Wait();
}

return;

public struct RobotBodyPart
{
    [JsonProperty(PropertyName = "ts")]
    public List<float>? Ts { get; set; }
    [JsonProperty(PropertyName = "pos")]
    public float Pos { get; set; }
    [JsonProperty(PropertyName = "inc_pos")]
    public float IncPos { get; set; }
    [JsonProperty(PropertyName = "vel")]
    public float Vel { get; set; }
    [JsonProperty(PropertyName = "cur")]
    public float Cur { get; set; }
    [JsonProperty(PropertyName = "pwm")]
    public float Pwm { get; set; }
    [JsonProperty(PropertyName = "status")]
    public int Status { get; set; }
    [JsonProperty(PropertyName = "vel_limit")]
    public float VelLimit { get; set; }
    [JsonProperty(PropertyName = "acc_limit")]
    public float AccLimit { get; set; }
    [JsonProperty(PropertyName = "cur_limit")]
    public float CurLimit { get; set; }
    [JsonProperty(PropertyName = "mode")]
    public int Mode { get; set; }
    [JsonProperty(PropertyName = "ref")]
    public float Ref { get; set; }
    [JsonProperty(PropertyName = "ref_pos")]
    public float RefPos { get; set; }
    [JsonProperty(PropertyName = "ref_acc")]
    public float RefAcc { get; set; }
    [JsonProperty(PropertyName = "ticks")]
    public int Ticks { get; set; }
    [JsonProperty(PropertyName = "integrator")]
    public float Integrator { get; set; }
    [JsonProperty(PropertyName = "fault_status")]
    public int FaultStatus { get; set; }
    [JsonProperty(PropertyName = "index_count")]
    public int IndexCount { get; set; }
    [JsonProperty(PropertyName = "limiter_mode")]
    public int LimiterMode { get; set; }
}

public sealed class BodyState
{
    [JsonProperty(PropertyName = "ts")]
    public List<float>? Ts { get; set; }

    [JsonProperty(PropertyName = "pelvis")]
    public RobotBodyPart Pelvis { get; set; }

    [JsonProperty(PropertyName = "neck")]
    public RobotBodyPart Neck { get; set; }

    [JsonProperty(PropertyName = "torso")]
    public RobotBodyPart Torso { get; set; }

    [JsonProperty(PropertyName = "lockout")]
    public float Lockout { get; set; }

    [JsonProperty(PropertyName = "limiter_torque")]
    public float LimiterTorque { get; set; }

    [JsonProperty(PropertyName = "limiter_lookahead_torque")]
    public float LimiterLookaheadTorque { get; set; }

}

public static class Interval
{
    public static System.Timers.Timer Set(Action action, int interval)
    {
        var timer = new System.Timers.Timer(interval);
        timer.Elapsed += (s, e) => {
            timer.Enabled = false;
            action();
            timer.Enabled = true;
        };
        timer.Enabled = true;
        return timer;
    }

    public static void Stop(System.Timers.Timer timer)
    {
        timer.Stop();
        timer.Dispose();
    }
}

public sealed class JiboBodyControl
{
    const int BREAK = 2;
    const int POS_VEL = 7;
    const int VEL = 4;

    public BodyState? JiboBodyState { get; private set; } = null;

    bool PelvisPositioned = false;
    bool TorsoPositioned = false;
    bool NeckPositioned = false;
    bool Finished = true;

    private float[] PoseArray = new float[3];

    System.Timers.Timer? IndexCheckInterval;
    System.Timers.Timer? SpinInterval;

    Uri? wsAxisState;
    Uri? wsAxisCommand;

    readonly CancellationTokenSource _cts = new();
    ClientWebSocket? _ClientWebSocket = new();
    ClientWebSocket? _ClientControlWebSocket = new();

    public void PositionRobot(float neckPos, float torsoPos, float pelvisPos)
    {
        //pose array, in degrees
        var poseArray_deg = new float[3];
        poseArray_deg[0] = neckPos;
        poseArray_deg[1] = torsoPos;
        poseArray_deg[2] = pelvisPos;

        //pose array, converted to radian
        PoseArray = DegToRad(poseArray_deg);
    }


    public void Start(string jiboIp)
    {
        PelvisPositioned = false;
        TorsoPositioned = false;
        NeckPositioned = false;
        Finished = false;
        JiboBodyState = null;

        int tryCount = 5;
        var host = jiboIp + ":8282";

        do
        {
            int delay = 1000;
            if (JiboBodyState == null)
            {
                Console.WriteLine("Waiting for initial body state...");
                Task.Delay(delay, _cts.Token).Wait();
                JiboBodyState = SetupStateSocketAsync(host).Result;
                UpdateStatus();
            }
        }
        while (JiboBodyState == null && !_cts.Token.IsCancellationRequested && --tryCount > 0);

        if (JiboBodyState != null)
        {
            if (SetupCommandSocket(host).Result)
            {
                Console.WriteLine("Initial body state received. Ready for control loop...");
                ControlLoop();
            }
            else
            {
                Console.WriteLine("Failed to connect to command socket. Control loop will not start.");
                Finished = true;
            }
        }
        else
        {
            Console.WriteLine("Failed to receive initial body state. Control loop will not start.");
            Finished = true;
        }
    }

    public async Task StopAsync()
    {
        var wsCloseTask = Task.Run(async () =>
        {
            _cts.Cancel();
            if (_ClientWebSocket != null && _ClientWebSocket.State == WebSocketState.Open)
            {
                await _ClientWebSocket.CloseAsync(WebSocketCloseStatus.NormalClosure, null, default);
            }
            wsAxisState = null;
            _ClientWebSocket = null;
            Finished = true;
            JiboBodyState = null;
        });

        await Task.WhenAny(wsCloseTask, Task.Delay(TimeSpan.FromSeconds(30), _cts.Token));
        Console.WriteLine("WebCommunications have been shut down");

        if (IndexCheckInterval != null)
        {
            Interval.Stop(IndexCheckInterval);
            IndexCheckInterval = null;
        }
        if (SpinInterval != null)
        {
            Interval.Stop(SpinInterval);
            SpinInterval = null;
        }
    }

    private async Task<bool> SetupCommandSocket(string host)
    {
        if (wsAxisCommand == null)
        {
            wsAxisCommand = new Uri($"ws://{host}/axis_command");
        }
        if (_ClientControlWebSocket == null || _ClientControlWebSocket.State != WebSocketState.Open)
        {
            _ClientControlWebSocket = null;
            _ClientControlWebSocket = new ClientWebSocket();
            _ClientControlWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(60);
        }
        try
        {
            if (_ClientControlWebSocket.State != WebSocketState.Open)
            {
                await _ClientControlWebSocket.ConnectAsync(wsAxisCommand, _cts.Token);
                Console.WriteLine("Command WebSocket connected.");
            }
            else
            {
                Console.WriteLine("Could not Open Command WebSocket.");
                return false;
            }

            return _ClientControlWebSocket != null && _ClientControlWebSocket.State == WebSocketState.Open;
        }
        catch (Exception ex)
        {
            Console.WriteLine($"Couldn\'t connect to body service\'s command websocket: {ex.Message}");
        }

        return false;
    }

    private async Task<BodyState?> SetupStateSocketAsync(string host)
    {
        if (wsAxisState == null)
        {
            wsAxisState = new Uri($"ws://{host}/axis_state");
        }
        if (_ClientWebSocket == null || _ClientWebSocket.State != WebSocketState.Open)
        {
            _ClientWebSocket = null;
            _ClientWebSocket = new ClientWebSocket();
            _ClientWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(60);
        }
           
        try
        {
            if (_ClientWebSocket.State != WebSocketState.Open)
            {
                await _ClientWebSocket.ConnectAsync(wsAxisState, _cts.Token);
                Console.WriteLine("WebSocket connected.");
            }

            var buffer = new byte[8192];

            if (_ClientWebSocket != null && _ClientWebSocket.State == WebSocketState.Open)
            {
                WebSocketReceiveResult result;
                using var ms = new MemoryStream();
                var buff = new ArraySegment<byte>(new byte[8192]);

                do
                {
                    result = await _ClientWebSocket.ReceiveAsync(buffer, _cts.Token);

                    if (result.MessageType == WebSocketMessageType.Close)
                    {
                        wsAxisState = null;
                        Console.WriteLine("WebSocket closed by server.");
                        return null;
                    }

                    ms.Write(buffer, 0, result.Count);
                }
                while (!result.EndOfMessage);

                string json = Encoding.UTF8.GetString(ms.ToArray());
                Console.WriteLine("Data:" + json);

                BodyState? bodyState = null;

                var node = JsonNode.Parse(json);
                if (node != null && node?["ts"]?[0]?.GetValue<float>() != 0)
                {
                    using (var stringReader = new StringReader(json))
                    {
                        using var jsonReader = new JsonTextReader(stringReader);
                        var serializer = new Newtonsoft.Json.JsonSerializer();
                        try
                        {
                            bodyState = serializer.Deserialize<BodyState>(jsonReader);
                        }
                        catch (Exception ex)
                        {
                            Console.WriteLine($"Error deserializing JSON: {ex.Message}");
                            bodyState = null;
                        }
                    }
                }

                return bodyState;
            }
            else
            {
                Console.WriteLine("Could not Open WebSocket.");
            }
        }
        catch (Exception ex)
        {
            Console.WriteLine($"Couldn\'t connect to body service\'s state websocket: {ex.Message}");
        }

        return null;
    }

    private void ControlLoop()
    {
        if (!_cts.Token.IsCancellationRequested)
        {
            if (JiboBodyState != null)
            {
                Task.Delay(500, _cts.Token).Wait();

                if (Finished)
                {
                    Console.WriteLine("Jibo was already positioned.");
                    StopAsync().Wait();
                }
                else
                {
                    Console.WriteLine("Positioning robot...");
                    StartSpinningJibo();
                    IndexCheckInterval = Interval.Set(() => {
                        if (Finished)
                        {
                            StopAsync().Wait();
                        }
                    }, 100);
                }
            }
        }
    }

    private void StartSpinningJibo()
    {
        if (_ClientControlWebSocket != null && _ClientControlWebSocket.State == WebSocketState.Open)
        {
            SpinInterval = Interval.Set(() => {

                string? commandJson = BuildCommands();
                if (commandJson != null)
                {
                    var bytes = Encoding.UTF8.GetBytes(commandJson);
                    var buffer = new ArraySegment<byte>(bytes);
                    try
                    {
                        _ClientControlWebSocket.SendAsync(buffer, WebSocketMessageType.Binary, true, _cts.Token).Wait();
                        Console.WriteLine("Command sent: " + commandJson);
                    }
                    catch (Exception ex)
                    {
                        Console.WriteLine($"Error sending command: {ex.Message}");
                    }
                }
            }, 1000);
        }
        else
        {
            Console.WriteLine("Cannot send command, control WebSocket is not open.");
        }
    }

    private float[] DegToRad(float[] poseArray)
    {
        for (int i = 0; i < poseArray.Length; ++i)
        {
            poseArray[i] = float.DegreesToRadians(poseArray[i]);
        }
        return poseArray;
    }

    public void UpdateStatus()
    {
        if (JiboBodyState != null)
        {
            var pelvis = JiboBodyState.Pelvis;
            var torso = JiboBodyState.Torso;
            var neck = JiboBodyState.Neck;

            var tolerance = 0.1;
            PelvisPositioned = (Math.Abs(Math.Abs(pelvis.Pos) -
                Math.Abs(PoseArray[2])) < tolerance);
            TorsoPositioned = (Math.Abs(Math.Abs(torso.Pos) -
                Math.Abs(PoseArray[1])) < tolerance);
            NeckPositioned = (Math.Abs(Math.Abs(neck.Pos) -
                Math.Abs(PoseArray[0])) < tolerance);

            Finished = false;
        }
        else
        {
            Finished = true;
        }
    }

    private string? BuildCommands()
    {
        if (JiboBodyState == null)
        {
            return null;
        }

        Console.WriteLine("Starting positioning sequence...");
        string vpelvis;
        string vtorso;
        string vneck;

        int val = 0;
        int mode;

        if (!PelvisPositioned)
        {
            mode = POS_VEL;
        }
        else
        {
            mode = BREAK;
        }
        vpelvis = Cmd(mode, val, PoseArray[2], JiboBodyState.Pelvis.Pos, JiboBodyState.Pelvis);

        if (!TorsoPositioned)
        {
            mode = POS_VEL;
        }
        else
        {
            mode = BREAK;
        }
        vtorso = Cmd(mode, val, PoseArray[1], JiboBodyState.Torso.Pos, JiboBodyState.Torso);

        if (!NeckPositioned)
        {
            mode = POS_VEL;
        }
        else
        {
            mode = BREAK;
        }
        vneck = Cmd(mode, val, PoseArray[0], JiboBodyState.Neck.Pos, JiboBodyState.Neck);

        StringBuilder cmd = new StringBuilder();
        cmd.Append('{');
        cmd.AppendFormat("\"ts\": [{0}, {1}],", JiboBodyState.Ts?[0], JiboBodyState.Ts?[1]);
        cmd.AppendFormat("\"pelvis\": {0},", vpelvis);
        cmd.AppendFormat("\"torso\": {0},", vtorso);
        cmd.AppendFormat("\"neck\": {0}", vneck);
        cmd.Append('}');

        return cmd.ToString();
    }

     private string Cmd(int mode, int vel, float desPos, float actPos, RobotBodyPart state)
    {
        StringBuilder cmd = new StringBuilder();
        cmd.Append('{');
        cmd.AppendFormat("\"mode\": {0},", mode);
        cmd.AppendFormat("\"value\": [{0}, {1}],", vel, desPos);
        cmd.AppendFormat("\"vel_limit\": {0},", state.VelLimit);
        cmd.AppendFormat("\"acc_limit\": {0},", state.AccLimit);
        cmd.AppendFormat("\"cur_limit\": {0}", state.CurLimit);
        cmd.Append('}');

        return cmd.ToString();
    }

}