using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Runtime.InteropServices;

using static pci.zecm;

namespace test
{
    class Program
    {
        public enum OP_MODE
        {
            OP_MODE_PP = 1,
            OP_MODE_PV = 3,
            OP_MODE_PT = 4,
            OP_MODE_HM = 6,
            OP_MODE_CSP = 8,
            OP_MODE_CSV = 9,
            OP_MODE_CST = 10,
        };

        const uint NUM_SERVOS = 1;
        const uint NUM_DO = 1;
        const uint NUM_DI = 1;

        static void EXIT_IF_FAIL(string expr, int ret)
        {
            if (ret != 0)
            {
                Console.WriteLine("{0} failed, error({1}): {2}", expr, ret, DriverGetErrStr(ret));
                Environment.Exit(1);
            }
        }
        static void RETURN_IF_FAIL(string expr, int ret, bool show_success = true)
        {
            if (ret != 0)
            {
                throw new Exception(string.Format("{0} failed, error({1}): {2}", expr, ret, DriverGetErrStr(ret)));
            }
            if (show_success) Console.WriteLine("{0} succeeded.", expr);
        }

        static UInt16 Uint16BitGet(UInt16 v, UInt16 bit) { return (UInt16)((v >> bit) & 0x1); }
        static void Uint16BitSet(ref UInt16 v, UInt16 bit, UInt16 value) { v = (UInt16)((v & ~(1 << bit)) | ((value & 1) << bit)); }

        class servo_ctrl_word
        {
            public servo_ctrl_word()
            {
                v = 0;
            }
            public UInt16 switch_on
            {
                get { return Uint16BitGet(v, 0); }
                set { Uint16BitSet(ref v, 0, value); }
            }
            public UInt16 enable_voltage
            {
                get { return Uint16BitGet(v, 1); }
                set { Uint16BitSet(ref v, 1, value); }
            }
            public UInt16 quick_stop
            {
                get { return Uint16BitGet(v, 2); }
                set { Uint16BitSet(ref v, 2, value); }
            }
            public UInt16 enable_operation
            {
                get { return Uint16BitGet(v, 3); }
                set { Uint16BitSet(ref v, 3, value); }
            }
            public UInt16 fault_reset
            {
                get { return Uint16BitGet(v, 7); }
                set { Uint16BitSet(ref v, 7, value); }
            }
            public UInt16 v;
        };

        class servo_stat_word
        {
            public servo_stat_word()
            {
                v = 0;
            }
            public UInt16 ready_to_switch_on
            {
                get { return Uint16BitGet(v, 0); }
                set { Uint16BitSet(ref v, 0, value); }
            }
            public UInt16 switched_on
            {
                get { return Uint16BitGet(v, 1); }
                set { Uint16BitSet(ref v, 1, value); }
            }
            public UInt16 operation_enabled
            {
                get { return Uint16BitGet(v, 2); }
                set { Uint16BitSet(ref v, 2, value); }
            }
            public UInt16 fault
            {
                get { return Uint16BitGet(v, 3); }
                set { Uint16BitSet(ref v, 3, value); }
            }
            public UInt16 voltage_enabled
            {
                get { return Uint16BitGet(v, 4); }
                set { Uint16BitSet(ref v, 4, value); }
            }
            public UInt16 quick_stop
            {
                get { return Uint16BitGet(v, 5); }
                set { Uint16BitSet(ref v, 5, value); }
            }
            public UInt16 switch_on_disabled
            {
                get { return Uint16BitGet(v, 6); }
                set { Uint16BitSet(ref v, 6, value); }
            }
            public UInt16 warning
            {
                get { return Uint16BitGet(v, 7); }
                set { Uint16BitSet(ref v, 7, value); }
            }
            public UInt16 remote
            {
                get { return Uint16BitGet(v, 9); }
                set { Uint16BitSet(ref v, 9, value); }
            }
            public UInt16 target_reached
            {
                get { return Uint16BitGet(v, 10); }
                set { Uint16BitSet(ref v, 10, value); }
            }
            public UInt16 internal_limit_active
            {
                get { return Uint16BitGet(v, 11); }
                set { Uint16BitSet(ref v, 11, value); }
            }
            public UInt16 v;
        };

        class pdo_entrys_config
        {
            public class servo_entrys_config
            {
                public Int32 po_offset_mode;
                public Int32 po_offset_ctrl;
                public Int32 po_offset_pos;
                public Int32 pi_offset_mode;
                public Int32 pi_offset_stat;
                public Int32 pi_offset_pos;
                public Int32 pi_offset_err;
                public Int32 pi_offset_inputs;
            }
            public pdo_entrys_config()
            {
                servo_offset = new servo_entrys_config[NUM_SERVOS];
                for (int i = 0; i < NUM_SERVOS; i++) servo_offset[i] = new servo_entrys_config();
                pi_offset_di = new Int32[NUM_DI];
                po_offset_do = new Int32[NUM_DO];

            }
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = (int)NUM_SERVOS)]
            public servo_entrys_config[] servo_offset;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = (int)NUM_DI)]
            public Int32[] pi_offset_di;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = (int)NUM_DO)]
            public Int32[] po_offset_do;
        };

        class servo_output
        {
            public servo_output()
            {
                mode = (byte)OP_MODE.OP_MODE_CSP;
                ctrl = new servo_ctrl_word();
                p = 0;
            }

            public byte mode;
            public servo_ctrl_word ctrl;
            public Int32 p;
        };

        class servo_input
        {
            public servo_input()
            {
                mode = (byte)OP_MODE.OP_MODE_CSP;
                stat = new servo_stat_word();
                p = 0;
                error = 0;
                inputs = 0;
            }

            public byte mode;
            public servo_stat_word stat;
            public Int32 p;
            public UInt16 error;
            public UInt32 inputs;
        };

        class proc_img_o
        {
            public servo_output[] servos = new servo_output[NUM_SERVOS];
            public UInt16[] outputs = new UInt16[NUM_DO];

            public proc_img_o()
            {
                for (int i = 0; i < NUM_SERVOS; i++) servos[i] = new servo_output();
                for (int i = 0; i < NUM_DO; i++) outputs[i] = new UInt16();
            }
            public void CopyToOutputs(ref byte[] buff)
            {
                for (int i = 0; i < NUM_SERVOS; i++)
                {
                    if (__entrys_config.servo_offset[i].po_offset_mode != -1) buff[__entrys_config.servo_offset[i].po_offset_mode] = servos[i].mode;
                    if (__entrys_config.servo_offset[i].po_offset_ctrl != -1) CopyToByteArray(servos[i].ctrl.v, ref buff, __entrys_config.servo_offset[i].po_offset_ctrl);
                    if (__entrys_config.servo_offset[i].po_offset_pos != -1) CopyToByteArray((UInt32)servos[i].p, ref buff, __entrys_config.servo_offset[i].po_offset_pos);
                }
                for (int i = 0; i < NUM_DO; i++)
                {
                    if (__entrys_config.po_offset_do[i] != -1) CopyToByteArray((UInt16)outputs[i], ref buff, __entrys_config.po_offset_do[i]);
                }
            }

            public static void CopyToByteArray(UInt16 v, ref byte[] buff, int start)
            {
                buff[start + 0] = (byte)v;
                buff[start + 1] = (byte)(v >> 8);
            }
            public static void CopyToByteArray(UInt32 v, ref byte[] buff, int start)
            {
                buff[start + 0] = (byte)v;
                buff[start + 1] = (byte)(v >> 8);
                buff[start + 2] = (byte)(v >> 16);
                buff[start + 3] = (byte)(v >> 24);
            }
            public static void CopyToByteArray(UInt64 v, ref byte[] buff, int start)
            {
                buff[start + 0] = (byte)v;
                buff[start + 1] = (byte)(v >> 8);
                buff[start + 2] = (byte)(v >> 16);
                buff[start + 3] = (byte)(v >> 24);
                buff[start + 4] = (byte)(v >> 32);
                buff[start + 5] = (byte)(v >> 40);
                buff[start + 6] = (byte)(v >> 48);
                buff[start + 7] = (byte)(v >> 56);
            }
        };

        class proc_img_i
        {
            public servo_input[] servos = new servo_input[NUM_SERVOS];
            public UInt16[] inputs = new UInt16[NUM_DI];

            public proc_img_i()
            {
                for (int i = 0; i < NUM_SERVOS; i++) servos[i] = new servo_input();
                for (int i = 0; i < NUM_DI; i++) inputs[i] = new UInt16();
            }

            public void CopyFromInputs(byte[] buff)
            {
                for (int i = 0; i < NUM_SERVOS; i++)
                {
                    if (__entrys_config.servo_offset[i].pi_offset_mode != -1) servos[i].mode = buff[__entrys_config.servo_offset[i].po_offset_mode];
                    if (__entrys_config.servo_offset[i].pi_offset_stat != -1) servos[i].stat.v = BitConverter.ToUInt16(buff, __entrys_config.servo_offset[i].pi_offset_stat);
                    if (__entrys_config.servo_offset[i].pi_offset_pos != -1) servos[i].p = BitConverter.ToInt32(buff, __entrys_config.servo_offset[i].pi_offset_pos);
                    if (__entrys_config.servo_offset[i].pi_offset_err != -1) servos[i].error = BitConverter.ToUInt16(buff, __entrys_config.servo_offset[i].pi_offset_err);
                }
                for (int i = 0; i < NUM_DO; i++)
                {
                    if (__entrys_config.po_offset_do[i] != -1) inputs[i] = BitConverter.ToUInt16(buff, __entrys_config.pi_offset_di[i]);
                }
            }
        };

        class servos_t
        {
            public servo_output output = new servo_output();         // 电机输出
            public servo_input input = new servo_input();            // 电机输入

            public bool motor_control(int axis)
            {
                output.ctrl.v = 0;
                output.ctrl.enable_voltage = 1;
                output.ctrl.quick_stop = 1;
                output.ctrl.enable_operation = (UInt16)(input.stat.switched_on | input.stat.operation_enabled);
                output.ctrl.switch_on = input.stat.ready_to_switch_on;

                if (input.stat.fault != 0 && fault_reset_delay == 0)
                {
                    // 延时1秒再尝试错误恢复，防止伺服在错误和恢复中反复快速切换损坏电路
                    Console.WriteLine("axis {0}: delay fault reset, ctrl:{1:X}, stat:{2:X}", axis, output.ctrl.v, input.stat.v);
                    fault_reset_delay = 2000; // 2s(即2000个周期)
                    run_delay = 2000; // 当错误恢复成功伺服出力后，延时2s再开始加速(根据抱闸动作时间适当调整)
                    return input.stat.operation_enabled != 0;
                }

                // 伺服发生错误，且在延时等待处理中
                if (fault_reset_delay != 0)
                {
                    fault_reset_delay--;
                    if (fault_reset_delay == 0)
                    {
                        // 延时减到0，尝试进行错误恢复
                        Console.WriteLine("axis {0}: start fault reset", axis);
                        output.ctrl.fault_reset = 1;
                    }
                }

                return input.stat.operation_enabled != 0;
            }
            public void motor_run(int axis, UInt64 loops, Int32 range_min, Int32 range_max, double step)
            {
                bool run = motor_control(axis);

                // 显示设置状态
                if (loops % 1000 == 0)
                {
                    Console.WriteLine("servo[{0}]: run:{4}, p={1,10}, ctrl:0x{2:X4} stat=0x{3:X4}", axis, input.p, output.ctrl.v, input.stat.v, run);
                }

                // 伺服未出力，或已出力但延时未结束，应停止转动
                if (!run || run_delay != 0)
                {
                    output.p = input.p;
                    zero_p = (double)output.p;
                    offset_p = 0;
                    reverse = false;
                }

                // 伺服已出力，延时计数递减
                if (run && run_delay != 0) run_delay--;

                // 伺服已出力，延时结束，进行正常控制
                if (run && run_delay == 0)
                {
                    // 不同轴单位可能不一样，下表定义各轴位置范围及增量
                    if (offset_p < range_min) reverse = false;
                    if (offset_p > range_max) reverse = true;
                    offset_p += (reverse ? -1 : 1) * step;
                    output.p = (Int32)(zero_p + offset_p);
                }
            }

            private int run_delay = 0;                              // 延时启动
            private int fault_reset_delay = 0;                      // 延时再错误恢复
            private double zero_p = 0;                              // 零点位置
            private double offset_p = 0;                            // 偏移输出位置
            private bool reverse = false;                           // 当前方向
        };

        static servos_t[] __servos = new servos_t[]  
        {
            new servos_t(),
            new servos_t(),
            new servos_t(),
        };

        private class user_control
        {
            const ushort LED_GREEN = 0;   // 绿灯
            const ushort LED_RED = 1;     // 红灯

            const ushort BTN_CLOSE = 0;   // 接近传感器
            const ushort BTN_RUN = 1;     // 绿色按钮
            const ushort BTN_STOP = 2;    // 红色按钮

            ushort led_green = 0;
            ushort led_red = 0;
            ushort btn_close = 0;
            ushort btn_run = 0;
            ushort btn_stop = 0;

            static int count = 0;

            public void UpdateButton(UInt16[] buff)
            {
                if (__entrys_config.pi_offset_di[0] != -1)
                {
                    btn_close = Uint16BitGet((UInt16)buff[0], BTN_CLOSE);
                    btn_run = Uint16BitGet((UInt16)buff[0], BTN_RUN);
                    btn_stop = Uint16BitGet((UInt16)buff[0], BTN_STOP);
                }
            }

            public void UpdateLED(ref UInt16[] buff)
            {
                if (__entrys_config.po_offset_do[0] != -1)
                {
                    ushort temp = (UInt16)buff[0];
                    Uint16BitSet(ref temp, LED_GREEN, led_green);
                    Uint16BitSet(ref temp, LED_RED, led_red);
                    buff[0] = temp;
                }
            }

            public void run(ref int run_cmd, ref int stop_cmd, ref int halt_cmd)
            {
                if (btn_run == 0)
                {
                    // 输入低有效
                    run_cmd = 1;
                    stop_cmd = 0;
                    halt_cmd = 0;
                }
                if(btn_stop == 0)
                {
                    // 输入低有效
                    run_cmd = 0;
                    stop_cmd = 1;
                }
                if (btn_close == 0)
                {
                    // 输入低有效
                    run_cmd = 0;
                    halt_cmd = 1;
                }

                if(run_cmd == 0)
                {
                    count++;
                    if (count > 500)
                    {
                        if (led_green == 0) led_green = 1;
                        else led_green = 0;
                        count = 0;
                    }
                }
                else
                {
                    led_green = 1;
                }
                if (halt_cmd == 0)
                {
                    led_red = 0;
                }
                else
                {
                    led_green = 0;
                    led_red = 1;
                }
            }
        }

        static user_control __user_controller = new user_control();

        static pdo_entrys_config __entrys_config;
        
        static private void pdo_entrys_config_init()
        {
            __entrys_config = new pdo_entrys_config();
            int servo_pi_offset = 12, servo_po_offset = 12;
            for(int i = 0; i < NUM_SERVOS; i++)
            {
                __entrys_config.servo_offset[i].po_offset_mode = -1;
                __entrys_config.servo_offset[i].po_offset_ctrl = 0 + i * servo_po_offset;
                __entrys_config.servo_offset[i].po_offset_pos = 2 + i * servo_po_offset;
                __entrys_config.servo_offset[i].pi_offset_mode = -1;
                __entrys_config.servo_offset[i].pi_offset_stat = 0 + i * servo_pi_offset;
                __entrys_config.servo_offset[i].pi_offset_pos = 2 + i * servo_pi_offset;
                __entrys_config.servo_offset[i].pi_offset_err = 6 + i * servo_pi_offset;
                __entrys_config.servo_offset[i].pi_offset_inputs = 8 + i * servo_pi_offset;
            }
            for(int i = 0; i < NUM_DI; i++)
            {
                __entrys_config.pi_offset_di[i] = i * 16 + (int)NUM_SERVOS * servo_pi_offset;
            }
            for (int i = 0; i < NUM_DO; i++)
            {
                __entrys_config.po_offset_do[i] = i * 16 + (int)NUM_SERVOS * servo_po_offset;
            }
        }

        static Int32[] __range_min = new Int32[] { -50000 };
        static Int32[] __range_max = new Int32[] {  50000 };
        static double[] __step_size = new double[] { 50 };

        static void ServosTest(IntPtr ecat_dev, string eni_file)
        {
            RETURN_IF_FAIL("EcatBusRun", EcatBusRun(ecat_dev, eni_file));

            // 请求切换状态
            byte state = 0;
            RETURN_IF_FAIL("EcatRequestMasterState", EcatRequestMasterState(ecat_dev, EcatStateO));
            for (int i = 0; ; i++)
            {
                RETURN_IF_FAIL("EcatGetMasterState", EcatGetMasterState(ecat_dev, ref state), false);
                Console.WriteLine("{1,2}: request is: 8, curr is: {0}", state, i);
                Thread.Sleep(500);
                if (state == EcatStateO) break;
            }

            byte run_mode = (byte)OP_MODE.OP_MODE_CSP;

            // 配置电机运行模式
            RETURN_IF_FAIL("EcatCoeSDODownload", EcatCoeSDODownload(ecat_dev, 3, 0x6060, 0, 1, ref run_mode, 1000));

            // 预计填入数据
            for (int i = 0; i < 10; i++)
            {
                EcatPIOutputQueuePush(ecat_dev, false, 100);
            }

            // 过程镜像输入输出循环
            ECAT_ENI_INFO ptInfo = new ECAT_ENI_INFO();
            RETURN_IF_FAIL("EcatGetEniInfo", EcatGetEniInfo(ecat_dev, ref ptInfo));
            Console.WriteLine("Eni Info input size:{0}, output size: {1}", ptInfo.u32PIInputByteSize, ptInfo.u32PIOutputByteSize);

            pdo_entrys_config_init();

            byte[] inputs = new Byte[ptInfo.u32PIInputByteSize];
            byte[] outputs = new Byte[ptInfo.u32PIOutputByteSize];
            proc_img_o po = new proc_img_o();
            proc_img_i pi = new proc_img_i();
            
            UInt64 loops = 0;
            UInt32 ecat_queue_size = 0;
            int run_cmd = 0, stop_cmd = 0, halt_cmd = 0;
            RETURN_IF_FAIL("EcatPIEnable", EcatPIEnable(ecat_dev));

            while (true)
            {
                if (EcatPIInputQueuePop(ecat_dev, false, 100) == 0)
                {
                    loops++;

                    // 显示设置状态
                    if (loops % 1000 == 0)
                    {
                        RETURN_IF_FAIL("EcatPIInputQueueSize", EcatPIOutputQueueSize(ecat_dev, ref ecat_queue_size), false);
                        Console.WriteLine("EcatPIInputQueue size: {0}", ecat_queue_size);
                    }

                    RETURN_IF_FAIL("EcatPIRead", EcatPIRead(ecat_dev, PI_AREA_LOCAL_INPUT, 0, ptInfo.u32PIInputByteSize, ref inputs[0]), false);

                    // 获取输入数据
                    pi.CopyFromInputs(inputs);
                    for (int i = 0; i < NUM_SERVOS; i++) __servos[i].input = pi.servos[i];
                    __user_controller.UpdateButton(pi.inputs);

                    // 启动电机
                    __user_controller.run(ref run_cmd, ref stop_cmd, ref halt_cmd);
                    if (run_cmd != 0)
                    {
                        for (int i = 0; i < NUM_SERVOS; i++) __servos[i].motor_run(i, loops, __range_min[i], __range_max[i], __step_size[i]);
                    }

                    // 生成输出数据
                    for (int i = 0; i < NUM_SERVOS; i++) po.servos[i] = __servos[i].output;
                    __user_controller.UpdateLED(ref po.outputs);
                    po.CopyToOutputs(ref outputs);

                    RETURN_IF_FAIL("EcatPIRead", EcatPIWrite(ecat_dev, PI_AREA_LOCAL_OUTPUT, 0, ptInfo.u32PIOutputByteSize, ref outputs[0]), false);
                    RETURN_IF_FAIL("EcatPIOutputQueuePush", EcatPIOutputQueuePush(ecat_dev, false, 100), false);
                }
            }
        }

        static void Main(string[] args)
        {
            IntPtr ecat_dev = new IntPtr();
            UInt32 channel = 0, alias = 0;
            string eni_file;

            if (args.Length != 3)
            {
                Console.WriteLine("usage: sync_control_test alias channel eni_file");
                Environment.Exit(1);
            }

            alias = Convert.ToUInt32(args[0]);
            channel = Convert.ToUInt32(args[1]);
            eni_file = args[2];

            //PCIe-2E 只有一个通道,PCIe-4E有两个通道
            EXIT_IF_FAIL("EcatOpen", EcatOpen(ref ecat_dev, BOARD_ALIAS(alias), channel));

            try
            {
                ServosTest(ecat_dev, eni_file);
            }
            catch (Exception ex)
            {
                Console.WriteLine("{0}", ex);
            }

            EXIT_IF_FAIL("EcatClose", EcatClose(ecat_dev));
        }
    }
}
