using System;
using System.Collections.Generic;
using System.IO;
using System.Windows.Forms;
using ACS.SPiiPlusNET;
using HexcalMC.Base;
using HexcalMC.Form;
using HexcalMC.Hexcal;
using HexcalMC.Properties;
using SharpGL;
using Telerik.WinControls.UI;

namespace HexcalMC
{
	//定一个 回家状态枚举，包括 从未回家，正在回家，已经回家
	public enum HomeStates
	{
		None, //默认状态
		NotHome,
		Homing,
		Homed
	}

	//定义 运动状态枚举，包括 正在运动，运动到位，Jog运动
	public enum MotionStates
	{
		None, //默认状态
		Moving,
		InPos,
		Jogging
	}

	public partial class MainFrom : RadRibbonForm
	{
		private readonly List<Point3D> _pointCloud = new List<Point3D>(); //运动中点集合


		public MainFrom()
		{
			InitializeComponent();


			#region 3D相关绘制，暂时取消

			//var sharpGlViewportControl = new SharpGLViewportControl
			//{
			//	Dock = DockStyle.Fill
			//};

			//var tabPage = new TabPage("3D 窗口");
			//tabPage.Controls.Add(sharpGlViewportControl);
			//tabControl1.TabPages.Add(tabPage);

			//// 设置容器控件的 DoubleBuffered 属性为 true
			//DoubleBuffered = true;
			//// 创建一个包含点云数据的列表
			//var points = new List<Point3D>
			//{
			//	new Point3D(0, 0, 0),
			//	new Point3D(1, 1, 1),
			//	new Point3D(-1, -1, -1)
			//	// 可以添加更多的点云数据
			//};

			//// 将点云数据传递给 SharpGLViewportControl
			//sharpGlViewportControl.SetPointCloud(points);

			tabControl1.TabPages.RemoveAt(1);

			#endregion
		}

		private void MainFrom_Load(object sender, EventArgs e)
		{
			FormBorderStyle = FormBorderStyle.FixedSingle; // 设置窗体边框样式为固定大小
			MaximizeBox = false; // 禁用窗体的最大化按钮
			DebugDfn.textBox_Msg = TextBoxMsg;


			//加载配置文件
			LoadConfig();

			_acs = new Api(); //初始化 ACS运动控制类


			//启动界面刷新
			timer_RefreshUI.Start();
			//TopMost = true;
		}

		private void MainFrom_Shown(object sender, EventArgs e) //窗体显示准备好接受用户输入时发生
		{
			////启动服务端,用于接收hexcal传来的指令
			//StartServer();

			//if (_enableAcs)
			//{
			//	Btn_ACSStart_Click(null, null); //模拟连接运动平台
			//}
		}

		private void MainFrom_FormClosed(object sender, FormClosedEventArgs e)
		{
			MyBase.TraceWriteLine("关闭程序");
			DebugDfn._strEndTime = DateTime.Now.ToString("yyyy.MM.dd HH-mm-ss");
			timer_RefreshUI.Stop();


			string copyFileName = DebugDfn.StrDebugSavePath + "\\Debug(" + DebugDfn._strStartTime + "  To  " +
			                      DebugDfn._strEndTime + ")" + ".txt";
			if (!File.Exists(DebugDfn.StrDebugSavePath))
			{
				//创建文件夹 DebugDfn.StrDebugSavePath
				Directory.CreateDirectory(DebugDfn.StrDebugSavePath);
			}

			File.Copy(DebugDfn.StrDebugFile, copyFileName);

			if (Errors.ErrorWrite != null)
				Errors.ErrorWrite.Close();
			if (Errors.OtherWrite != null)
				Errors.OtherWrite.Close();
			if (Errors.StatusWrite != null)
				Errors.StatusWrite.Close();
		}

		private void LoadConfig() //加载配置文件
		{
			//判断配置文件是否存在
			if (!File.Exists(StrConfigFile))
			{
				MessageBox.Show("配置文件不存在，请检查配置文件", "异常", MessageBoxButtons.OK, MessageBoxIcon.Error);
				return;
			}

			//上电使能ACS
			_enableAcs = FileIni.ReadBool(StrConfigFile, "MOTOR", "ENABLE_ACS");

			MotionSpeed = FileIni.ReadDouble(StrConfigFile, "MOTOR", "MOTION_SPEED"); //运动定位速度
			//正限位
			XMaxstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "X_MAXSTROKESW");
			YMaxstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Y_MAXSTROKESW");
			ZMaxstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Z_MAXSTROKESW");

			//负限位
			XMinstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "X_MINSTROKESW");
			YMinstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Y_MINSTROKESW");
			ZMinstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Z_MINSTROKESW");
		}

		private void Plot2D(List<Point3D> pointCloud)
		{
			// 清空画布
			formsPlot1.Plot.Clear();

			//pointCloud 是否为空
			if (pointCloud.Count <= 0)
			{
				return;
			}

			List<double> dataX = new List<double>();
			List<double> dataY = new List<double>();

			foreach (Point3D point3D in pointCloud)
			{
				dataX.Add(point3D.X);
				dataY.Add(point3D.Y);
			}

			formsPlot1.Plot.AddScatter(dataX.ToArray(), dataY.ToArray());
			formsPlot1.Refresh();
		}

		private void Plot3D() //绘制3D图
		{
			List<Point3D> points = new List<Point3D>();

			// 假设已经有了一些点的数据，将其添加到 List<Point3D> 对象中
			points.Add(new Point3D(1, 2, 3));
			points.Add(new Point3D(4, 5, 6));

			LoadPoints(points);
		}


		#region hexcal软件交互

		private TcpIpServer _mTcpIpServer; //创建tcpserver，用于接收hexcal传来的指令，并解析传递平台
		private bool _mBHexcalConnected;
		public string LISTEN_ADDRESS = "100.0.0.1";

		private void StartServer()
		{
			// 对_mTcpIpServer增加判断是否已经启动且存在设备连接	
			if (_mTcpIpServer != null && _mTcpIpServer.ConnectStatus)
			{
				//弹窗提醒已经启动
				MyBase.TraceWriteLine("TCP服务端已经启动，请勿重复启动");
				MessageBox.Show("TCP服务端已经启动，请勿重复启动", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
				return;
			}

			//判断 要监听的IP地址是否存在

			if (!Internet.IsIpReachable(LISTEN_ADDRESS))
			{
				MyBase.TraceWriteLine("启动服务失败，请确认本地网卡是否启用且配置 " + LISTEN_ADDRESS);
				MessageBox.Show("请确认本地网卡是否启用且配置 " + LISTEN_ADDRESS, "启动服务失败");
				return;
			}


			//启动服务器，并获取数据，解析
			_mTcpIpServer = new TcpIpServer(LISTEN_ADDRESS, Convert.ToString(1234));
			_mTcpIpServer.UseMode = 1; //设置通讯返回数据流格式
			try
			{
				//启动监听
				if (_mTcpIpServer.StartListen())
				{
					//绑定两个事件 OnRaisedStatus 和OnRaisedMessage
					_mTcpIpServer.OnRaisedMessage += ReceiveMessage; //接收消息回调
					_mTcpIpServer.OnRaisedStatus += ReceiveStatus; //连接状态
					_mTcpIpServer.DataReceived += ReceiveByte;
					DebugDfn.AddLogText("TCP服务端启动成功 ");
				}
				else
				{
					MessageBox.Show("TCP服务端启动失败，请检查网络连接，重新打开软件", "异常", MessageBoxButtons.OK, MessageBoxIcon.Error);
				}
			}
			catch (Exception ex)
			{
				DebugDfn.AddLogText("启动TCP服务端异常" + ex);
			}
		}

		private void ReceiveByte(object sender, byte[] e)
		{
			DebugDfn.AddLogText("接收到" + BitConverter.ToString(e));
		}

		private void ReceiveMessage(string clientIp, string msg) //接收的内容
		{
			//打印ClientIP 和 Msg
			DebugDfn.AddLogText("接收到" + clientIp + ": " + msg);

			//根据源地址的不同，执行不同处理
			string sourceIp = clientIp.Split(':')[0];
			switch (sourceIp)
			{
				case "100.0.0.1": //L2系统

					//解析处理数据
					ParseHexcalMsg(msg);

					break;
				default:
					DebugDfn.AddLogText("未知来源，没有应答");
					break;
			}
		}

		public static string ConstructString(string variableName, double[] values)
		{
			string result = variableName + " ";
			for (int i = 0; i < values.Length; i++)
			{
				result += values[i].ToString("F6");
				if (i < values.Length - 1)
				{
					result += ", ";
				}
			}

			return result;
		}

		public static string ConstructPosString(Point3D point)
		{
			double[] values = { point.X, point.Y, point.Z, 0.0, 0.0, 0.0, 0.0 };
			return ConstructString("POS", values);
		}

		public static Point3D ParsePoint3DFromCommand(string input)
		{
			string[] parts = input.Split(' ')[1].Split(',');
			if (parts.Length >= 3)
			{
				double x = double.Parse(parts[0]);
				double y = double.Parse(parts[1]);
				double z = double.Parse(parts[2]);
				return new Point3D(x, y, z);
			}

			throw new ArgumentException("输入字符串格式不正确。");
		}

		private void CheckPlatformStatus()
		{
			if (_currentMotionState == MotionStates.None || _currentMotionState == MotionStates.InPos) //默认或到位
			{
				SendMsgToHexcal("READY");
			}
			else
			{
				SendMsgToHexcal("BUSY");
			}
		}

		private void ParseHexcalMsg(string msg) //编写一个Hexcal协议解析函数
		{
			DebugDfn.AddLogText("正在解析 " + msg);

			//去除Msg中\r\n
			msg = msg.Replace("\r\n", "");

			//判断是否含有故障ERROR字样
			if (msg.Contains("ERROR"))
			{
				//弹窗提醒
				MessageBox.Show("CMM错误", msg, MessageBoxButtons.OK, MessageBoxIcon.Error);
				return;
			}

			if (msg.Contains("\x02") || msg.Contains("\u0002"))
			{
				CheckPlatformStatus();
			}
			else if (msg.Contains("\x03") || msg.Contains("\u0003"))
			{
				CheckPlatformStatus();
			}
			else if (msg.Contains("^B")) //查询状态, READY或BUSY
			{
				CheckPlatformStatus();
			}

			else if (msg.Contains("CMMTYP")) //测量机类型
			{
				SendMsgToHexcal("CMMTYP   MA 19617, FDC       V15.00,      6  6  2    , 0");
			}
			else if (msg.Contains("VERSION")) //版本号
			{
				SendMsgToHexcal("00-000-000-00000 FDC V51.04.0000 DATE: 12/21/22 TIME: 12:50:55");
			}

			else if (msg.Contains("SHOW MAXSTROKESW")) //最大行程,根据实际情况填写
			{
				//MAXSTROKESW 233.200000,346.500000,15.100000,0.000000,0.000000,0.000000,0.000000

				double[] values = { XMaxstrokesw, YMaxstrokesw, ZMaxstrokesw, 0.0, 0.0, 0.0, 0.0 };
				string resultString = ConstructString("MAXSTROKESW", values);
				SendMsgToHexcal(resultString);
			}

			else if (msg.Contains("SHOW MINSTROKESW")) //最小行程,根据实际情况填写
			{
				//MINSTROKESW -68.800000,-55.500000,-286.900000,0.000000,0.000000,0.000000,0.000000

				double[] values = { XMinstrokesw, YMinstrokesw, ZMinstrokesw, 0.0, 0.0, 0.0, 0.0 };
				string resultString = ConstructString("MINSTROKESW", values);
				SendMsgToHexcal(resultString);
			}
			else if (msg.Contains("SHOW MAXVEL")) //最大速度
			{
				SendMsgToHexcal("MAXVEL 300.000000,300.000000,300.000000,0.000000,0.000000,0.000000,0.000000,0.000000");
			}
			else if (msg.Contains("SHOW MAXACC")) //最大加速度
			{
				SendMsgToHexcal(
					"MAXACC  300.000000,300.000000,300.000000,0.000000,0.000000,0.000000,0.000000,0.000000");
			}
			else if (msg.Contains("SHOW SENSWKP"))
			{
				SendMsgToHexcal("X_ SENSWKP 4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
			}
			else if (msg.Contains("SHOW X_SENSAXIS"))
			{
				SendMsgToHexcal("X_SENSAXIS 6,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
			}
			else if (msg.Contains("SHOW Y_SENSAXIS")) //查询Y轴
			{
				SendMsgToHexcal("Y_SENSAXIS 2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
			}
			else if (msg.Contains("SHOW Z_SENSAXIS")) //查询Z轴
			{
				SendMsgToHexcal("Z_SENSAXIS 7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
			}
			else if (msg.Contains("SHOW TEMPCOMPTYPE")) //温度补偿，温度补偿 >1 表示支持温度补偿，此处不支持
			{
				SendMsgToHexcal("TEMPCOMPTYPE 0");
			}
			else if (msg.Contains("READTP"))
			{
				SendMsgToHexcal("READTP 0.000000");
			}
			else if (msg.Contains("SHOW ESTOP")) //查询急停状态，根据真是情况调整
			{
				SendMsgToHexcal("ESTOP FALSE");
			}
			else if (msg.Contains("CMHWST"))
			{
				SendMsgToHexcal("CMHWST 8257,0,1792,0");
			}
			else if (msg.Contains("SHOW MOVPAR")) //查询速度
			{
				SendMsgToHexcal(
					"MOVPAR 300.000000,300.000000,300.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.00000 0,0.000000");
			}
			else if (msg.Contains("SHOW MAXVEL")) //查询最大速度
			{
				SendMsgToHexcal("MAXVEL 300.000000,300.000000,300.000000,0.000000,0.000000,0.000000,0.000000,0.000000");
			}
			else if (msg.Contains("SHOW ACCEL")) //查询加速度
			{
				SendMsgToHexcal(
					"ACCEL 1000.000000,1000.000000,1000.000000,0.000000,0.000000,0.000000,0.000000,0.000000");
			}
			else if (msg.Contains("MOVPAR")) //设置速度 xyz 轴的速度
			{
				SendMsgToHexcal("%");
			}
			else if (msg.Contains("ACCEL")) //设置加速度
			{
				SendMsgToHexcal("%");
			}
			else if (msg.Contains("PRBPIN")) //设置侧头偏置
			{
				SendMsgToHexcal("%");
			}
			else if (msg.Contains("ENABLE TEMP")) //设置温度补偿
			{
				SendMsgToHexcal("%");
			}
			else if (msg.Contains("WKPPAR"))
			{
				SendMsgToHexcal("%");
			}
			else if (msg.Contains("SCLTMP"))
			{
				SendMsgToHexcal("%");
			}
			else if (msg.Contains("DISABLE GEO"))
			{
				SendMsgToHexcal("%");
			}
			else if (msg.Contains("AUTZER")) //回家指令
			{
				SendMsgToHexcal("%"); //收到并执行，同时状态改为忙碌

				//执行回家
				Ishome();
			}
			else if (msg.Contains("MOVABS")) //移动指令，解析移动位置 
			{
				//收到指令 ，形如 MOVABS 0.015000,127.172997,-114.897003,0.000000\r\n
				SendMsgToHexcal("%");

				Point3D point = ParsePoint3DFromCommand(msg);
				SetPositionXyz(point); //开始移动

				_pointCloud.Add(point); //添加到点集合
			}
			else if (msg.Contains("GETPOS")) //获取位置
			{
				//POS 167.553898,-55.400421,-208.548678,0.000000,0.000000,0.000000,0.000000
				Point3D point3D = GetPositionXyz(); //获取当前位置
				string resultString = ConstructPosString(point3D);
				SendMsgToHexcal(resultString);
			}
			else
			{
				DebugDfn.AddLogText("未知命令，没有应答");
			}
		}

		private void ReceiveStatus(TcpIpServer.EnumTcpIpServer iType, string msg)
		{
			//记录到日志
			DebugDfn.AddLogText(iType + " : " + msg);

			//根据连接状态，更新界面
			switch (iType)
			{
				case TcpIpServer.EnumTcpIpServer.ClientConnect:
					_mBHexcalConnected = true;
					break;
				default:
					_mBHexcalConnected = false;
					break;
			}
		}

		private void SendMsgToHexcal(string msg)
		{
			//发送数据
			DebugDfn.AddLogText("回复 " + msg);
			_mTcpIpServer.SendMessageToAllClients(msg += "\r\n"); //回复内容末尾加上\r\n，协议要求
		}

		private void Btn_StartServer_Click(object sender, EventArgs e)
		{
			StartServer();
		}

		private void Btn_StopServer_Click(object sender, EventArgs e)
		{
			//关闭服务端
			if (_mTcpIpServer != null)
			{
				_mTcpIpServer.StopListen();
			}
		}

		#endregion

		#region ACS平台相关

		#region 运动平台变量区

		public Api _acs;

		private const int MaxUiLimitCnt = 8;
		private const int MaxUiIoCnt = 8;

		private readonly int _mNTotalAxis = 0;
		private int _mNTotalBuffer = 0;
		private Axis[] _mArrAxisList = null;

		public bool _mAcsConnected; //ACS通讯状态

		// For update values
		private MotorStates _mNMotorState; //运动状态

		private ProgramStates _mNProgramState; //程序状态
		private object _mObjReadVar;
		private Array _mArrReadVector;
		private double _mLfRPos, _mLfFPos, _mLfPe, _mLfFvel; //参考位置，反馈位置 位置误差 反馈速度 double类型
		private int _mNValues, _mNOutputState;

		private Label[] _mLblLeftLimit; //左限位
		private Label[] _mLblRightLimit; //右限位

		private HomeStates _homeStates; //回家状态
		private MotionStates _currentMotionState; //当前运动状态
		private readonly int _motionTimeout = 50000; //定义运动超时时间


		public static Axis[] UseAxis =
			{ Axis.ACSC_AXIS_1, Axis.ACSC_AXIS_0, Axis.ACSC_AXIS_8, Axis.ACSC_NONE }; //定义启用的轴，后面运动时会使用

		//定义轴的运动状态
		private readonly bool[] axisFinished = new bool[10]; // 假设只有轴 0、1和8，数组大小为10（0到9）

		//定义 XYZ三个轴的左右行程范围
		public string StrConfigFile = Application.StartupPath + "\\File\\config.ini";
		public static double MotionSpeed = 60;
		public static double XMaxstrokesw = 730; //正限位
		public static double YMaxstrokesw = 1000;
		public static double ZMaxstrokesw = 5;

		public static double XMinstrokesw = -30; //负限位
		public static double YMinstrokesw = -10;
		public static double ZMinstrokesw = -280;

		private bool _enableAcs; //读取配置，是否上电自动连接平台

		private Point3D _mPoint3D; //定义一个3D点,存储当前平台位置
		private Point3D _mPoint3DLast; //存储上一周期的位置

		#endregion

		private void BtnEnable_Click(object sender, EventArgs e) //使能所有轴
		{
			if (_mAcsConnected)
			{
				//!!!! Important !! Must insert '-1' at the last
				_acs.EnableM(UseAxis);
			}
			else
			{
				//弹窗提醒尚未连接
				MessageBox.Show("未连接到运动平台，请先点击连接");
			}
		}

		private void BtnDisable_Click(object sender, EventArgs e) //轴取消
		{
			// Disable all of axes
			_acs.DisableAll();
		}

		private bool IsMotionInPose()
		{
			bool x_inpose = false, y_inpose = false, z_inpose = false;

			_mNMotorState = _acs.GetMotorState(Axis.ACSC_AXIS_1);
			if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
			{
				x_inpose = true;
			}

			_mNMotorState = _acs.GetMotorState(Axis.ACSC_AXIS_0);
			if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
			{
				y_inpose = true;
			}

			_mNMotorState = _acs.GetMotorState(Axis.ACSC_AXIS_8);
			if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
			{
				z_inpose = true;
			}

			if (x_inpose && y_inpose && z_inpose)
			{
				return true;
			}

			return false;
		}

		private void TmrMonitor_Tick(object sender, EventArgs e) //用于刷新状态
		{
			int iAxisNo = cboAxisNo.SelectedIndex;
			if (_mAcsConnected)
			{
				try
				{
					//获取平台当前位置
					_mPoint3D = GetPositionXyz();

					// Get Motor State ACSPL+ Variable : MST (integer)
					_mNMotorState = _acs.GetMotorState((Axis)iAxisNo);

					// Returned value is integer, you need to use bitmaks

					// 运动中
					if ((_mNMotorState & MotorStates.ACSC_MST_MOVE) != 0)
					{
						lblMoving.Image = Resources.On;
						_currentMotionState = MotionStates.Moving;
					}
					else
					{
						lblMoving.Image = Resources.Off;
					}

					// 就位
					if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
					{
						lblInPos.Image = Resources.On;
						//_currentMotionState = MotionStates.InPos;
					}
					else
					{
						lblInPos.Image = Resources.Off;
					}

					// 加速
					if ((_mNMotorState & MotorStates.ACSC_MST_ACC) != 0)
					{
						lblAcc.Image = Resources.On;
					}
					else
					{
						lblAcc.Image = Resources.Off;
					}

					// 使能
					if ((_mNMotorState & MotorStates.ACSC_MST_ENABLE) != 0)
					{
						lblEnable.Image = Resources.On;
					}
					else
					{
						lblEnable.Image = Resources.Off;
					}
				}
				catch (Exception ex)
				{
					DebugDfn.AddLogText("ACS平台刷新异常" + ex);
					MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
				}
			}

			if (IsMotionInPose())
			{
				_currentMotionState = MotionStates.InPos;
				DebugDfn.AddLogText("运动到位");
			}
			else
			{
				_currentMotionState = MotionStates.Moving;
				DebugDfn.AddLogText("运动中");
			}
		}

		private void Btn_ACSStart_Click(object sender, EventArgs e) //连接
		{
			btn_ACSStart.Enabled = false;
			btn_ACSStop.Enabled = true;

			try
			{
				_acs.OpenCommEthernetTCP(
					txtIP.Text, // IP Address (Default : 10.0.0.100)
					Convert.ToInt32(txtPort.Text.Trim())); // TCP/IP Port nubmer (default : 701)

				_mAcsConnected = true;
			}
			catch (Exception ex)
			{
				DebugDfn.AddLogText("ACS平台连接异常" + ex);
				MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
			}


			// 启动定时器
			tmrMonitor.Interval = 50;
			tmrMonitor.Start();


			//运动相关初始化操作
			InitMotion();
		}

		private void Btn_ACSStop_Click(object sender, EventArgs e) //断开连接
		{
			if (_mAcsConnected) _acs.CloseComm();

			tmrMonitor.Stop();

			btn_ACSStart.Enabled = true;
			btn_ACSStop.Enabled = false;
		}

		private void UpdateLimitState(int axisNo, int fault) //刷新限位
		{
			if (axisNo < MaxUiLimitCnt)
			{
				if ((fault & (int)SafetyControlMasks.ACSC_SAFETY_LL) != 0)
					_mLblLeftLimit[axisNo].Image = Resources.Error;
				else _mLblLeftLimit[axisNo].Image = Resources.Off;
				if ((fault & (int)SafetyControlMasks.ACSC_SAFETY_RL) != 0)
					_mLblRightLimit[axisNo].Image = Resources.Error;
				else _mLblRightLimit[axisNo].Image = Resources.Off;
			}
		}

		private void Ishome() //读取回家状态，当未回家时执行回家指令
		{
			// 1、连接状态检查，如果未连接，提示
			if (!_mAcsConnected)
			{
				DebugDfn.AddLogText("[Ishome] ACS平台未连接,请先点击连接");
				MessageBox.Show("ACS平台未连接,请先点击连接", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
				return;
			}

			// 2、回家状态检查是否已经回家
			if (_mAcsConnected && _homeStates == HomeStates.Homed)
			{
				//弹窗提示
				MessageBox.Show("轴已经回家", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
				return;
			}

			// 3、读取回家状态
			if (_acs != null && _mAcsConnected)

			{
				var yawHome = _acs.ReadVariable("YAW_HOME_DONE");

				if (Convert.ToBoolean(yawHome))
				{
					//弹窗提示
					MessageBox.Show("轴已经回家", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
					_homeStates = HomeStates.Homed;
					return;
				}
				// 4、执行回家指令,弹窗等待用户确认

				DialogResult result = MessageBox.Show("轴未回家，即将执行回家指令", "提示", MessageBoxButtons.OK,
					MessageBoxIcon.Information);

				if (result == DialogResult.OK)
				{
					// 在这里执行接下来的操作,例如回家指令

					_acs.RunBuffer(ProgramBuffer.ACSC_BUFFER_6, null); //执行回家指令，这里的buffer6是回家指令的buffer
					_homeStates = HomeStates.Homing;
					_currentMotionState = MotionStates.Moving;
					DebugDfn.AddLogText("回家运动中");

					//等待回家完成
					for (int i = 0; i < UseAxis.Length; i++)
					{
						_acs.WaitMotionEnd(UseAxis[i], _motionTimeout); //等待回家完成
					}

					_homeStates = HomeStates.Homed;
					_currentMotionState = MotionStates.InPos;
					DebugDfn.AddLogText("回家完成");
				}
				else
				{
					//弹窗提醒
					MessageBox.Show("点击了取消，未进行任何动作", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
				}
			}
		}

		private void InitMotion() //定义 运动状态初始化函数，包括内部变量，轴启用，回家判断等
		{
			//运动相关变量初始化
			_homeStates = HomeStates.None;
			_currentMotionState = MotionStates.None;
			_currentMotionState = MotionStates.None;

			if (!_mAcsConnected)
			{
				MessageBox.Show("未连接运动平台，请先连接运动平台");
				return;
			}

			if (_acs == null)
			{
				DebugDfn.AddLogText("运动控制对象不存在");
				return;
			}

			//轴启用，加电
			_acs.EnableM(UseAxis);
			for (int i = 0; i < UseAxis.Length; i++)
			{
				_acs.WaitMotorEnabled(UseAxis[i], 1, _motionTimeout); //等待电机使能
			}

			DebugDfn.AddLogText("电机已启用");

			//回家
			Ishome();

			SetSpeedXyz();

			//注册到位事件
			//_acs.PHYSICALMOTIONEND += ACS_PHYSICALMOTIONEND;
			_acs.EnableEvent(Interrupts.ACSC_INTR_PHYSICAL_MOTION_END);
		}

		private void ACS_PHYSICALMOTIONEND(AxisMasks axis)
		{
			//int bit = 0x01;
			//int axisNo = 0;

			//for (int i = 0; i < 64; i++)
			//{
			//	if ((int)axis == bit)
			//	{
			//		axisNo = i;
			//		axisFinished[axisNo] = true; // 将对应轴的完成情况标记为 true
			//		Console.WriteLine($" %d %d",axisNo, axisFinished[axisNo]);
			//		break;
			//	}

			//	bit = bit << 1;
			//}
			//打印数组 axisFinished


			// 检查轴0、1和8是否全部完成
			//if (axisFinished[0] && axisFinished[1] && axisFinished[8])
			{
				// 三个轴均完成后的操作
				Invoke((MethodInvoker)delegate
				{
					_currentMotionState = MotionStates.InPos;
					//DebugDfn.AddLogText("运动到位");
				});
			}
		}

		private bool IsWithinStrokes(Point3D point) //判断点是否在行程范围内
		{
			if (point.X >= XMinstrokesw && point.X <= XMaxstrokesw &&
			    point.Y >= YMinstrokesw && point.Y <= YMaxstrokesw &&
			    point.Z >= ZMinstrokesw && point.Z <= ZMaxstrokesw)
			{
				return true;
			}

			return false;
		}

		private void SetPositionXyz(Point3D point3D) //运动到指定位置
		{
			if (!_mAcsConnected)
			{
				DebugDfn.AddLogText("ACS平台未连接,请先点击连接");
				MessageBox.Show("ACS平台未连接,请先点击连接", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
				return;
			}

			if (_currentMotionState != MotionStates.Moving)
			{
				_currentMotionState = MotionStates.Moving; //设置当前运动状态
				//判断 point3D是否合法
				if (point3D != null)
				{
					if (IsWithinStrokes(point3D)) //判断点是否在行程范围内
					{
						double[] pointsArray =
						{
							point3D.X,
							point3D.Y,
							point3D.Z
						};

						//执行运动指令
						_acs.ToPointM(MotionFlags.ACSC_NONE, UseAxis, pointsArray); //多轴运动到指定位置

						////等待运动完成
						//for (int i = 0; i < USE_AXIS.Length; i++)
						//{
						//	_acs.WaitMotionEnd(USE_AXIS[i], _motionTimeout); //等待回家完成
						//}

						//_currentMotionState = MotionStates.InPos;
						//DebugDfn.AddLogText("运动到位");
					}
					else
					{
						DebugDfn.AddLogText("目标位置超出行程范围，请重新设置");
						MessageBox.Show("目标位置超出行程范围，请重新设置", "异常", MessageBoxButtons.OK, MessageBoxIcon.Error);
					}
				}
				else
				{
					DebugDfn.AddLogText("目标位置为空，请重新设置");
					MessageBox.Show("目标位置为空，请重新设置", "异常", MessageBoxButtons.OK, MessageBoxIcon.Error);
				}
			}
		}

		private Point3D GetPositionXyz(int positionMode = 1) //获取当前位置
		{
			double xPosition = 0, yPosition = 0, zPosition = 0;
			Point3D point3D = new Point3D(xPosition, yPosition, zPosition);
			if (!_mAcsConnected)
			{
				DebugDfn.AddLogText("ACS平台未连接,请先点击连接");
				MessageBox.Show("ACS平台未连接,请先点击连接", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
				return point3D;
			}

			//获取当前位置, 两种 GetRPosition，GetFPosition
			if (positionMode == 1)
			{
				//获取反馈位置 Feedback position (Encoder value) ACSPL+ Variable : FPO (real)
				xPosition = _acs.GetFPosition(UseAxis[0]);
				yPosition = _acs.GetFPosition(UseAxis[1]);
				zPosition = _acs.GetFPosition(UseAxis[2]);
				//DebugDfn.AddLogText("反馈位置: " + xPosition + " " + yPosition + " " + zPosition);
			}
			else
			{
				//获取参考位置 ACSPL+ Variable : RPOS (real)
				xPosition = _acs.GetRPosition(UseAxis[0]);
				yPosition = _acs.GetRPosition(UseAxis[1]);
				zPosition = _acs.GetRPosition(UseAxis[2]);
				DebugDfn.AddLogText("参考位置: " + xPosition + " " + yPosition + " " + zPosition);
			}


			//构造point3D格式
			point3D = new Point3D(xPosition, yPosition, zPosition);

			return point3D;
		}

		private void SetSpeedXyz() //获取运动参数
		{
			//获取实际速度
			double feedbackVelocity = (double)_acs.ReadVariable("FVEL", ProgramBuffer.ACSC_NONE, 0, 0);
			DebugDfn.AddLogText("实际速度: " + feedbackVelocity);

			//设置Y轴 速度参数
			_acs.SetVelocity(Axis.ACSC_AXIS_0, MotionSpeed);
			_acs.SetAcceleration(Axis.ACSC_AXIS_0, MotionSpeed * 10);
			_acs.SetDeceleration(Axis.ACSC_AXIS_0, MotionSpeed * 10);

			//设置X轴速度参数
			_acs.SetVelocity(Axis.ACSC_AXIS_1, MotionSpeed);
			_acs.SetAcceleration(Axis.ACSC_AXIS_1, MotionSpeed * 10);
			_acs.SetDeceleration(Axis.ACSC_AXIS_1, MotionSpeed * 10);

			//设置Z轴速度参数
			_acs.SetVelocity(Axis.ACSC_AXIS_8, MotionSpeed);
			_acs.SetAcceleration(Axis.ACSC_AXIS_8, MotionSpeed * 10);
			_acs.SetDeceleration(Axis.ACSC_AXIS_8, MotionSpeed * 10);
		}

		private void openGLControl1_OpenGLDraw(object sender, RenderEventArgs args)
		{
			OpenGL gl = openGLControl1.OpenGL;

			gl.ClearColor(0, 0, 0, 0);
			gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);

			gl.LoadIdentity();
			//gl.Translate(0.0f, 0.0f, -5.0f);  // 平移场景
		}

		private void LoadPoints(List<Point3D> points)
		{
			OpenGL gl = openGLControl1.OpenGL;

			gl.ClearColor(0, 0, 0, 0);
			gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);

			gl.LoadIdentity();
			//gl.Translate(0.0f, 0.0f, -5.0f);  // 平移场景

			gl.Begin(OpenGL.GL_POINTS);
			foreach (Point3D point in points)
			{
				// 设置点的颜色
				gl.Color(1.0f, 0.0f, 0.0f); // 红色

				// 设置点的大小
				gl.PointSize(5.0f); // 大小为5个单位
				gl.Vertex(point.X, point.Y, point.Z);
			}

			gl.End();

			gl.Flush();
		}

		private void openGLControl1_OpenGLInitialized(object sender, EventArgs e)
		{
		}

		private void openGLControl1_Resized(object sender, EventArgs e)
		{
		}

		private void openGLControl1_GDIDraw(object sender, RenderEventArgs args)
		{
		}

		#endregion ACS平台相关

		#region 菜单栏

		private void btn_motion_Click(object sender, EventArgs e)
		{
			Motion motion = new Motion(_acs);
			motion.Show();
		}

		private void Rtb_about_Click(object sender, EventArgs e) //关于界面
		{
			AboutBox mAboutBox = new AboutBox();
			mAboutBox.StartPosition = FormStartPosition.CenterScreen;
			mAboutBox.Show();
		}

		private void rtb_demo_Click(object sender, EventArgs e)
		{
			DemoShow demoShow = new DemoShow(_acs);
			demoShow.Show();
			demoShow.BringToFront();
		}

		private void Timer_RefreshUI_Tick(object sender, EventArgs e) //UI刷新
		{
			//状态灯刷新
			lamp_acs.State = _mAcsConnected ? LampColor.Green : LampColor.Silver;
			lamp_hexcal.State = _mBHexcalConnected ? LampColor.Green : LampColor.Silver;

			//时间栏
			//获取当前时间，构造形如 精确到秒,例如 2023-10-08 16:01:23
			rle_timer.Text = "当前时间: " + DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss");

			Plot2D(_pointCloud); //绘图

			//更新位置
			if (_mPoint3D != null)
			{
				rtb_xPos.Text = _mPoint3D.X.ToString("F3");
				rtb_yPos.Text = _mPoint3D.Y.ToString("F3");
				rtb_zPos.Text = _mPoint3D.Z.ToString("F3");
			}
		}

		#endregion 菜单栏
	}
}