using System;
using System.Collections.Generic;

/// <summary>
/// 
/// </summary>
public class CVT
{
    /// <summary>
    /// 
    /// </summary>
    /// <param name="M"></param>
    /// <returns></returns>
    public static double MToMM(object M)
    {
        return Math.Round(Convert.ToDouble(M.ToString()) * 1000, 2);
    }
    /// <summary>
    /// 
    /// </summary>
    /// <param name="MM"></param>
    /// <returns></returns>
    public static double MMToM(object MM)
    {
        return Math.Round(Convert.ToDouble(MM.ToString()) / 1000, 5);
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="angle"></param>
    /// <returns></returns>
    public static double AngleToRad(object angle)
    {
        return Math.Round(Convert.ToDouble(angle.ToString()) * Math.PI / 180, 5);
    }
    /// <summary>
    /// 
    /// </summary>
    /// <param name="rad"></param>
    /// <returns></returns>
    public static double RadToAngle(object rad)
    {
        return Math.Round(Convert.ToDouble(rad.ToString()) * 180 / Math.PI, 5);
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="M"></param>
    /// <returns></returns>
    public static double MToMM(string M)
    {
        return Math.Round(Convert.ToDouble(M) * 1000, 2);
    }
    /// <summary>
    /// 
    /// </summary>
    /// <param name="MM"></param>
    /// <returns></returns>
    public static double MMToM(string MM)
    {
        return Math.Round(Convert.ToDouble(MM) / 1000, 5);
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="angle"></param>
    /// <returns></returns>
    public static double AngleToRad(string angle)
    {
        return Math.Round(Convert.ToDouble(angle) * Math.PI / 180, 5);
    }
    /// <summary>
    /// 
    /// </summary>
    /// <param name="rad"></param>
    /// <returns></returns>
    public static double RadToAngle(string rad)
    {
        return Math.Round(Convert.ToDouble(rad) * 180 / Math.PI, 5);
    }



    /// <summary>
    /// 
    /// </summary>
    /// <param name="M"></param>
    /// <returns></returns>
    public static double MToMM(double M)
    {
        return Math.Round(M * 1000, 2);
    }
    /// <summary>
    /// 
    /// </summary>
    /// <param name="MM"></param>
    /// <returns></returns>
    public static double MMToM(double MM)
    {
        return Math.Round(MM / 1000, 5);
    }
    /// <summary>
    /// 
    /// </summary>
    /// <param name="angle"></param>
    /// <returns></returns>
    public static double AngleToRad(double angle)
    {
        return Math.Round(angle * Math.PI / 180, 5);
    }
    /// <summary>
    /// 
    /// </summary>
    /// <param name="rad"></param>
    /// <returns></returns>
    public static double RadToAngle(double rad)
    {
        return Math.Round(rad * 180 / Math.PI, 5);
    }




    /// <summary>
    /// 坐标系的转换
    /// ThreeJsToMatlab
    /// 原则：Y、Z互换，且Y取负方向
    /// 2020.10.19
    /// </summary>
    /// <param name="threePosition"></param>
    /// <returns></returns>
    public static double[] ThreeJsToMatlab__Y_Z(double[] threePosition)
    {
        double[] matlabPosition = new double[6];
        matlabPosition[0] = threePosition[0];
        matlabPosition[1] = threePosition[2] * (-1);
        matlabPosition[2] = threePosition[1];
        matlabPosition[3] = threePosition[3];
        matlabPosition[4] = threePosition[4];
        matlabPosition[5] = threePosition[5];
        return matlabPosition;
    }



    /// <summary>
    /// 坐标系的转换
    /// ThreeJsToMatlab
    /// </summary>
    /// <param name="threePosition"></param>
    /// <returns></returns>
    public static double[] ThreeJsToKdl(double[] threePosition)
    {
        double[] matlabPosition = new double[6];

        //// 2020.10.19 原则：当时好用的变换
        //matlabPosition[0] = threePosition[2] * (-1);
        //matlabPosition[1] = threePosition[0] * (-1);
        //matlabPosition[2] = threePosition[1];
        //matlabPosition[3] = threePosition[5] * (-1);
        //matlabPosition[4] = threePosition[3] * (-1);
        //matlabPosition[5] = threePosition[4];

        //// 2020.11.24 原则：不做变换
        //matlabPosition[0] = threePosition[0];
        //matlabPosition[1] = threePosition[1];
        //matlabPosition[2] = threePosition[2];
        //matlabPosition[3] = threePosition[3];
        //matlabPosition[4] = threePosition[4];
        //matlabPosition[5] = threePosition[5];

        //// 2020.11.25 原则： 
        //matlabPosition[0] = threePosition[2] ;
        //matlabPosition[1] = threePosition[0] * (-1);
        //matlabPosition[2] = threePosition[1];
        //matlabPosition[3] = threePosition[5] ;
        //matlabPosition[4] = threePosition[3] * (-1);
        //matlabPosition[5] = threePosition[4];

        // 2020.11.25 原则：现在好用的
        matlabPosition[0] = threePosition[2];
        matlabPosition[1] = threePosition[0];
        matlabPosition[2] = threePosition[1];
        matlabPosition[3] = threePosition[5];
        matlabPosition[4] = threePosition[3];
        matlabPosition[5] = threePosition[4];

        return matlabPosition;
    }





    /// <summary>
    /// 字符串 转换 双浮点
    /// </summary>
    /// <param name="arrayStr"></param>
    /// <returns></returns>
    public static double[] StringArrayToDoubleArray(string[] arrayStr)
    {
        double[] arrayDouble = new double[arrayStr.Length];
        for (int i = 0; i < arrayStr.Length; i++)
            arrayDouble[i] = Convert.ToDouble(arrayStr[i]);
        return arrayDouble;
    }



    /// <summary>
    /// obj 转换 双浮点
    /// </summary>
    /// <param name="arrayStr"></param>
    /// <returns></returns>
    public static double[] ObjectArrayToDoubleArray(object[] arrayObj)
    {
        double[] arrayDouble = new double[arrayObj.Length];
        for (int i = 0; i < arrayObj.Length; i++)
            arrayDouble[i] = Convert.ToDouble(arrayObj[i].ToString());
        return arrayDouble;
    }



    /// <summary>
    /// 毫米转米
    /// 角度转弧度
    /// 10个值
    /// 2021.2.26
    /// </summary>
    /// <param name="p_Ten"></param>
    public static double[] MMToM_AngleToRad_Ten(double[] p_Ten)
    {
        return new double[] {
         CVT.MMToM(p_Ten[0]),
         CVT.MMToM(p_Ten[1]),
         CVT.MMToM(p_Ten[2]),
         p_Ten[3],
         p_Ten[4],
         p_Ten[5],
         p_Ten[6],
         CVT.AngleToRad(p_Ten[7]),
         CVT.AngleToRad(p_Ten[8]),
         CVT.AngleToRad(p_Ten[9]) };
       
    }
    /// <summary>
    /// 毫米转米
    /// 角度转弧度
    /// 7个值
    /// 2021.2.26
    /// </summary>
    /// <param name="p_Seven"></param>
    public static double[] MMToM_AngleToRad_Seven(double[] p_Seven)
    {
        return new double[] {
            CVT.MMToM(p_Seven[0]),
            CVT.MMToM(p_Seven[1]),
            CVT.MMToM(p_Seven[2]),
            p_Seven[3],
            p_Seven[4],
            p_Seven[5],
            p_Seven[6] };
       
    }
    /// <summary>
    /// 毫米转米
    /// 角度转弧度
    /// 6个值
    /// 2021.2.26
    /// </summary>
    /// <param name="p_Six"></param>
    public static double[] MMToM_AngleToRad_Six(double[] p_Six)
    {
        return new double[]{
        CVT.MMToM(p_Six[0]),
        CVT.MMToM(p_Six[1]),
        CVT.MMToM(p_Six[2]),
        CVT.AngleToRad(p_Six[3]),
        CVT.AngleToRad(p_Six[4]),
        CVT.AngleToRad(p_Six[5])};
      
    }


    /// <summary>
    /// 米转毫米
    /// 弧度转角度
    /// 10个值
    /// 2021.2.26
    /// </summary>
    /// <param name="p_Ten"></param>
    public static double[] MToMM_RadToAngle_Ten(double[] p_Ten)
    {
        return new double[] {
        CVT.MToMM(p_Ten[0]),
        CVT.MToMM(p_Ten[1]),
        CVT.MToMM(p_Ten[2]),
        p_Ten[3],
        p_Ten[4],
        p_Ten[5],
        p_Ten[6],
        CVT.RadToAngle(p_Ten[7]),
        CVT.RadToAngle(p_Ten[8]),
        CVT.RadToAngle(p_Ten[9])
    };
       
    }
    /// <summary>
    /// 米转毫米
    /// 弧度转角度
    /// 7个值
    /// 2021.2.26
    /// </summary>
    /// <param name="p_Seven"></param>
    public static double[] MToMM_RadToAngle_Seven(double[] p_Seven)
    {
        return new double[] {
          CVT.MToMM(p_Seven[0]),
          CVT.MToMM(p_Seven[1]),
          CVT.MToMM(p_Seven[2]),
          p_Seven[3],
          p_Seven[4],
          p_Seven[5],
          p_Seven[6] };
       
    }
    /// <summary>
    /// 米转毫米
    /// 弧度转角度
    /// 6个值
    /// 2021.2.26
    /// </summary>
    /// <param name="p_Six"></param>
    public static double[] MToMM_RadToAngle_Six(double[] p_Six)
    {
        return new double[]{
            CVT.MToMM(p_Six[0]),
            CVT.MToMM(p_Six[1]),
            CVT.MToMM(p_Six[2]),
            CVT.RadToAngle(p_Six[3]),
            CVT.RadToAngle(p_Six[4]),
            CVT.RadToAngle(p_Six[5])
        };
     
    }


    /// <summary>
    /// 
    /// </summary>
    /// <param name="radList"></param>
    /// <returns></returns>
    public static List<double> RadToAngle(List<double> radList)
    {
        var angleList = new List<double>();

        foreach (double rad in radList)
        {
            angleList.Add(Math.Round(rad * 180 / Math.PI, 5));
        }

        return angleList;
    }
    /// <summary>
    /// 
    /// </summary>
    /// <param name="angleList"></param>
    /// <returns></returns>
    public static List<double> AngleToRad(List<double> angleList)
    {
        var radList = new List<double>();

        foreach (double angle in angleList)
        {
            radList.Add(Math.Round(angle * Math.PI / 180, 5));
        }

        return radList;
    }






}