HDRobotics/Assets/Scripts/Model/ProgramModel.cs

using Newtonsoft.Json;
using Newtonsoft.Json.Linq;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Net.Http;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using UdpClientLib;
using UnityEngine;

public interface IProgramModel
{

}

public class ProgramModel : IProgramModel
{
    private string tcpBaseUrl;
    private string udpBaseUrl;
    HttpClient httpClient = new HttpClient();
    private SingleTcpClient tcpClient;
    private SingleUdpClient udpClient;
    public UdpClientManager manager = new UdpClientManager();

    private List<JobInfoDTO> allProgramsCache = new List<JobInfoDTO>();
    public RobotProgram CurrentProgram { get; private set; }
    private RobotData robotData;
    private RobotController robotController;

    private readonly object lockObject = new object();
    private bool hasNewData;
    public bool IsMoving;
    public bool isError;
    private Vector3 startMovementPosition;
    private int currentToolNum = 0;

    private readonly SemaphoreSlim clientLock = new SemaphoreSlim(1, 1);

    public ProgramModel(string hostip, int tcpPort, int udpPort, RobotController robotController)
    {
        tcpBaseUrl = $"http://{hostip}:{tcpPort}";
        udpBaseUrl = $"http://{hostip}:{udpPort}";
        _ = HandleAsyncWork(hostip, tcpPort, udpPort);
    }

    public async Task HandleAsyncWork(string hostip, int tcpPort, int udpPort)
    {
        tcpClient = new SingleTcpClient();
        await tcpClient.ConnectAsync("Tcp-Client", hostip, tcpPort);
        udpClient = manager.AddClient("Udp-client", hostip, udpPort);
        manager.PrintStatus();
    }

    public async Task InitializeAsync()
    {
        await LoadAllPrograms();
        hasNewData = false;
        IsMoving = false;
        isError = false;

        try
        {
            currentToolNum = await GetRobotToolNum();
        }
        catch (Exception e)
        {
            Debug.LogWarning($"<22><><EFBFBD><EFBFBD> <20><> <20><>ȣ<EFBFBD><C8A3> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD><DFBD>ϴ<EFBFBD>: {e.Message}. <20>⺻<EFBFBD><E2BABB>(0)<29><> <20><><EFBFBD><EFBFBD><EFBFBD>մϴ<D5B4>.");
            currentToolNum = 1;
        }

        return;
    }

    public bool IsNewDataAvailable()
    {
        lock (lockObject)
        {
            return hasNewData;
        }
    }

    public RobotData GetLatestRobotData()
    {
        lock (lockObject)
        {
            hasNewData = false; // <20><><EFBFBD><EFBFBD><EFBFBD>͸<EFBFBD> <20>о<EFBFBD><D0BE><EFBFBD><EFBFBD>Ƿ<EFBFBD> <20>÷<EFBFBD><C3B7>׸<EFBFBD> <20><><EFBFBD><EFBFBD>
            return robotData;
        }
    }

    /// <summary>
    /// <20><><EFBFBD>α׷<CEB1> <20><><EFBFBD><EFBFBD>/<2F>ҷ<EFBFBD><D2B7><EFBFBD><EFBFBD><EFBFBD> <20>ý<EFBFBD><C3BD><EFBFBD>
    /// </summary>

    public async Task<bool> CheckProgramExists(string jobProgramName)
    {
        string requestUri = $"{tcpBaseUrl}/file_manager/file_exist?pathname=project/jobs/{jobProgramName}";
        HttpResponseMessage result = await httpClient.GetAsync(requestUri);
        string jsonResponse = await result.Content.ReadAsStringAsync();

        return jsonResponse.Equals("true");
    }

    public async Task<bool> CreateNewProgram(string userInputId)
    {
        string robotModelName;

        if (string.IsNullOrEmpty(userInputId)) return false;

        string newProgramId = $"{userInputId}.job";

        if (await CheckProgramExists(newProgramId))
        {
            Debug.LogError("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>̹<EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>մϴ<D5B4>.");
            return false;
        }
        else
            Debug.Log($"{newProgramId} <20><><EFBFBD><EFBFBD>");

        try
        {
            robotModelName = await GetRobotModelNameAsync();
        }
        catch (Exception e)
        {
            Debug.LogError($"<22>κ<EFBFBD> <20>𵨸<EFBFBD><F0B5A8B8><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD><DFBD>ϴ<EFBFBD>: {e.Message}");
            return false;
        }

        NewJobRequestDTO newJob = new NewJobRequestDTO
        {
            fname = newProgramId,
            model_name = robotModelName,
            n_add_ax = 0
        };

        string jsonString = JsonConvert.SerializeObject(newJob);
        HttpContent jsonPayload = new StringContent(jsonString, Encoding.UTF8, "application/json");

        string requestUri = $"{tcpBaseUrl}/project/jobs/create_job";
        try
        {
            HttpResponseMessage result = await httpClient.PostAsync(requestUri, jsonPayload);
            if (result.IsSuccessStatusCode)
            {
                await LoadProgram(newProgramId);
                object payload = CmdLinePayload("end", 0);
                jsonString = JsonConvert.SerializeObject(payload);
                jsonPayload = new StringContent(jsonString, Encoding.UTF8, "application/json");

                HttpResponseMessage jsonResponse = await httpClient.PostAsync(
                    $"{tcpBaseUrl}/project/jobs/{CurrentProgram.ProgramId}/ins_cmd_line", jsonPayload);
                if (jsonResponse.IsSuccessStatusCode) 
                    return true;
                else 
                    return false;
            }
            else
                return false;
        }
        catch (Exception e)
        {
            Debug.LogError($"<22><><EFBFBD>α׷<CEB1> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>: {userInputId}, {e.Message}");
            return false;
        }
    }

    private async Task<string> GetRobotModelNameAsync()
    {
        string requestUri = $"{tcpBaseUrl}/project/rgen";

        HttpResponseMessage result = await httpClient.GetAsync(requestUri);
        string jsonResponse = await result.Content.ReadAsStringAsync();

        JObject data = JObject.Parse(jsonResponse);

        string modelName = (string)data.SelectToken("robot_model");

        if (string.IsNullOrEmpty(modelName))
        {
            throw new Exception("<22>κ<EFBFBD> <20><><EFBFBD><EFBFBD> API <20><><EFBFBD>信<EFBFBD><E4BFA1> <20>𵨸<EFBFBD><F0B5A8B8><EFBFBD> ã<><C3A3> <20><> <20><><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD>.");
        }

        return modelName;
    }

    public async Task<bool> GetRobotMotorStateAsync()
    {
        string requestUri = $"{tcpBaseUrl}/project/rgen";

        HttpResponseMessage result = await httpClient.GetAsync(requestUri);
        string jsonResponse = await result.Content.ReadAsStringAsync();

        JObject data = JObject.Parse(jsonResponse);

        int motorState = (int)data.SelectToken("enable_state");
        if (motorState == 0 || motorState == 256)
            return true;
        else if (motorState == 1)
            return false;
        else
        {
            throw new Exception("<22>κ<EFBFBD> <20><><EFBFBD><EFBFBD> API <20><><EFBFBD>信<EFBFBD><E4BFA1> <20><><EFBFBD><EFBFBD> <20><><EFBFBD>¸<EFBFBD> ã<><C3A3> <20><> <20><><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD>.");
        }
    }

    public async Task<int> GetRobotToolNum()
    {
        string requestUri = $"{tcpBaseUrl}/project/rgen";

        HttpResponseMessage result = await httpClient.GetAsync(requestUri);
        string jsonResponse = await result.Content.ReadAsStringAsync();

        JObject data = JObject.Parse(jsonResponse);

        int toolNum = (int)data.SelectToken("tool_no");

        return toolNum;
    }

    public async Task GetTCPAsync(CancellationToken token)
    {
        while (!token.IsCancellationRequested)
        {
            //await clientLock.WaitAsync(token);
            try
            {
                string requestUri = $"{tcpBaseUrl}/project/robot/po_cur";

                HttpResponseMessage result = await httpClient.GetAsync(requestUri);
                string jsonResponse = await result.Content.ReadAsStringAsync();

                var tempRobotData = JsonConvert.DeserializeObject<RobotData>(jsonResponse, new JsonSerializerSettings { CheckAdditionalContent = false });

                lock (lockObject)
                {
                    robotData = tempRobotData;
                    hasNewData = true;
                }
                await Task.Delay(50, token);
            }
            catch (Exception e)
            {
                Debug.Log(e);
                await Task.Delay(1000); // <20><><EFBFBD><EFBFBD> <20><> <20><> <20><> <20><><EFBFBD><EFBFBD>
            }
            finally
            {
                //clientLock.Release();
            }
        }
    }

    public async Task<bool> LoadProgram(string programId)
    {
        string requestUri = $"{tcpBaseUrl}/file_manager/files?pathname=project/jobs/{programId}&common";

        HttpResponseMessage result = await httpClient.GetAsync(requestUri);
        string rawTextContent = await result.Content.ReadAsStringAsync();

        if (string.IsNullOrEmpty(rawTextContent))
        {
            return false;
        }

        CurrentProgram = new RobotProgram(programId, rawTextContent);
        
        return true;
    }

    private async Task LoadAllPrograms()
    {
        allProgramsCache.Clear();
        string jsonResponse = null;
        string wrappedJson = null;

        try
        {
            HttpResponseMessage result = await httpClient.GetAsync($"{tcpBaseUrl}/project/jobs_info");
            jsonResponse = await result.Content.ReadAsStringAsync();

            wrappedJson = $"{{\"jobs\":{jsonResponse}}}";
            JobListWrapper wrapper = JsonUtility.FromJson<JobListWrapper>(wrappedJson);

            if (wrapper != null && wrapper.jobs != null)
            {
                allProgramsCache = wrapper.jobs;
            }
            else
            {
                Debug.LogError("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> job <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>Ľ<EFBFBD><C4BD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD>: " + jsonResponse);
            }
        }
        catch (ArgumentException e)
        {
            Debug.LogError($"JSON <20>Ľ<EFBFBD> <20><><EFBFBD><EFBFBD>: {e.Message}");
        }
        catch (Exception e)
        {
            Debug.LogError($"LoadAllPrograms <20><> <20><> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>: {e.Message}");
        }
    }

    public List<string> GetAllProgramIds()
    {
        List<string> ids = new List<string>();
        foreach (var jobInfo in allProgramsCache)
        {
            ids.Add(jobInfo.fname);
        }
        return ids;
    }

    /// <summary>
    /// <20>κ<EFBFBD> <20><>ġ <20><><EFBFBD><EFBFBD> <20>ý<EFBFBD><C3BD><EFBFBD>
    /// </summary>
    
    // <20><><EFBFBD>α׷<CEB1><D7B7><EFBFBD> <20><><EFBFBD>ɹ<EFBFBD> <20>߰<EFBFBD>
    public object CmdLinePayload(string cmd_line, int sno_ref)
    {
        object payload;
        if (cmd_line == "end")
        {
            payload = new
            {
                cmd_line = cmd_line,
                sno_ref = sno_ref
            };
        }
        else
        {
            payload = new
            {
                adjust_branch = true,
                cmd_line = cmd_line,
                fno_ref = -1,
                mechinfo = -1,
                move_cursor = 1,
                ofs = 0,
                save_file = true,
                set_cur_pose = 1,
                sno_ref = sno_ref
            };
        }
        return payload;
    }

    public async Task<bool> SavePointToProgramAsync(RobotData pointData, int index = -1)
    {
        if (CurrentProgram == null)
        {
            Debug.LogError("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>α׷<CEB1><D7B7><EFBFBD> <20>ε<EFBFBD><CEB5><EFBFBD><EFBFBD><EFBFBD> <20>ʾҽ<CABE><D2BD>ϴ<EFBFBD>.");
            return false;
        }

        string jsonPayload;
        string requestUri;
        HttpContent content;
        object payload;

        try
        {
            if (index == -1)
            {
                // sno_ref = <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>ȣ, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0
                int sno_ref = (CurrentProgram.Steps.Count > 0) ? CurrentProgram.Steps.Last().StepNumber : 0;

                payload = CmdLinePayload($"move P,spd=50%,accu=3,tool={currentToolNum}", sno_ref);

                requestUri = $"{tcpBaseUrl}/project/jobs/{CurrentProgram.ProgramId}/ins_cmd_line";
            }
            else
            {
                // sno = <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>ȣ
                int sno = CurrentProgram.Steps[index].StepNumber;

                payload = new
                {
                    mechinfo = -1,
                    save_file = true,
                    sno = sno
                };

                requestUri = $"{tcpBaseUrl}/project/jobs/{CurrentProgram.ProgramId}/pose_modify";
            }

            jsonPayload = JsonConvert.SerializeObject(payload);
            content = new StringContent(jsonPayload, Encoding.UTF8, "application/json");

            HttpResponseMessage result = await httpClient.PostAsync(requestUri, content);

            if (result.IsSuccessStatusCode)
            {
                // <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>ϸ<EFBFBD>, <20><><EFBFBD><EFBFBD> ij<>ÿ<EFBFBD><C3BF><EFBFBD> <20>ݿ<EFBFBD>
                if (index == -1)
                    CurrentProgram.AddStep(pointData, currentToolNum); // <20><><EFBFBD><EFBFBD> ij<>ÿ<EFBFBD> <20>߰<EFBFBD>
                else
                    CurrentProgram.UpdateStep(index, pointData); // <20><><EFBFBD><EFBFBD> ij<><C4B3> <20><><EFBFBD><EFBFBD>

                return true;
            }

            Debug.LogError($"SavePointToProgramAsync <20><><EFBFBD><EFBFBD>: {await result.Content.ReadAsStringAsync()}");
            return false;
        }
        catch (Exception e)
        {
            Debug.LogError($"<22><><EFBFBD><EFBFBD>Ʈ <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>: {e.Message}");
            return false;
        }
    }

    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>Ʈ <20><><EFBFBD><EFBFBD> <20><>û
    public async Task<bool> DeletePointFromProgramAsync(int index)
    {
        if (CurrentProgram == null || index < 0 || index >= CurrentProgram.Steps.Count)
        {
            Debug.LogError($"DeletePointFromProgramAsync: <20>߸<EFBFBD><DFB8><EFBFBD> <20>ε<EFBFBD><CEB5><EFBFBD> {index}");
            return false;
        }

        // sno_ref = <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>ȣ (0-based index<65><78> <20>ƴ<EFBFBD>)
        int sno_ref = CurrentProgram.Steps[index].StepNumber;

        var payload = new
        {
            adjust_branch = true,
            fno_ref = 0,
            ofs = 0,
            save_file = true,
            sno_ref = sno_ref
        };
        string jsonPayload = JsonConvert.SerializeObject(payload);
        HttpContent content = new StringContent(jsonPayload, Encoding.UTF8, "application/json");

        string requestUri = $"{tcpBaseUrl}/project/jobs/{CurrentProgram.ProgramId}/del_cmd_line";

        try
        {
            HttpResponseMessage result = await httpClient.PostAsync(requestUri, content);
            if (result.IsSuccessStatusCode)
            {
                // <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><>, <20><><EFBFBD><EFBFBD> ij<>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
                CurrentProgram.DeleteStep(index);
                return true;
            }

            Debug.LogError($"DeletePointFromProgramAsync <20><><EFBFBD><EFBFBD>: {await result.Content.ReadAsStringAsync()}");
            return false;
        }
        catch (Exception e)
        {
            Debug.LogError($"<22><><EFBFBD><EFBFBD>Ʈ <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>: {e.Message}");
            return false;
        }
    }

    // Ÿ<><C5B8> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>̵<EFBFBD>
    public async Task<bool> MoveToPoseTcpAsync(Vector3 position)
    {
        //await clientLock.WaitAsync();
        try
        {
            startMovementPosition.x = Convert.ToSingle(Math.Round(-1 * position.x * 1000, 2));
            startMovementPosition.y = Convert.ToSingle(Math.Round(-1 * position.z * 1000, 2));
            startMovementPosition.z = Convert.ToSingle(Math.Round(position.y * 1000, 2));

            string jsonResponse = await tcpClient.SendPostRequestAsync("/project/robot/move_to_pose_manual", $"{{\"pose_tg\":{{\"crd\":\"robot\",\"_type\":\"Pose\",\"mechinfo\":1,\"x\":{startMovementPosition.x},\"y\":{startMovementPosition.y},\"z\":{startMovementPosition.z}, \"rx\":{robotData.rx}, \"ry\":{robotData.ry}, \"rz\":{robotData.rz}}}}}");

            if (jsonResponse.Contains("200"))
            {
                Debug.Log("TCP POST (Move) <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>");
                IsMoving = true;
                return true;
            }
            else
            {
                Debug.LogError($"TCP POST (Move) <20><><EFBFBD><EFBFBD>");
                return false;
            }
        }
        catch (Exception e)
        {
            Debug.Log(e);
            return false;
        }
        finally
        {
            //clientLock.Release();
        }
    }

    // TCP POST <20>̵<EFBFBD> <20><><EFBFBD><EFBFBD> üũ
    public async Task StartMovementCheckLoopAsync(CancellationToken token)
    {
        try
        {
            while (!token.IsCancellationRequested)
            {
                if (IsMoving)
                {
                    await udpClient.SendFilledBytesAsync(new Dictionary<int, byte> { { 2, 0x20 } });
                    await Task.Delay(100);

                    RobotData currentPose = null;
                    lock (lockObject) { currentPose = this.robotData; }

                    if (currentPose != null)
                    {
                        bool isApproximatelyX = Mathf.Approximately(startMovementPosition.x, Convert.ToSingle(Math.Round(currentPose.x, 2)));
                        bool isApproximatelyY = Mathf.Approximately(startMovementPosition.y, Convert.ToSingle(Math.Round(currentPose.y, 2)));
                        bool isApproximatelyZ = Mathf.Approximately(startMovementPosition.z, Convert.ToSingle(Math.Round(currentPose.z, 2)));
                        if (isApproximatelyX && isApproximatelyY && isApproximatelyZ)
                        {
                            IsMoving = false;
                        }
                    }
                }
                else
                {
                    await Task.Delay(500, token);
                }
            }
        }
        catch (TaskCanceledException)
        {
            Debug.Log("MovementCheckLoopAsync Canceled.");
        }
        catch (Exception e)
        {
            Debug.LogError($"MovementCheckLoopAsync Error: {e.Message}\n{e.StackTrace}");
        }
    }

    //private async Task GetMovementState()
    //{
    //    while (!cancellationTokenSource.Token.IsCancellationRequested)
    //    {
    //        try
    //        {
    //            var jsonResponse = await tcpClient.SendGetRequestAsync("/project/rgen");
    //            var pasrsingJsonResponse = HttpResponseParser.ExtractJsonFromHttpResponse(jsonResponse);
    //            var tempRobotData = JsonConvert.DeserializeObject<RobotStateData>(pasrsingJsonResponse, new JsonSerializerSettings { CheckAdditionalContent = false });

    //            jsonResponse = await tcpClient.SendGetRequestAsync($"/logManager/search?cat_p=E&id_min={Convert.ToInt32(tempRobotData.eid_last_err) - 3}&id_max={tempRobotData.eid_last_err}");
    //            pasrsingJsonResponse = HttpResponseParser.ExtractJsonFromHttpResponse(jsonResponse);
    //            tempRobotData = JsonConvert.DeserializeObject<RobotStateData>(pasrsingJsonResponse, new JsonSerializerSettings { CheckAdditionalContent = false });

    //            isError = tempRobotData != null && tempRobotData.code != null && (tempRobotData.code.Contains("228") || tempRobotData.code.Contains("6037"));
    //            Debug.Log(isError);
    //            await Task.Delay(100);
    //        }
    //        catch (System.Exception)
    //        {
    //        }
    //    }
    //}

    public async Task<string> GetMovingState()
    {
        string jsonResponse = await tcpClient.SendGetRequestAsync("/project/robot/moving_to_pose_manual");
        string parsingJsonResponse = HttpResponseParser.ExtractJsonFromHttpResponse(jsonResponse);

        return parsingJsonResponse;
    }
}