import static lib.mtn_lib20.*;
import static lib.mtn_lib20_com.*;
import static lib.mtn_lib20_ext.*;
import static lib.mtn_lib20_oem.*;
import static lib.mtn_lib20_err.*;
import com.sun.jna.*;
import com.sun.jna.ptr.*;

public class Main {
    static {
        String libPath = System.getProperty("user.dir") + "\\lib";
        System.setProperty("java.library.path", libPath);
        System.load(libPath + "\\msvcr100.dll");
        System.load(libPath + "\\msvcp100.dll");
        System.load(libPath + "\\nmc_lib20.dll");
    }
    // 错误处理工具类
    private static class ErrorHandler {
        public static void checkError(short rtn) {
            if (rtn != 0) {
                throw new RuntimeException(String.format("操作失败，错误代码: %d", rtn));
            }
        }
    }

    public static void main(String[] args) {
        System.out.println("坐标系飞行跟随功能测试开始...");

        try {
            short devNum = 0;                       // 设备序号，从0开始
            short axisNum = 0;                      // 轴号从0开始
            PHAND pDevHandle = new PHAND();         // 设备句柄
            PHAND[] pAxisHandles = new PHAND[4];    // 轴句柄数组
            PHAND pCrdHandle = new PHAND();         // 坐标系句柄
            
            // 打开控制器
            short rtn = NMC_DevOpen(devNum, pDevHandle);
            ErrorHandler.checkError(rtn);
            System.out.println("打开设备 - 成功");
            HAND devHandle = pDevHandle.getHandle();

            // 初始化轴句柄数组
            for (int i = 0; i < 4; i++) {
                pAxisHandles[i] = new PHAND();
            }

            // 打开4个轴并设置伺服使能
            for (short i = axisNum; i < 4; i++) {
                rtn = NMC_MtOpen(devHandle, i, pAxisHandles[i]);
                ErrorHandler.checkError(rtn);
                System.out.printf("打开轴%d - 成功\n", i);
                
                HAND axisHandle = pAxisHandles[i].getHandle();
                rtn = NMC_MtSetSvOn(axisHandle);
                ErrorHandler.checkError(rtn);
                System.out.printf("轴%d伺服使能 - 成功\n", i);
                
                rtn = NMC_MtZeroPos(axisHandle);
                ErrorHandler.checkError(rtn);
                System.out.printf("轴%d位置清零 - 成功\n", i);
            }

            // 打开坐标系
            rtn = NMC_CrdOpen(devHandle, pCrdHandle);
            ErrorHandler.checkError(rtn);
            System.out.println("打开坐标系 - 成功");
            HAND crdHandle = pCrdHandle.getHandle();

            // 配置坐标系参数
            TCrdConfig crdCfg = new TCrdConfig();
            crdCfg.axCnts = 2;          // 设置3轴坐标系
            crdCfg.pAxArray[0] = 0;     // 轴0对应X
            crdCfg.pAxArray[1] = 1;     // 轴1对应Y
            crdCfg.pAxArray[2] = -1;    // 轴2对应Z，-1表示不使用
            crdCfg.pAxArray[3] = -1;    // 轴3对应A，-1表示不使用
            crdCfg.port[0] = 0;         // 轴0端口为0
            crdCfg.port[1] = 0;         // 轴1端口为0
            crdCfg.port[2] = 0;         // 轴2端口为0
            crdCfg.port[3] = 0;         // 轴3端口为0

            //配置坐标系
            rtn = NMC_CrdConfig(crdHandle, crdCfg);
            ErrorHandler.checkError(rtn);
            System.out.println("配置坐标系 - 成功");

            // 设置坐标系运动参数
            TCrdPara crdPara = new TCrdPara();
            crdPara.orgFlag = 1;         // 默认原点(0,0,0,0)
            crdPara.offset[0] = 0;      // X轴偏移为0
            crdPara.offset[1] = 0;      // Y轴偏移为0
            crdPara.offset[2] = 0;      // Z轴偏移为0
            crdPara.offset[3] = 0;      // A轴偏移为0
            crdPara.synAccMax = 100;    // 同步最大加速度100p/ms^2
            crdPara.synVelMax = 500;    // 最大合成速度,允许范围内，两个参数尽可能大
            
            rtn = NMC_CrdSetPara(crdHandle, crdPara);
            ErrorHandler.checkError(rtn);
            System.out.println("设置坐标系运动参数 - 成功");
            /******************* 以下配置可选 *******************/
            // 设置坐标系安全参数
            TCrdSafePara crdSafePara = new TCrdSafePara();
            crdSafePara.estpDec = 1000;     // 急停减速度
            crdSafePara.maxAcc = 50;        // 坐标系最大加速度50p/ms^2
            crdSafePara.maxVel = 800;       // 坐标系最大速度800p/ms
            rtn = NMC_CrdSetSafePara(crdHandle, crdSafePara);
            ErrorHandler.checkError(rtn);
            System.out.println("设置坐标系安全参数 - 成功");
            /******************* 以上配置可选 *******************/

            // 清除坐标系错误状态和缓冲区
            rtn = NMC_CrdClrError(crdHandle);
            ErrorHandler.checkError(rtn);
            System.out.println("清除坐标系错误状态 - 成功");

            //压入指令之前需要清空缓存区
            rtn = NMC_CrdBufClr(crdHandle);
            ErrorHandler.checkError(rtn);
            System.out.println("清除坐标系缓冲区 - 成功");

            // 设置飞行跟随参数
            TFlyFollowPara flyPrm = new TFlyFollowPara();
            flyPrm.masterAxNo = 3;              // 传送带的轴
            flyPrm.followedAxNo = 1;            // 跟随哪个轴的方向运动，0:X，1:Y
            flyPrm.posCoe = 1;                  // 传送轴当量和跟随方向轴的当量比例，跟随当量 / 传送轴当量
            flyPrm.useEnc = 0;                  // 使用编码器，0：规划，1：编码器
            
            rtn = NMC_CrdSetFlyFollow(crdHandle, flyPrm);
            ErrorHandler.checkError(rtn);
            System.out.println("设置飞行跟随参数 - 成功");
            
            // 添加运动指令
            NativeLong segNo = new NativeLong(0);
            NativeLong[] tgtPos = new NativeLong[4];  // 目标位置数组
            for (int i = 0; i < 4; i++) {
                tgtPos[i] = new NativeLong(0);
            }

            // 缓冲区等待特定位置，满足条件才执行下一条指令
            rtn = NMC_CrdBufWaitPos(crdHandle, segNo, (short)3, (short)1, 
                                  new NativeLong(5000), (short)1, new NativeLong(100000));
            ErrorHandler.checkError(rtn);
            segNo = new NativeLong(segNo.longValue() + 1);
            System.out.println("添加等待位置指令 - 成功");

            // 缓冲区设置飞行跟随功能的启动和关闭
            rtn = NMC_CrdBufEnableFlyFollow(crdHandle, segNo, (short)1);
            ErrorHandler.checkError(rtn);
            segNo = new NativeLong(segNo.longValue() + 1);
            System.out.println("启用飞行跟随 - 成功");

            // 画正方型
            tgtPos[0] = new NativeLong(10000); tgtPos[1] = new NativeLong(10000); tgtPos[2] = new NativeLong(0);
            rtn = NMC_CrdLineXYZEx(crdHandle, segNo, (short)0x07, tgtPos, 0.000f, 50.000f, 4.000f, (short)0);
            ErrorHandler.checkError(rtn);
            segNo = new NativeLong(segNo.longValue() + 1);
            
            tgtPos[0] = new NativeLong(20000); tgtPos[1] = new NativeLong(10000); tgtPos[2] = new NativeLong(0);
            rtn = NMC_CrdLineXYZEx(crdHandle, segNo, (short)0x07, tgtPos, 0.000f, 50.000f, 4.000f, (short)0);
            ErrorHandler.checkError(rtn);
            segNo = new NativeLong(segNo.longValue() + 1);
            
            tgtPos[0] = new NativeLong(20000); tgtPos[1] = new NativeLong(20000); tgtPos[2] = new NativeLong(0);
            rtn = NMC_CrdLineXYZEx(crdHandle, segNo, (short)0x07, tgtPos, 0.000f, 50.000f, 4.000f, (short)0);
            ErrorHandler.checkError(rtn);
            segNo = new NativeLong(segNo.longValue() + 1);
            
            tgtPos[0] = new NativeLong(10000); tgtPos[1] = new NativeLong(20000); tgtPos[2] = new NativeLong(0);
            rtn = NMC_CrdLineXYZEx(crdHandle, segNo, (short)0x07, tgtPos, 0.000f, 50.000f, 4.000f, (short)0);
            ErrorHandler.checkError(rtn);
            segNo = new NativeLong(segNo.longValue() + 1);
            
            tgtPos[0] = new NativeLong(10000); tgtPos[1] = new NativeLong(10000); tgtPos[2] = new NativeLong(0);
            rtn = NMC_CrdLineXYZEx(crdHandle, segNo, (short)0x07, tgtPos, 0.000f, 50.000f, 4.000f, (short)0);
            ErrorHandler.checkError(rtn);
            segNo = new NativeLong(segNo.longValue() + 1);
            
            System.out.println("添加矩形路径指令 - 成功");

            // 结束(指令压入)缓冲区运动（等待运动完后才结束区运动，并置空闲标志）
            rtn = NMC_CrdEndMtn(crdHandle);
            ErrorHandler.checkError(rtn);
            System.out.println("运动准备结束 - 成功");

            // 开始坐标系运动
            rtn = NMC_CrdStartMtn(crdHandle);
            ErrorHandler.checkError(rtn);
            System.out.println("开始坐标系运动 - 成功");

            // 4轴走点位运动
            HAND axis3Handle = pAxisHandles[3].getHandle();
            rtn = NMC_MtMovePtpRel(axis3Handle, 1, 1, 0, 0, 1, (short)0, new NativeLong(90000));
            ErrorHandler.checkError(rtn);
            System.out.println("轴3开始相对运动 - 成功");

        } catch (Exception e) {
            System.err.println("测试过程中发生错误: " + e.getMessage());
            e.printStackTrace();
        } finally {
            System.out.println("\n释放资源...");
            // 实际应用中需要添加关闭轴、坐标系和设备的代码
            // NMC_CrdClose(pCrdHandle.getHandle());
            // for (PHAND pAxisHandle : pAxisHandles) {
            //     NMC_MtClose(pAxisHandle.getHandle());
            // }
            // NMC_DevClose(pDevHandle.getHandle());
        }
        
        System.out.println("坐标系飞行跟随功能测试完成");
    }
}