using rokae.clr;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static rokae.clr.CartesianPosition;

namespace xCoreSDK_CSharp.examples
{
    internal class WeldingDemo
    {
        internal xMateRobotDemo robotDemo;
        Thread readThread;
        CancellationTokenSource cancellationSource = new CancellationTokenSource();
        public WeldingDemo() { }
        public WeldingDemo(xMateRobotDemo xmate)
        {
            this.robotDemo = xmate;
        }

        private bool waitForFinish(String cmdID, int index)
        {
            ErrorCode ec;
            while (true)
            {
                var info = robotDemo.robot.queryEventInfo(Event.moveExecution, out ec);
                var _cmdID = (String)info["cmdID"];
                var _idx = (int)info["wayPointIndex"];
                var _err = (ErrorCode)info["error"];
                if (_err.value != 0)
                {
                    WeldForm.weldFormInstance.InfoBox = $"指令{_cmdID}:{_idx} 错误: {_err.message}";
                    return false;
                }

                if (cmdID == _cmdID && _idx == index)
                {
                    //WeldForm.weldFormInstance.InfoBox = $"指令 {cmdID}:{index} 已完成";
                    return true;
                }

                Thread.Sleep(200);
            }
        }
        void printErrorInfo(string func, ErrorCode _ec)
        {
            if (_ec.value != 0)
            {
                WeldForm.weldFormInstance.InfoBox = (func + ": " + _ec.message);
            }
        }
        private void readLaserDataAndOffset(Welding welding)
        {
            List<int> weld_offset = new List<int>();
            List<int> weld_valid = new List<int>();
            ErrorCode ec;
            while (true)
            {
                // 先配置好寄存器，读寄存器数值
                robotDemo.robot.readRegister("weld_offset", ref weld_offset, out ec);
                robotDemo.robot.readRegister("weld_valid", ref weld_valid, out ec);
                if (ec.value != 0 || weld_valid.Count < 2 || weld_offset.Count < 2)
                {
                    WeldForm.weldFormInstance.InfoBox = "读寄存器数据错误";
                    cancellationSource.Cancel();
                }
                bool valid = false;
                if (weld_valid[1] == 255)
                {
                    valid = true;
                }
                // 焊缝偏移原始数据单位0.1mm
                double x = weld_offset[0] / 10000.0;
                double z = weld_offset[1] / 10000.0;
                welding.addLaserTrackOffset(x, z, valid, out ec);

                if (cancellationSource.IsCancellationRequested)
                {
                    break;
                }
                // 每30ms更新一次传感器数据
                Thread.Sleep(30);
            }
        }

        private bool moveBlocked(MoveCommand.Type type, MoveCommand cmd)
        {
            ErrorCode ec;
            string id = "";
            robotDemo.robot.moveAppend(type, cmd, ref id, out ec);
            if (ec.value != 0)
            {
                printErrorInfo("moveAppend", ec);
                return false;
            }
            robotDemo.robot.moveStart(out ec);
            if (ec.value != 0)
            {
                printErrorInfo("moveStart", ec);
                return false;
            }
            return waitForFinish(id, 0);
        }

        void laserTrackMotion(Welding welding)
        {
            ErrorCode ec;
            // 设置焊枪工具坐标系
            Toolset toolset = new Toolset()
            {
                end = new Frame(new double[] { 0.01052, 0.02531, 0.35549, 0.0, -45.0 / 180.0 * Math.PI, 45.0 / 180.0 * Math.PI })
            };
            robotDemo.robot.setToolset(toolset, out ec);
            robotDemo.robot.setDefaultSpeed(10, out ec);
            robotDemo.robot.setDefaultZone(0, out ec);

            // 设置传感器手眼标定结果
            Frame sensor_frame = new Frame(new double[] { 0.029456, -0.014151, 0.070047, 4.5006 / 180.0 * Math.PI, 4.0438 / 180.0 * Math.PI, -97.358 / 180.0 * Math.PI });
            welding.setSensorFrame(sensor_frame, out ec);

            MoveCommand absj = new MoveCommand();
            absj.jointTarget.joints = new List<double> { 0, 0.523559, -1.570796, 0, -1.047198, 0};
            absj.speed = 500;
            MoveCommand l = new MoveCommand();
            l.cartTarget.trans = new double[] { 0.54965740200151569, 0.4433802542367879, 0.043973778965744614 };
            l.cartTarget.rpy = new double[] { 3.1415519451956078, 0.78542606866956755, 2.3561663227577045 };

            WeldForm.weldFormInstance.InfoBox = "开始运动到起点";
            if(!moveBlocked(MoveCommand.Type.MoveAbsJ, absj))
            {
                cancellationSource.Cancel();
                WeldForm.weldFormInstance.InfoBox = "发生错误，停止";
                return;
            }
            WeldForm.weldFormInstance.InfoBox = "开始激光跟踪";

            // 开始跟踪，需要在停止状态下打开，否则导致提前结束跟踪
            while (robotDemo.robot.operationState(out ec) != OperationState.idle) ;
            welding.startTracking(out ec);
            if(!moveBlocked(MoveCommand.Type.MoveL, l))
            {
                cancellationSource.Cancel();
                WeldForm.weldFormInstance.InfoBox = "发生错误，停止";
                return;
            }
            welding.stopTracking(out ec);
            WeldForm.weldFormInstance.InfoBox = "激光跟踪结束";
            cancellationSource.Cancel();
        }

        public void laserTrackExample()
        {
            cancellationSource.Dispose();
            cancellationSource = new CancellationTokenSource();

            Welding welding = robotDemo.robot.welding();
            Task.Factory.StartNew(()=> readLaserDataAndOffset(welding), cancellationSource.Token);
            Task.Factory.StartNew(()=> laserTrackMotion(welding), cancellationSource.Token);
            cancellationSource.Token.Register(() =>
            {
                robotDemo.robot.stop(out ErrorCode ec);
            });
        }

        public void stopTracking()
        {
            robotDemo.robot.stop(out ErrorCode ec);
            while (robotDemo.robot.operationState(out ec) != OperationState.idle);
            
            cancellationSource.Cancel();
            robotDemo.robot.welding().stopTracking(out ec);
            robotDemo.robot.moveReset(out ec);
        }

        String PrintFrame(Frame frame)
        {
            string transString = $"Trans: [ {frame.trans[0]}, {frame.trans[1]}, {frame.trans[2]} ]";
            string rpyString = $"RPY: [ {frame.rpy[0]}, {frame.rpy[1]}, {frame.rpy[2]} ]";

            return $"{transString}, {rpyString}";
        }
        void printInfo(String info)
        {
            Debug.WriteLine(info);
            WeldForm.weldFormInstance.InfoBox = info;
        }

        void calcWeldOffsetMulti(Welding welding)
        {
            ErrorCode ec;
            // 计算坐标系偏移
            Frame start_point = new Frame(new double[] { 0.563, -0.2, 0.4324, Math.PI, 0, Math.PI });
            Frame target_point = new Frame(new double[] { 0.563, 0.0, 0.4324, Math.PI, 0, Math.PI });
            Frame start_offs = new Frame();
            Frame target_offs = new Frame();
            start_offs.trans[0] = 0.003;
            target_offs.trans[1] = 0.008;

            // 偏移方式一: 焊道坐标系Z轴方向同末端工具坐标系Z轴
            welding.calcWeldOffset(ref start_point, start_offs, ref target_point, target_offs, out ec);
            printInfo($"[偏移方式一] 偏移后起始点:{PrintFrame(start_point)},\n目标点:{PrintFrame(target_point)}");

            // 偏移方式二: 焊道坐标系Z轴由XY叉乘得到
            // 辅助接口: 计算焊道坐标系
            Frame line_start = new Frame(new double[] { 0.553, 0.107, 0.309, Math.PI, 0, -Math.PI });
            Frame line_target = new Frame(new double[] { 0.68, 0.03543, 0.309, Math.PI, 0, -Math.PI });
            Frame teach_point_1 = new Frame(new double[] { 0.5473, -0.044443, 0.309, -2.50422633, -0.02798354, 3.12087822082 });
            Frame teach_point_2 = new Frame(new double[] { 0.515890, 0.0220580, 0.309, 2.803391761, -0.2970729056392, -2.9592301950 });
            double offset_1 = 0.05, offset_2 = 0.04;

            double[] offset_start = new double[3] { 0, 0, 0 };
            double[] offset_target = new double[3] { 0, 0, 0 };

            welding.transformOffsetByTeaching(line_start, line_target, teach_point_1, offset_1, teach_point_2, offset_2,ref offset_start, ref offset_target,out ec);
            printInfo($"坐标系转换结果 - 起点:{string.Join(", ", offset_start.Select(i => i.ToString()))}, 终点:{string.Join(", ", offset_target.Select(i => i.ToString()))}");
            welding.calcWeldOffsetCalculatedZ(ref start_point, start_offs, ref target_point, target_offs,out ec);
            printInfo($"[偏移方式二] 偏移后起始点:{PrintFrame(start_point)},\n目标点:{PrintFrame(target_point)}");

            // 调整摆动偏移量 X+0.001m
            // 支持摆动中调整
            welding.adjustTrackOffset(new double[]{ 1e-3, 0, 0},out ec);

            // 多段轨迹多层多道偏移计算
            Frame way_point0 = new Frame(new double[]{ 0.98873878627071521, 0.32799716615705909, 0.50667443610447238, -2.7495292945202761, -1.8806033765539331e-06, -3.1415917428588926 }) ;
            Frame way_point1 = new Frame(new double[] { 0.70435025687185759, 0.41419272120934003, 0.31575354435938097, -2.5732143763667032, -0.17399606099405901, -3.1415922388504662 });
            Frame way_point2 = new Frame(new double[] { 0.86204854682289622, 0.57179479424978619, 0.35782254260870483, -2.8272425864960207, -0.16851548275263212, -3.0980736037376517 });
            List<Frame> way_points =new List<Frame>{ start_point, way_point0, way_point1, way_point2, target_point};
            
            Frame teach_point_3 = new Frame(new double[] { 0.90, 0.4, 0.5, -2.7495292945202761, -1.8806033765539331e-06, -3.1415917428588926 });
            Frame teach_point_4 = new Frame(new double[] { 0.72, 0.42, 0.32, -2.5732143763667032, -0.17399606099405901, -3.1415922388504662 });
            Frame teach_point_5 = new Frame(new double[] { 0.86, 0.59, 0.34, -2.5, -0.16851548275263212, -3.0980736037376517 });
            List<Frame> teach_points = new List<Frame>{ teach_point_3, teach_point_4, teach_point_5, teach_point_2};
            List<double[]> offsets= new List<double[]>{ new double[]{ 0.01, 0, 0.01, 0, 0, 0, 0}, new double[] { 0, 0, 0.015, 0, 0, 0, 0}, new double[] { 0.02, 0, 0.02, 0, 0, 0, 0} };
            int error_index = 0;
            var result = welding.calcWeldOffset_Multi(way_points, teach_points, offsets,ref error_index,out ec);
            if (ec.value!=0)
            {
                printInfo( $"偏移计算失败{ec.message}, 错误点位:{error_index}");
            }
            else
            {
                printInfo( $"多轨迹偏移结果:{ error_index}" );
                for (int i = 0; i < result.Count; ++i)
                {
                    printInfo($" --- 第{i + 1}道 ---");
                    for (int j = 0; j < result[i].Count; ++j)
                    {
                        printInfo($"轨迹点{j}:{PrintFrame(result[i][j])}");
                    }
                }
            }
        }
    
        public void multi()
        {
            Welding welding = robotDemo.robot.welding();
            calcWeldOffsetMulti(welding);
        }
    }
}
