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 pAxisHandle = new PHAND();        // 轴句柄
            PHAND pCrdHandle = new PHAND();         // 坐标系句柄
            
            // 打开控制器
            short rtn = NMC_DevOpen(devNum, pDevHandle);
            ErrorHandler.checkError(rtn);
            System.out.println("打开设备 - 成功");
            HAND devHandle = pDevHandle.getHandle();

            //打开轴,并获得轴句柄
            rtn = NMC_MtOpen(devHandle, axisNum, pAxisHandle);
            ErrorHandler.checkError(rtn);
            System.out.printf("打开轴%d - 成功\n", axisNum);

            //打开轴使能(根据实际情况使用)
            HAND axisHandle = pAxisHandle.getHandle();
            rtn = NMC_MtSetSvOn(axisHandle);
            ErrorHandler.checkError(rtn);
            System.out.printf("轴%d伺服使能 - 成功\n", axisNum);

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

            // 配置坐标系参数
            TCrdConfig crdCfg = new TCrdConfig();
            crdCfg.axCnts = 3;          // 设置3轴坐标系
            crdCfg.pAxArray[0] = 0;     // 轴0对应X
            crdCfg.pAxArray[1] = 1;     // 轴1对应Y
            crdCfg.pAxArray[2] = 2;     // 轴2对应Z
            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 = 10;     // 同步最大加速度10p/ms^2
            crdPara.synVelMax = 50;     // 最大合成速度50p/ms
            
            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("清除坐标系缓冲区 - 成功");

            /**********************************************************************************
             * 以上为坐标系初始化完成，坐标系(插补)运动基本流程：
             * 打开坐标系->初始化坐标系->压入运动指令->压入结束指令->开始运动->查询坐标系状态->等待运动完成
             ************************************************************************************/

            // 往缓冲区压入数据，其他缓冲区指令请参考编程手册和头文件
            NativeLong segNo = new NativeLong(1);
            NativeLong[] targetPos = new NativeLong[4];  // 目标位置数组
            NativeLong[] centerPos = new NativeLong[4];  // 圆心位置数组
            
            for (int i = 0; i < 4; i++) {
                targetPos[i] = new NativeLong(0);
                centerPos[i] = new NativeLong(0);
            }

            //直线插补，运动到(0,0,0)
            rtn = NMC_CrdLineXYZ(crdHandle, segNo, (short)0x7, targetPos, 0, 2.0, 0.5);
            ErrorHandler.checkError(rtn);
            System.out.println("添加直线运动指令 - 成功");
            segNo = new NativeLong(segNo.longValue() + 1);

            // 圆弧插补
            targetPos[0] = new NativeLong(10000); 
            targetPos[1] = new NativeLong(0);
            centerPos[0] = new NativeLong(5000);
            rtn = NMC_CrdArcCenter(crdHandle, segNo, targetPos, centerPos, (short)1, 0, 2.0, 0.5);
            ErrorHandler.checkError(rtn);
            System.out.println("添加圆弧运动指令 - 成功");
            segNo = new NativeLong(segNo.longValue() + 1);

            // 延时100ms
            rtn = NMC_CrdBufDelay(crdHandle, segNo, CRD_BUFF_DELAY_SCALE_MS, new NativeLong(100));
            ErrorHandler.checkError(rtn);
            System.out.println("添加延时指令 - 成功");
            segNo = new NativeLong(segNo.longValue() + 1);

            // 输出DO信号
            rtn = NMC_CrdBufDo(crdHandle, segNo, CRD_BUFF_DO_GPDO1, new NativeLong(1), new NativeLong(0));
            ErrorHandler.checkError(rtn);
            System.out.println("添加DO输出指令 - 成功");
            segNo = new NativeLong(segNo.longValue() + 1);

            // 结束运动指令
            rtn = NMC_CrdEndMtn(crdHandle);
            ErrorHandler.checkError(rtn);
            System.out.println("运动准备结束 - 成功");

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

            // 等待运动完成
            ShortByReference crdSts= new ShortByReference();
            while (true) {
                rtn = NMC_CrdGetSts(crdHandle, crdSts);
                ErrorHandler.checkError(rtn);
                if ((crdSts.getValue() & BIT_CORD_BUSY) == 0) {
                    break;  // 运动完成
                }
                Thread.sleep(100);  // 避免CPU占用过高
            }
            System.out.println("坐标系运动完成");

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