using MathNet.Numerics.LinearAlgebra;
using Prism.Commands;
using Prism.Mvvm;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Windows;
using TeamAAS_VP.Models;
using TeamAAS_VP;
using TeamAAS_VP.Resources.Languages;
using TeamAAS_VP.Models.Robot;

namespace TeamAAS_VP.ViewModels.Product
{
    public class CalculatePalletPosViewModel : BindableBase
    {
        private Pallet _CurrentPallet;

        public Pallet CurrentPallet
        {
            get { return _CurrentPallet; }
            set { SetProperty(ref _CurrentPallet,value);
                if (value != null)
                {
                    CalculateSpace(value);
                }
            }
        }

        private double _RowSpace;

        public double RowSpace
        {
            get { return _RowSpace; }
            set { SetProperty(ref _RowSpace,value); }
        }

        private double _ColSpace;

        public double ColSpace
        {
            get { return _ColSpace; }
            set { SetProperty(ref _ColSpace, value); }
        }

        private DelegateCommand _CalculateP1Command;
        public DelegateCommand CalculateP1Command =>
            _CalculateP1Command ?? (_CalculateP1Command = new DelegateCommand(ExecuteCalculateP1Command));

        private DelegateCommand _CalculateP2Command;
        public DelegateCommand CalculateP2Command =>
            _CalculateP2Command ?? (_CalculateP2Command = new DelegateCommand(ExecuteCalculateP2Command));

        void ExecuteCalculateP2Command()
        {
            try
            {
                Calculatet(RowSpace, false);
                NotificationEvent?.Invoke(Lang.完成, 1);
            }
            catch (Exception ex)
            {
                NotificationEvent?.Invoke(ex.Message, 1);
            }
        }

        public event Action<string, double> NotificationEvent;
        void ExecuteCalculateP1Command()
        {
            try
            {
                Calculatet(ColSpace, true);
                NotificationEvent?.Invoke(Lang.完成, 1);
            }
            catch (Exception ex)
            {
                NotificationEvent?.Invoke(ex.Message,1);
            }
        }

        public CalculatePalletPosViewModel()
        {

        }

        private void Calculatet(double Space,bool P1orP2)
        {
            try
            {
                //----------------------------------------计算旋转矩阵-----------------------------------------------------------------
                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { CurrentPallet.P0.X, CurrentPallet.P0.Y });
                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { CurrentPallet.P1.X, CurrentPallet.P1.Y });
                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { CurrentPallet.P2.X, CurrentPallet.P2.Y });
                //计算正交时的X与Y轴的单位向量
                Vector<double> E1 = (p2 - p1) / (p2 - p1).L2Norm();
                Vector<double> E2 = (p3 - p1 - E1.DotProduct(p3 - p1) * E1) / (p3 - p1 - E1.DotProduct(p3 - p1) * E1).L2Norm();
                //得到旋转矩阵
                Matrix<double> R = Matrix<double>.Build.DenseOfColumnVectors(E1, E2);

                //--------------------------------------计算切变矩阵--------------------------------------------------------
                double a = (E1.DotProduct(p3 - p1) * E1).L2Norm() / (p3 - p1 - (E1.DotProduct(p3 - p1) * E1))[1];
                Matrix<double> Shear = Matrix<double>.Build.DenseOfArray(new double[,] { { 1, a }, { 0, 1 } });

                if (P1orP2)
                {
                    //计算P1点位
                    //切变前的坐标
                    Vector<double> q = Vector<double>.Build.Dense(new double[] { Space * (CurrentPallet.Column - 1), 0 });
                    //切变后的坐标
                    Vector<double> h = Shear * q;
                    //转换后的机器人坐标
                    Vector<double> rp = R * h + p1;
                    CurrentPallet.P1.X = (float)rp[0];
                    CurrentPallet.P1.Y = (float)rp[1];
                }
                else
                {
                    //计算P2点位
                    //切变前的坐标
                    Vector<double> q = Vector<double>.Build.Dense(new double[] { 0, Space * (CurrentPallet.Row - 1) });
                    //切变后的坐标
                    Vector<double> h = Shear * q;
                    //转换后的机器人坐标
                    Vector<double> rp = R * h + p1;
                    CurrentPallet.P2.X = (float)rp[0];
                    CurrentPallet.P2.Y = (float)rp[1];
                }
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(Lang.计算Pallet的P1或者P2时出错, ex);
                MessageBox.Show(ex.Message);
            }
        }

        private void CalculateSpace(Pallet pallet)
        {
            try
            {
                //----------------------------------------计算旋转矩阵-----------------------------------------------------------------
                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { pallet.P0.X, pallet.P0.Y });
                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { pallet.P1.X, pallet.P1.Y });
                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { pallet.P2.X, pallet.P2.Y });
                //计算正交时的X与Y轴的单位向量
                Vector<double> E1 = (p2 - p1) / (p2 - p1).L2Norm();
                Vector<double> E2 = (p3 - p1 - E1.DotProduct(p3 - p1) * E1) / (p3 - p1 - E1.DotProduct(p3 - p1) * E1).L2Norm();
                //得到旋转矩阵
                Matrix<double> R = Matrix<double>.Build.DenseOfColumnVectors(E1, E2);

                //--------------------------------------计算切变矩阵--------------------------------------------------------
                double a = (E1.DotProduct(p3 - p1) * E1).L2Norm() / (p3 - p1 - (E1.DotProduct(p3 - p1) * E1))[1];
                Matrix<double> Shear = Matrix<double>.Build.DenseOfArray(new double[,] { { 1, a }, { 0, 1 } });

                //--------------------------------------计算托盘行列间距---------------------------------------------------------
                ColSpace = (p2 - p1).L2Norm() / (pallet.Column - 1);
                RowSpace = (p3 - p1 - E1.DotProduct(p3 - p1) * E1).L2Norm() / (pallet.Row - 1);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(Lang.计算Pallet的行列间距时出错, ex);
                MessageBox.Show(ex.Message);
            }
        }
    }
}