using System;
using System.Threading;
using System.Collections.Generic;
using ABB.Robotics.Controllers;
using ABB.Robotics.Controllers.Discovery;
using ABB.Robotics.Controllers.IOSystemDomain;
using Robot.Common;


namespace MesWork
{
    public class B_Robot_Abb
    {
        /// <summary>
        /// 
        /// </summary>
        private Controller abb = null;
        /// <summary>
        /// 
        /// </summary>
        public bool OnLine = false;
        /// <summary>
        /// 
        /// </summary>
        public string ip = "-1";
        /// <summary>
        /// 
        /// </summary>
        public string cname = "";
        /// <summary>
        /// 
        /// </summary>
        public int interval = -1;
        /// <summary>
        /// 
        /// </summary>
        public bool isRun = true;
      
        /// <summary>
        /// 
        /// </summary>
        public string Id = "-1";
        /// <summary>
        /// 
        /// </summary>
        public string Name = "";
 
       

        public string robotWorkbench_1 = "";
        public string robotWorkbench_Old_1 = "";
        public string robotWorkbench_2 = "";
        public string robotWorkbench_Old_2 = "";

        public string TagID_Axis_7 = "";



        /// <summary>
        /// 
        /// </summary>
        public double[] j = new double[12];
        /// <summary>
        /// 
        /// </summary>
        public double[] jDemo = new double[12];
        /// <summary>
        /// 
        /// </summary>
        public List<JointConfig> Joints = null;
        /// <summary>
        /// 
        /// </summary>
        public string sendStr_Old = "";

        /// <summary>
        /// 
        /// </summary>
        public B_Robot_Abb()
        {

        }


        /// <summary>
        /// 
        /// </summary>
        /// <param name="ipPort"></param>
        /// <param name="interval"></param>
        /// <param name="j1_6"></param>
        /// <param name="modelManagement"></param>
        /// <param name="modelCode"></param>
        public void Start(string ip, string cname,int interval, int intervalUpdate, string DeviceCode, string OpName, List<JointConfig> Joints
            , string TagID_DoorOpen = "", string TagID_DoorClose = "", string TagID_Axis_7 = "")
        {
            this.ip = ip;
            this.cname = cname;
            this.interval = interval;
            this.Id = DeviceCode;
            this.Name = OpName;
            this.Joints = Joints;
            this.TagID_Axis_7 = TagID_Axis_7;

            var robotWorkbench_1_Addr = TagID_DoorOpen.Split(',')[0];
            var robotWorkbench_1_TagID = TagID_DoorOpen.Split(',')[1];
            var robotWorkbench_2_Addr = TagID_DoorClose.Split(',')[0];
            var robotWorkbench_2_TagID = TagID_DoorClose.Split(',')[1];


            new Thread(new ThreadStart(delegate ()
            {
                while (isRun)
                {
                    try
                    {

                        if (abb == null)
                        {
                            ReConnRobot();
                        }
                        OnLine = abb.Connected;
                        if (OnLine)
                        {
                            j = ReadResult_To_Array();

                            robotWorkbench_1 = ReadIO(robotWorkbench_1_Addr);
                            if (robotWorkbench_Old_1 != robotWorkbench_1) 
                            {
                                MesWorkForm.S_C_B_TagIDTrigger(Name, robotWorkbench_1_TagID, robotWorkbench_1);
                            }
                            robotWorkbench_Old_1 = robotWorkbench_1;

                            robotWorkbench_2 = ReadIO(robotWorkbench_2_Addr);
                            if (robotWorkbench_Old_2 != robotWorkbench_2)
                            {
                                MesWorkForm.S_C_B_TagIDTrigger(Name, robotWorkbench_2_TagID, robotWorkbench_2);
                            }
                            robotWorkbench_Old_2 = robotWorkbench_2;

                        }
                        else
                        {
                            Thread.Sleep(10 * 1000);
                            ReConnRobot();
                            continue;
                        }
                        for (int i = 0; i < Joints.Count ; i++)
                        {
                            Joints[i].DeviceCode = DeviceCode;
                            Joints[i].Value = j[i].ToString();
                        }

                        var sendStr = MesWorkForm.Move_Joint(Joints);
                        if (sendStr_Old != sendStr)
                        {
                            sendStr_Old = sendStr;
                            MesWorkForm.SendMsg(sendStr);
                        }

                    }
                    catch (Exception err) { }
                    Thread.Sleep(interval);
                }
            }))
            { IsBackground = true }.Start();

            new Thread(new ThreadStart(delegate () {
                while (isRun)
                {
                    try
                    {
                        for (int i = 0; i < Joints.Count; i++)
                        {
                            Joints[i].DeviceCode = DeviceCode;
                            Joints[i].Value = j[i].ToString();
                        }

                        var sendStr = MesWorkForm.Move_Joint(Joints);
                        MesWorkForm.SendMsg(sendStr);

                    }
                    catch (Exception err) 
                    { }
                    Thread.Sleep(intervalUpdate);
                }
            }))
            { IsBackground = true }.Start();
        }


        private string ReadIO(string addr)
        {
            var valueStr = "";
           // using (var m = Mastership.Request(abb.Rapid))
            {
                var _d = (DigitalSignal)abb.IOSystem.GetSignal(addr);
                if (_d != null)
                {
                    valueStr = _d.Value.ToString();
                }
            }
            switch (valueStr.ToLower())
            {
                case "0":
                case "false":
                    valueStr = "0";
                    break;
                case "1":
                case "true":
                    valueStr = "1";
                    break;
                default:
                    break;
            }
            return valueStr;
        }

        ~B_Robot_Abb()
        {
            DisConn();
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="ip"></param>
        /// <param name="port"></param>
        public void DisConn()
        {
            isRun = false;
            Thread.Sleep(199);
            abb?.Logoff();

        }


       /// <summary>
       /// 
       /// </summary>
        private void ReConnRobot()
        {
            if (MesWorkForm.IsDemoMesServer == 1)
            {
                //abb = ControllerFactory.CreateFrom();
                //abb.Logon(UserInfo.DefaultUser);
                abb = new Controller();
                return;
            }

            if (abb != null)
            {
                abb?.Logoff();
                abb = null;
            }

            var scanner = new NetworkScanner();
            var controllers = scanner.GetControllers();
            foreach (var controllerInfo in controllers)
            {
                if (controllerInfo.IPAddress.ToString() == ip & controllerInfo.SystemName == cname)
                {
                    abb = ControllerFactory.CreateFrom(controllerInfo);
                    abb.Logon(UserInfo.DefaultUser);
                }
            }

        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="j1_6"></param>
        /// <returns></returns>
        private double[] ReadResult_To_Array()
        {

            if (MesWorkForm.IsDemoMesServer == 1)
            {
                return jDemo;
            }
            float j7 = 0f;
            try
            {
                j7 = Convert.ToSingle(MesWorkForm.ReadPLC(TagID_Axis_7).ToString());

            }
            catch (Exception err )
            {
            }

            var joint = new double[12];
            var currJoint = abb.MotionSystem.ActiveMechanicalUnit.GetPosition();
            joint[0] = currJoint.RobAx.Rax_1;
            joint[1] = currJoint.RobAx.Rax_2;
            joint[2] = currJoint.RobAx.Rax_3;
            joint[3] = currJoint.RobAx.Rax_4;
            joint[4] = currJoint.RobAx.Rax_5;
            joint[5] = currJoint.RobAx.Rax_6;
            joint[6] = j7;

            return joint;
        }


    }
}
