using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Leadshine.SMC.IDE.Motion;
using System.Threading;
using System.Drawing.Drawing2D;
using MathNet.Numerics.LinearAlgebra;
using System.IO;

namespace 机器人
{
    public class Inverse
    {
        public Inverse()
        { 

        }
        
        double con;
        double[] a;   
        double[] b;   
        double d1min, d1max;  
        double d2min, d2max;  
        double d3min, d3max; 
        double R1_min, R1_max;
        double R2_min, R2_max;
        double R3_min, R3_max;
        double U1_min, U1_max;   
        double U12_min, U12_max;
        double U2_min, U2_max;   
        double U22_min, U22_max;
        double S3_min, S3_max;  
        double S32_min, S32_max;

        double psi, theta, z;                                         
        public static double d1, d2, d3;                                         
        double R1, R2, R3, U1, U2, U12, U22, S3, S32; 

        public static double[,] A1;                              //记录各位置下各运动轴位置
        public static double[,] B1;                             //记录所有当前位置到下一位置电机运动量
        public static double[,] D1;                            //记录所有当前位置到下一位置电机脉冲量
        public void inverse_run(double[] G_Z, double[] G_A, double[] G_B) 
        {
            
            con = Math.PI / 180;
            a = new double[3]; b = new double[3];
            a[0] = 系统界面.a1; a[1] = 系统界面.a2; a[2] = 系统界面.a3;  
            b[0] = 系统界面.b1; b[1] = 系统界面.b2; b[2] = 系统界面.b3;   
            d1min = 系统界面.d1min; d1max = 系统界面.d1max;  
            d2min = 系统界面.d2min; d2max = 系统界面.d2max;  
            d3min = 系统界面.d3min; d3max = 系统界面.d3max;  
          
            R1_min = 系统界面.R1_min; R1_max = 系统界面.R1_max;
            R2_min = 系统界面.R2_min; R2_max = 系统界面.R2_max;
            R3_min = 系统界面.R3_min; R3_max = 系统界面.R3_max;
          
            U1_min = 系统界面.U1_min; U1_max = 系统界面.U1_max;     
            U12_min = 系统界面.U12_min; U12_max = 系统界面.U12_max;
            U2_min = 系统界面.U2_min; U2_max = 系统界面.U2_max;     
            U22_min = 系统界面.U22_min; U22_max = 系统界面.U22_max;
            S3_min = 系统界面.S3_min; S3_max = 系统界面.S3_max;     
            S32_min = 系统界面.S3_min; S32_max = 系统界面.S3_max;
            motor_pulse(G_Z, G_A, G_B); 
        }
       
        public void motor_pulse( double[] G_Z, double[] G_A, double[] G_B)
        {
            double PKM_motor_pos0 = 211.8962010041709;   
            A1 = new double[G_Z.Length + 1, 3]; B1 = new double[G_Z.Length, 3]; D1 = new double[G_Z.Length, 4];

            A1[0, 0] = PKM_motor_pos0;
            A1[0, 1] = PKM_motor_pos0;
            A1[0, 2] = PKM_motor_pos0;  
               
            for (int j = 0; j < G_Z.Length; j = j + 1)
            {
                psi = G_A[j]*con;
                theta = G_B[j]*con;
                z = G_Z[j];  
                inverse_position(psi, theta, z);                 
                if (R1_min < R1 && R1 < R1_max && R2_min < R2 && R2 < R2_max && R3_min < R3 && R3 < R3_max && U1_min < U1 && U1 < U1_max && U12_min < U12
                    && U12 < U12_max && U2_min < U2 && U2 < U2_max && U22_min < U22 && U22 < U22_max && S3_min < S3 && S3 < S3_max
                    && d1 < d1max && d2 < d2max && d3 < d3max && d1 > d1min && d2 > d2min && d3 > d3min) 
                {
                    A1[j + 1, 0] = d1;
                    A1[j + 1, 1] = d2;
                    A1[j + 1, 2] = d3;           

                    B1[j, 0] = A1[j + 1, 0] - A1[j, 0];//各轴相对位移量
                    B1[j, 1] = A1[j + 1, 1] - A1[j, 1];
                    B1[j, 2] = A1[j + 1, 2] - A1[j, 2]; 
                }
                else
                {
                    MessageBox.Show("轨迹点不在空间内");
                    return;
                }
            }
            int lp1 = 4;     //丝杠导程
            int fp = 10000;  //控制器分辨率   
           
            for (int n = 0; n < G_Z.Length; n = n + 1)
            {
                D1[n, 0] = Math.Round(B1[n, 0] * fp / lp1);
                D1[n, 1] = Math.Round(B1[n, 1] * fp / lp1);
                D1[n, 2] = Math.Round(B1[n, 2] * fp / lp1);

            }
        }
           
             public void inverse_position(double psi, double theta, double z)
        {
            var JZ = Matrix<double>.Build; //初始化一个矩阵构建对象
            var XL = Vector<double>.Build;   //初始化一个向量的构建对象         
            double phi = 0;
            double x = z * Math.Tan(theta);
            double y = 0;                     /

            double[] RotateA = { 1, 0, 0, 0, Math.Cos(psi), -Math.Sin(psi), 0, Math.Sin(psi), Math.Cos(psi) }; 
            double[] RotateB = { Math.Cos(theta), 0, Math.Sin(theta), 0, 1, 0, -Math.Sin(theta), 0, Math.Cos(theta) }; 
            double[] RotateC = { Math.Cos(phi), -Math.Sin(phi), 0, Math.Sin(phi), Math.Cos(phi), 0, 0, 0, 1 };  

            var RZ = JZ.Dense(3, 3, RotateC).Transpose();  //将一维数组转化为3X3的多维数组,数组从上之下，从左至右，transpose转置
            var RY = JZ.Dense(3, 3, RotateB).Transpose(); 
            var RX = JZ.Dense(3, 3, RotateA).Transpose();
            var R = RZ * RY * RX;          

            

            var vb10 = XL.Dense(3); vb10[0] =0;  vb10[1] = b[0] * (-1); vb10[2] =0; //vb10 = XL.Dense(3); 
            var vb20 = XL.Dense(3); vb20[0] = 0;    vb20[1] = b[1] ;  vb20[2] = 0;
            var vb30 = XL.Dense(3); vb30[0] = b[2] ;  vb30[1] = 0;    vb30[2] = 0;
            
            var vb1 = R * vb10;
            var vb2 = R * vb20;
            var vb3 = R * vb30;     

            var va1 = XL.Dense(3); va1[0] =0;    va1[1] = a[0] * (-1); va1[2] = 0;
            var va2 = XL.Dense(3); va2[0] = 0;    va2[1] = a[1] ;  va2[2] = 0;
            var va3 = XL.Dense(3); va3[0] = a[2] ;  va3[1] = 0;    va3[2] = 0;    

            var vp = XL.Dense(3);  vp[0] = x;  vp[1] = y;  vp[2] = z; 
            var vc0 = XL.Dense(3); vc0[0] = 0; vc0[1] = 1; vc0[2] = 0;
            var vc = R * vc0;
            var vd = XL.Dense(3); vd[0] = 1; vd[1] = 0; vd[2] = 0;     
            

            var qvw1 = vp + vb1 - va1;
            var qvw2 = vp + vb2 - va2;
            var qvw3 = vp + vb3 - va3;   

             d1 = Math.Sqrt(qvw1[0] * qvw1[0] + qvw1[1] * qvw1[1] + qvw1[2] * qvw1[2]);
             d2 = Math.Sqrt(qvw2[0] * qvw2[0] + qvw2[1] * qvw2[1] + qvw2[2] * qvw2[2]);
             d3 = Math.Sqrt(qvw3[0] * qvw3[0] + qvw3[1] * qvw3[1] + qvw3[2] * qvw3[2]);    

            var vw1 = qvw1 / d1;
            var vw2 = qvw2 / d2;
            var vw3 = qvw3 / d3;     

            R1 = Math.Acos((vb1[0] * vw1[0] + vb1[1] * vw1[1] + vb1[2] * vw1[2]) / b[0]);
            R1 = R1 / con;
            U1 = Math.Acos((va1[0] * vw1[0] + va1[1] * vw1[1] + va1[2] * vw1[2]) / a[0]); 
            U1 = U1 / con;
            U12 = Math.Acos(vd[0] * vw1[0] + vd[1] * vw1[1] + vd[2] * vw1[2]);            
            U12 = U12 / con;            
            R2 = Math.Acos((vb2[0] * vw2[0] + vb2[1] * vw2[1] + vb2[2] * vw2[2]) / b[1]); 
            R2 = R2 / con;
            U2 = Math.Acos((va2[0] * vw2[0] + va2[1] * vw2[1] + va2[2] * vw2[2]) / a[1]); 
            U2 = U2 / con;
            U22 = Math.Acos(vd[0] * vw2[0] + vd[1] * vw2[1] + vd[2] * vw2[2]);            
            U22 = U22 / con;            
            R3 = Math.Acos((va3[0] * vw3[0] + va3[1] * vw3[1] + va3[2] * vw3[2]) / a[2]); 
            R3 = R3 / con;
            S3 = Math.Acos((vb3[0] * vw3[0] + vb3[1] * vw3[1] + vb3[2] * vw3[2]) / b[2]); 
            S3 = S3 / con;
            S32 = Math.Acos(vc[0] * vw3[0] + vc[1] * vw3[1] + vc[2] * vw3[2]);            
            S32 = S32 / con;            
        }

    }
}