using DTTest;
using Newtonsoft.Json;
using Newtonsoft.Json.Linq;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

/// <summary>
/// 
/// </summary>
namespace ModelBasic
{
    /// <summary>
    /// 
    /// </summary>
    public class ParsingData
    {

        /// <summary>
        /// 在Json中得到Base、Tool、Tcp位置
        /// 2021.1.15
        /// </summary>
        /// <param name="jsonStr">Json</param>
        /// <param name="threejsBase">xyz rpy</param>
        /// <param name="threejsTool">xyz qxqyqzqw</param>
        /// <param name="threejsTcp">xyz qxqyqzqw</param>
        /// <param name="threejsInitJoint">返回初始机器人姿态坐标值，传出的threejsInitJoint 是rad 和m</param>
        public static void GetRobotParameter_MM(Data_RobotCurr robotCurr, out double[] threejsBaseQ, out double[] threejsToolQ, out double[] threejsTcpQ,out double[] threejsInitJoint)
        {
            threejsBaseQ = new double[] { 
                CVT.MToMM(robotCurr.Base.tx),
                CVT.MToMM(robotCurr.Base.ty), 
                CVT.MToMM(robotCurr.Base.tz),
                robotCurr.Base.qx,
                robotCurr.Base.qy,
                robotCurr.Base.qz,
                robotCurr.Base.qw};
            threejsToolQ = new double[] {
                CVT.MToMM(robotCurr.Tool.tx),
                CVT.MToMM(robotCurr.Tool.ty),
                CVT.MToMM(robotCurr.Tool.tz),
                robotCurr.Tool.qx,
                robotCurr.Tool.qy,
                robotCurr.Tool.qz,
                robotCurr.Tool.qw};
            threejsTcpQ = new double[] {
                CVT.MToMM(robotCurr.Tcp.tx),
                CVT.MToMM(robotCurr.Tcp.ty),
                CVT.MToMM(robotCurr.Tcp.tz),
                robotCurr.Tcp.qx,
                robotCurr.Tcp.qy,
                robotCurr.Tcp.qz,
                robotCurr.Tcp.qw};

            #region 处理初始joint
           
            var currentJoint = robotCurr.CurrentJoint;
            var jointNum = currentJoint.Count;

            threejsInitJoint = new double[jointNum];

            for (int i = 0; i < jointNum; i++)
            {
                var item = currentJoint[i];
                double value = Convert.ToDouble(item.JValue);
                // 转动副 从弧度转换为角度
                if (item.JType == "0")
                {
                    value = value * 180 / Math.PI;
                }
                else // 滑动副 从m转换为mm
                {
                    //value = value * 1000;
                }
                threejsInitJoint[i] = value;
            }
           
           

            #endregion
        }




        /// <summary>
        /// 
        /// </summary>
        /// <param name="dataSplit"></param>
        /// <param name="items"></param>
        /// <returns></returns>
        public static JObject GetPositionDataInfo(string jStr)
        {
            var itemsArray = (JArray)JsonConvert.DeserializeObject(jStr);
            var jObjectParam = itemsArray[0].ToString();
            return (JObject)JsonConvert.DeserializeObject(jObjectParam);
        }



    }
}