using System;
using System.Collections.Generic;
using System.Linq;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Markup;
using System.Windows.Media;
using System.Windows.Shapes;
using System.Xml;

namespace RoboforkApp
{
    public class DesignerCanvas : Canvas
    {
        const double RADIUS_NODE = 25d; //8d;
        const double RADIUS_CURVER_LINE = 130d;
        const double DISTANCE_AUTO_NODES = 100d;
        const double DISTANCE_START_NODES = 30d;
        const double DISTANCE_END_NODES = 30d;
        const double DISTANCE_FREE_NODES = 40d;
        const double STROKE_ROOT_LINE = 6d;
        const double STROKE_LINE = 4d;
        const double STROKE_NODE = 1d;
        const double DISTANCE_RATIO = 100;

        const double UCNODE_SETLEFT = 17;
        const double UCNODE_SETTOP = 17;
        const double UCNODE_WIDTH = 34;
        const double UCNODE_HEIGHT = 34;

        private List<Rectangle> shapeList = new List<Rectangle>();
        private TextBlock shapePosIndicator;
        private TextBlock shapeSizeIndicator;
        private int currentLine;
        private ucStartEndButton _startPoint;
        private ucNode _ucNodeFree;
        private ucStartEndButton _goalPoint;
        private ucDisplayCoordinate _displayAxiPosition;

        // Add variable for draw route
        public Path pLine = new Path();
        public Path pRootLine = new Path();
        public Path pCurverLine = new Path();
        public Path pRedNode = new Path();
        public Path pYellowNode = new Path();
        public GeometryGroup gGrpLine = new GeometryGroup();
        public GeometryGroup gGrpRootLine = new GeometryGroup();
        public GeometryGroup gGrpCurverLine = new GeometryGroup();
        public GeometryGroup gGrpRedNode = new GeometryGroup();
        public GeometryGroup gGrpYellowNode = new GeometryGroup();
        public int currentShape;
        public static bool isStartDrawRoute = false;
        public static bool isGoalDrawRoute = false;

        public struct NodeInfo
        {
            public double X;
            public double Y;
            public String Mode1;
            public String Mode2;
            public String Mode3;
        }
        List<NodeInfo> NodeInfo_List = new List<NodeInfo>();

        // Add variable for Set Schedule
        public Path pScheduleNode = new Path();
        public Path pScheduleLine = new Path();
        public GeometryGroup gGrpScheduleNode = new GeometryGroup();
        public GeometryGroup gGrpScheduleLine = new GeometryGroup();


        public static bool EditNodeFlag = false;
        public static bool DragDeltaFlag = false;

        //2017/03/04 NAM ADD START
        public static bool isDrawingNode = false;
        public static bool isDragNode = false;
        public Path pNewLine = new Path();
        public GeometryGroup gGrpNewLine = new GeometryGroup();

        public struct NewNodeInfo
        {
            public double X;
            public double Y;
            public String Mode;
        }
        List<NewNodeInfo> NewNodeInfo_List = new List<NewNodeInfo>();
        List<TextBlock> NodeNo = new List<TextBlock>();
        List<ucNode> ucNode_Lst = new List<ucNode>();
        public List<ucNode> ucScheduleNode_Lst = new List<ucNode>();
        int stt = 1;
        //2017/03/04 NAM ADD END


        public ScheduleCanvas scheduleCanvas;


        // Add variable for Set Auto Nodes
        public Path pBlueNode = new Path();
        private GeometryGroup gGrpBlueNode = new GeometryGroup();

        // Add variable for Set Free Nodes
        public Path pFreeNode = new Path();
        //private GeometryGroup gGrpFreeNode = new GeometryGroup();

        // The part of the rectangle the mouse is over.
        private enum HitType
        {
            None, Body, UL, UR, LR, LL, L, R, T, B
        };
        public enum OperationState
        {
            None, DrawObstract, DrawRoute, DrawSetFreeNode, EditNode, NewDrawSetFreeNode
        };
        public enum MouseState
        {
            None, Draw, Drag,
        }
        public OperationState Operation = OperationState.None;
        public MouseState mouseState = MouseState.None;

        // The draw start point.
        private Point StartDrawPoint;

        // The drag's last point.
        private Point LastPoint;

        // The part of the rectangle under the mouse.
        HitType MouseHitType = HitType.None;

        public void Init() {
            if (shapePosIndicator == null)
            {
                shapePosIndicator = new TextBlock()
                {
                    Foreground = Brushes.Black,
                    Background = Brushes.Transparent,
                    FontSize = 20,
                };
            }
            if (shapeSizeIndicator == null)
            {
                shapeSizeIndicator = new TextBlock()
                {
                    Foreground = Brushes.Black,
                    Background = Brushes.Transparent,
                    FontSize = 20,
                };
            }
        }

        // Return a HitType value to indicate what is at the point.
        private HitType SetHitType(Point point)
        {
            if (shapeList.Count == 0)
            {
                currentShape = 0;
                return HitType.None;
            }
            for (int i = 0; i < shapeList.Count; i++)
            {
                Rectangle rect = shapeList[i];
                double left = Canvas.GetLeft(rect);
                double top = Canvas.GetTop(rect);
                double right = left + rect.Width;
                double bottom = top + rect.Height;
                if (point.X < left) continue;
                if (point.X > right) continue;
                if (point.Y < top) continue;
                if (point.Y > bottom) continue;
                currentShape = i;

                const double GAP = 10;
                if (point.X - left < GAP)
                {
                    // Left edge.
                    if (point.Y - top < GAP) return HitType.UL;
                    if (bottom - point.Y < GAP) return HitType.LL;
                    return HitType.L;
                }
                if (right - point.X < GAP)
                {
                    // Right edge.
                    if (point.Y - top < GAP) return HitType.UR;
                    if (bottom - point.Y < GAP) return HitType.LR;
                    return HitType.R;
                }
                if (point.Y - top < GAP) return HitType.T;
                if (bottom - point.Y < GAP) return HitType.B;
                return HitType.Body;
            }
            currentShape = 0;
            return HitType.None;
        }

        // Set a mouse cursor appropriate for the current hit type.
        private void SetMouseCursor()
        {
            // See what cursor we should display.
            Cursor desired_cursor = Cursors.Arrow;
            switch (MouseHitType)
            {
                case HitType.None:
                    desired_cursor = Cursors.Arrow;
                    break;
                case HitType.Body:
                    desired_cursor = Cursors.ScrollAll;
                    break;
                case HitType.UL:
                case HitType.LR:
                    desired_cursor = Cursors.SizeNWSE;
                    break;
                case HitType.LL:
                case HitType.UR:
                    desired_cursor = Cursors.SizeNESW;
                    break;
                case HitType.T:
                case HitType.B:
                    desired_cursor = Cursors.SizeNS;
                    break;
                case HitType.L:
                case HitType.R:
                    desired_cursor = Cursors.SizeWE;
                    break;
            }

            // Display the desired cursor.
            if (Cursor != desired_cursor) Cursor = desired_cursor;
        }

        // constance
        int indicatorAligment = 2;

        /*
        private Point? dragStartPoint = null;
        
        */
        public IEnumerable<DesignerItem> SelectedItems
        {
            get
            {
                var selectedItems = from item in this.Children.OfType<DesignerItem>()
                                    where item.IsSelected == true
                                    select item;

                return selectedItems;
            }
        }

        public void DeselectAll()
        {
            /*
            foreach (DesignerItem item in this.SelectedItems)
            {
                item.IsSelected = false;
            }*/
        }

        protected override void OnMouseDown(MouseButtonEventArgs e)
        {
            base.OnMouseDown(e);

            MouseHitType = SetHitType(Mouse.GetPosition(this));
            SetMouseCursor();

            if (Operation == OperationState.DrawRoute && isStartDrawRoute)
            {
                if (isGoalDrawRoute)
                {
                    return;
                }

                // Check state draw
                if (MouseHitType == HitType.None)
                {
                    if (gGrpLine.Children.Count == 1)
                    {
                        LineGeometry lineGeometry = (LineGeometry)gGrpLine.Children[0];
                        lineGeometry.EndPoint = LastPoint;

                        // Check end route
                        if (IsEndRoute(_goalPoint, lineGeometry))
                        {
                            isGoalDrawRoute = true;
                            ProcessEndRoute();
                            return;
                        }
                    }
                    else if (IsCurverNode((LineGeometry)gGrpLine.Children[currentLine - 1]
                                        , (LineGeometry)gGrpLine.Children[currentLine]))
                    {
                        // Set end point to finish draw line
                        LineGeometry lineGeometry = (LineGeometry)gGrpLine.Children[currentLine];
                        lineGeometry.EndPoint = LastPoint;

                        // Add node to curver postion
                        AddNode(lineGeometry.StartPoint, gGrpRedNode);

                        // Check end route
                        if (IsEndRoute(_goalPoint, lineGeometry))
                        {
                            isGoalDrawRoute = true;
                            ProcessEndRoute();
                            return;
                        }
                    }
                    else
                    {
                        // Remove current line
                        gGrpLine.Children.RemoveAt(currentLine);

                        // Set end point to finish draw line
                        LineGeometry lineGeometry = (LineGeometry)gGrpLine.Children[currentLine - 1];
                        lineGeometry.EndPoint = LastPoint;

                        // Check end route
                        if (IsEndRoute(_goalPoint, lineGeometry))
                        {
                            isGoalDrawRoute = true;
                            ProcessEndRoute();
                            return;
                        }
                    }

                    // Draw new line
                    DrawLine(LastPoint, LastPoint, gGrpLine);
                    // Setting start point for new line
                    StartDrawPoint = LastPoint;

                    mouseState = MouseState.Draw;
                    currentLine = gGrpLine.Children.Count - 1;
                    return;
                }
            }
            else if (Operation == OperationState.DrawSetFreeNode)
            {
                bool RightClick = false;
                if (IsStopDrawRoute(e))
                    RightClick = true;

                StartDrawPoint = e.MouseDevice.GetPosition(this);

                SetFreeNodes(StartDrawPoint, RightClick);
            }
            ////2017/03/04 NAM EDT START
            //else if (Operation == OperationState.NewDrawSetFreeNode)
            //{
            //    bool RightClick = false;
            //    bool LeftClick = false;
            //    if (IsStopDrawRoute(e))
            //        RightClick = true;

            //    StartDrawPoint = e.MouseDevice.GetPosition(this);
            //    CreateNode(StartDrawPoint,LeftClick, RightClick);
            //    //NewSetFreeNodes(StartDrawPoint, RightClick);
            //}
            ////2017/03/04 NAM EDT END

            else if (Operation == OperationState.EditNode)
            {
                Point node_edited = e.MouseDevice.GetPosition(this);

                // start Edit Node Infor
                EditNode(node_edited);

            }
            else if (Operation == OperationState.DrawObstract)
            {
                if (MouseHitType == HitType.None)
                {
                    Rectangle shape = new Rectangle();
                    shape.Width = 1;
                    shape.Height = 1;
                    // Create a SolidColorBrush and use it to
                    // paint the rectangle.
                    shape.Stroke = Brushes.Blue;
                    shape.StrokeThickness = 1;
                    shape.Fill = new SolidColorBrush(Colors.LightCyan);
                    StartDrawPoint = e.MouseDevice.GetPosition(this);
                    shape.SetValue(Canvas.LeftProperty, StartDrawPoint.X);
                    shape.SetValue(Canvas.TopProperty, StartDrawPoint.Y);
                    this.Children.Add(shape);
                    shapeList.Add(shape);

                    mouseState = MouseState.Draw;
                    currentShape = shapeList.Count() - 1;

                    double shapeX = Canvas.GetLeft(shapeList[currentShape]);
                    double shapeY = Canvas.GetTop(shapeList[currentShape]);
                    shapePosIndicator.Text = "(" + Math.Round(shapeX, 0) + "," + Math.Round(shapeY, 0) + ")";
                    shapePosIndicator.SetValue(Canvas.LeftProperty, shapeX + indicatorAligment);
                    shapePosIndicator.SetValue(Canvas.TopProperty, shapeY + indicatorAligment);

                    double width = (int)shapeList[currentShape].Width;
                    double height = (int)shapeList[currentShape].Height;
                    shapeSizeIndicator.Text = "(" + Math.Round(width, 0) + "," + Math.Round(height, 0) + ")";
                    shapeSizeIndicator.SetValue(Canvas.LeftProperty, shapeX + indicatorAligment + width);
                    shapeSizeIndicator.SetValue(Canvas.TopProperty, shapeY + indicatorAligment + height);

                    this.Children.Add(shapePosIndicator);
                    this.Children.Add(shapeSizeIndicator);


                    return;
                }
                else
                {
                    if (shapeList.Count() != 0)
                        shapeList[currentShape].Fill = new SolidColorBrush(Colors.LightCyan);

                    double shapeX = Canvas.GetLeft(shapeList[currentShape]);
                    double shapeY = Canvas.GetTop(shapeList[currentShape]);
                    shapePosIndicator.Text = "(" + Math.Round(shapeX, 0) + "," + Math.Round(shapeY, 0) + ")";
                    shapePosIndicator.SetValue(Canvas.LeftProperty, shapeX + indicatorAligment);
                    shapePosIndicator.SetValue(Canvas.TopProperty, shapeY + indicatorAligment);

                    double width = (int)shapeList[currentShape].Width;
                    double height = (int)shapeList[currentShape].Height;
                    shapeSizeIndicator.Text = "(" + Math.Round(width, 0) + "," + Math.Round(height, 0) + ")";
                    shapeSizeIndicator.SetValue(Canvas.LeftProperty, shapeX + indicatorAligment + width);
                    shapeSizeIndicator.SetValue(Canvas.TopProperty, shapeY + indicatorAligment + height);

                    this.Children.Add(shapePosIndicator);
                    this.Children.Add(shapeSizeIndicator);
                }

                LastPoint = Mouse.GetPosition(this);
                mouseState = MouseState.Drag;
            }
            e.Handled = true;
        }

        protected override void OnMouseMove(MouseEventArgs e)
        {
            base.OnMouseMove(e);

            if (mouseState == MouseState.None)
            {
                MouseHitType = SetHitType(Mouse.GetPosition(this));
                SetMouseCursor();
            }
            else if (Operation == OperationState.DrawRoute && isStartDrawRoute)
            {
                LineGeometry lineGeometry = (LineGeometry)gGrpLine.Children[currentLine];

                // See how much the mouse has moved.
                Point point = Mouse.GetPosition(this);
                double offset_x = point.X - StartDrawPoint.X;
                double offset_y = point.Y - StartDrawPoint.Y;

                // Get the line's current position.
                double new_x = lineGeometry.StartPoint.X;
                double new_y = lineGeometry.StartPoint.Y;

                if (offset_x != 0 || offset_y != 0)
                {
                    if (Math.Abs(offset_x) >= Math.Abs(offset_y))
                    {
                        new_x = point.X;
                    }
                    else
                    {
                        new_y = point.Y;
                    }
                }

                // Set end point of current line
                LastPoint = new Point(new_x, new_y);
                lineGeometry.EndPoint = LastPoint;
                DisplayCoordinate(LastPoint);
            }
            else if (Operation == OperationState.DrawObstract)
            {
                if (mouseState == MouseState.Drag)
                {
                    // See how much the mouse has moved.
                    Point point = Mouse.GetPosition(this);
                    double offset_x = point.X - LastPoint.X;
                    double offset_y = point.Y - LastPoint.Y;

                    // Get the rectangle's current position.
                    double new_x = Canvas.GetLeft(shapeList[currentShape]);
                    double new_y = Canvas.GetTop(shapeList[currentShape]);
                    double new_width = shapeList[currentShape].Width;
                    double new_height = shapeList[currentShape].Height;

                    // Update the rectangle.
                    switch (MouseHitType)
                    {
                        case HitType.Body:
                            new_x += offset_x;
                            new_y += offset_y;
                            break;
                        case HitType.UL:
                            new_x += offset_x;
                            new_y += offset_y;
                            new_width -= offset_x;
                            new_height -= offset_y;
                            break;
                        case HitType.UR:
                            new_y += offset_y;
                            new_width += offset_x;
                            new_height -= offset_y;
                            break;
                        case HitType.LR:
                            new_width += offset_x;
                            new_height += offset_y;
                            break;
                        case HitType.LL:
                            new_x += offset_x;
                            new_width -= offset_x;
                            new_height += offset_y;
                            break;
                        case HitType.L:
                            new_x += offset_x;
                            new_width -= offset_x;
                            break;
                        case HitType.R:
                            new_width += offset_x;
                            break;
                        case HitType.B:
                            new_height += offset_y;
                            break;
                        case HitType.T:
                            new_y += offset_y;
                            new_height -= offset_y;
                            break;
                    }
                    // Don't use negative width or height.
                    if ((new_width > 0) && (new_height > 0))
                    {
                        // Update the rectangle.
                        Canvas.SetLeft(shapeList[currentShape], new_x);
                        Canvas.SetTop(shapeList[currentShape], new_y);
                        shapeList[currentShape].Width = new_width;
                        shapeList[currentShape].Height = new_height;

                        // Save the mouse's new location.
                        LastPoint = point;

                    }
                }
                else if (mouseState == MouseState.Draw)
                {

                    // See how much the mouse has moved.
                    Point point = Mouse.GetPosition(this);


                    double offset_x = point.X - StartDrawPoint.X;
                    double offset_y = point.Y - StartDrawPoint.Y;

                    // Get the rectangle's current position.
                    double start_x = Canvas.GetLeft(shapeList[currentShape]);
                    double start_y = Canvas.GetTop(shapeList[currentShape]);
                    double new_x = Canvas.GetLeft(shapeList[currentShape]);
                    double new_y = Canvas.GetTop(shapeList[currentShape]);
                    double new_width = offset_x;
                    double new_height = offset_y;
                    if (offset_x < 0)
                    {
                        new_x = point.X;
                        new_width = -offset_x;
                    }
                    if (offset_y < 0)
                    {
                        new_y = point.Y;
                        new_height = -offset_y;
                    }
                    Canvas.SetLeft(shapeList[currentShape], new_x);
                    Canvas.SetTop(shapeList[currentShape], new_y);
                    shapeList[currentShape].Width = new_width;
                    shapeList[currentShape].Height = new_height;

                }

                double shapeX = Canvas.GetLeft(shapeList[currentShape]);
                double shapeY = Canvas.GetTop(shapeList[currentShape]);
                shapePosIndicator.Text = "(" + Math.Round(shapeX, 0) + "," + Math.Round(shapeY, 0) + ")";
                shapePosIndicator.SetValue(Canvas.LeftProperty, shapeX + indicatorAligment);
                shapePosIndicator.SetValue(Canvas.TopProperty, shapeY + indicatorAligment);

                double width = (int)shapeList[currentShape].Width;
                double height = (int)shapeList[currentShape].Height;
                shapeSizeIndicator.Text = "(" + Math.Round(width, 0) + "," + Math.Round(height, 0) + ")";
                shapeSizeIndicator.SetValue(Canvas.LeftProperty, shapeX + indicatorAligment + width);
                shapeSizeIndicator.SetValue(Canvas.TopProperty, shapeY + indicatorAligment + height);
            }
            e.Handled = true;
        }

        protected override void OnMouseUp(MouseButtonEventArgs e)
        {
            base.OnMouseUp(e);
            if (Operation == OperationState.DrawObstract)
            {
                if (shapeList.Count() != 0)
                    shapeList[currentShape].Fill = new SolidColorBrush(Colors.Blue);
                shapePosIndicator.Text = "";
                shapeSizeIndicator.Text = "";
                this.Children.Remove(shapePosIndicator);
                this.Children.Remove(shapeSizeIndicator);

                mouseState = MouseState.None;
                currentShape = 0;
            }
            e.Handled = true;
        }

        protected override void OnPreviewMouseUp(MouseButtonEventArgs e)
        {
            base.OnPreviewMouseUp(e);

            EllipseGeometry ellipseGeometry;
            bool flag = false;

            StartDrawPoint = e.MouseDevice.GetPosition(this);
            
            if (EditNodeFlag == true )
            {
                execEditNode(StartDrawPoint);
                EditNodeFlag = false;
                return;
            }

            if (DragDeltaFlag == true)
            {
                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {

                    for (int j = 0; j < ucNode_Lst.Count; j++)
                    {
                        if (j == i)
                        {
                            _ucNodeFree = ucNode_Lst[j];
                            flag = true;
                        }
                    }
                    if (flag)
                    {
                        if (gGrpNewLine.Children.Count > 0)
                        {
                            for (int k = 0; k < gGrpNewLine.Children.Count; k++)
                            {
                                LineGeometry lineGeometry = (LineGeometry)gGrpNewLine.Children[k];
                                Point p1 = lineGeometry.StartPoint;
                                Point p2 = lineGeometry.EndPoint;

                                //bool pInL = PointInLine(StartDrawPoint, p1, p2, 25);

                                //if (pInL)
                                //{

                                //    //this.Children.Remove(_ucNodeFree);
                                //    //this.Children.Add(_ucNodeFree);
                                //    return;
                                //}
                            }
                        }

                        ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];

                        ellipseGeometry.Center = new Point(StartDrawPoint.X, StartDrawPoint.Y);

                        double centerY = Canvas.GetTop(_ucNodeFree);
                        double centerX = Canvas.GetLeft(_ucNodeFree);

                        mouseState = MouseState.Draw;


                        StartDrawPoint = new Point(centerX + UCNODE_SETLEFT, centerY + UCNODE_SETTOP);
                        ellipseGeometry.Center = new Point(centerX + UCNODE_SETLEFT, centerY + UCNODE_SETTOP);

                        this.Children.Remove(_ucNodeFree);
                        this.Children.Add(_ucNodeFree);
                    }
                }

                ReDrawAllNode();

                SetScheduleRoute();
                DragDeltaFlag = false;
            }

            

            return;
        }

        /// <summary>
        /// On Preview Mouse Down
        /// </summary>
        /// <param name="e"></param>
        protected override void OnPreviewMouseDown(MouseButtonEventArgs e)
        {
            base.OnPreviewMouseDown(e);
            if (Operation != OperationState.NewDrawSetFreeNode)
            {
                return;
            }

            Point currentPoint = e.MouseDevice.GetPosition(this);
            //bool _isStart = IsStartEndRoute(_startPoint, currentPoint);

            if (isDrawingNode == false)
            {
                //double centerY = Canvas.GetTop(_startPoint);
                //double centerX = Canvas.GetLeft(_startPoint);

                isDrawingNode = true;
                InitDrawRoute();
                //DrawLine(new Point(centerX + 25, centerY + 25), new Point(centerX + 25, centerY + 25), gGrpLine);
                mouseState = MouseState.Draw;
                //currentLine = gGrpLine.Children.Count - 1;
                //StartDrawPoint = new Point(centerX + 25, centerY + 25);

                //this.Children.Remove(_startPoint);
                //this.Children.Add(_startPoint);


                //test
                bool RightClick = false;
                bool LeftClick = false;


                if (IsMouseLeftClick(e))
                    LeftClick = false;
                if (IsStopDrawRoute(e))
                    RightClick = true;

                StartDrawPoint = e.MouseDevice.GetPosition(this);
                //CreateNode(StartDrawPoint, LeftClick, RightClick);
                execCreateNode(StartDrawPoint);
                //test


                return;
            }
            //2017/03/04 NAM EDT START
            else
            {

                bool RightClick = false;
                bool LeftClick = false;


                if (IsMouseLeftClick(e))
                    LeftClick = true;
                if (IsStopDrawRoute(e))
                    RightClick = true;

                StartDrawPoint = e.MouseDevice.GetPosition(this);

                //Nam can sua
                //if (RightClick)
                //{
                //    execCreateNode(StartDrawPoint);
                //}
                CreateNode(StartDrawPoint, LeftClick, RightClick);
            }
            //2017/03/04 NAM EDT END


            bool _isgoal = IsStartEndRoute(_goalPoint, LastPoint);
            if (_isgoal && isGoalDrawRoute == false)
            {
                isGoalDrawRoute = true;
                ProcessEndRoute();
            }
        }

        #region Functions for draw route

        /// <summary>
        /// Check start or end draw route
        /// </summary>
        /// <param name="_ucStartEndButton">Button start</param>
        /// <param name="currentPoint">Position for check</param>
        /// <returns>true: start, false: not start</returns>
        private bool IsStartEndRoute(ucStartEndButton _ucStartEndButton, Point currentPoint)
        {
            if(_ucStartEndButton == null)
            {
                return false;
            }

            double centerX = Canvas.GetLeft(_ucStartEndButton);
            double centerY = Canvas.GetTop(_ucStartEndButton);

            if (currentPoint.X < centerX + 50 && currentPoint.X > centerX
                && currentPoint.Y < centerY + 50 && currentPoint.Y > centerY)
            {
                return true;
            }
            return false;
        }

        /// <summary>
        /// Process when end draw route
        /// </summary>
        private void ProcessEndRoute()
        {
            Operation = OperationState.None;
            AutoEditLine();
            this.Children.Remove(_displayAxiPosition);
            this.Children.Remove(_goalPoint);
            this.Children.Add(_goalPoint);
        }

        /// <summary>
        /// Check end draw route
        /// </summary>
        /// <param name="_ucStartEndButton">Button end</param>
        /// <param name="currentPoint">Position for check</param>
        /// <returns>true: end, false: not end</returns>
        private bool IsEndRoute(ucStartEndButton _ucStartEndButton, LineGeometry lineGeometry)
        {
            if (_ucStartEndButton == null)
            {
                return false;
            }

            double centerX = Canvas.GetLeft(_ucStartEndButton);
            double centerY = Canvas.GetTop(_ucStartEndButton);
            Point startPoint = lineGeometry.StartPoint;
            Point endPoint = lineGeometry.EndPoint;

            if(IsVerticalLine(lineGeometry))
            {
                if(endPoint.X < centerX || endPoint.X > centerX + 50)
                    return false;

                if (startPoint.Y > centerY + 50 && endPoint.Y > centerY + 50)
                    return false;

                if (startPoint.Y < centerY && endPoint.Y < centerY)
                    return false;
            }else
            {
                if (endPoint.Y < centerY || endPoint.Y > centerY + 50)
                    return false;

                if (startPoint.X > centerX + 50 && endPoint.X > centerX + 50)
                    return false;

                if (startPoint.X < centerX && endPoint.X < centerX)
                    return false;
            }

            return true;
        }

        /// <summary>
        /// Make root
        /// </summary>
        public void MakeRoot()
        {
            //LineGeometry lineGeometry;
            //EllipseGeometry ellipseGeometry;
            //Point startPoint;
            //Point endPoint;

            // If still not route
            if (gGrpLine.Children.Count == 0)
                return;

            pLine.Stroke = new SolidColorBrush(Colors.Red);
            pLine.StrokeThickness = STROKE_ROOT_LINE;

            //// Setting for path line
            //pRootLine.Stroke = new SolidColorBrush(Colors.Red);
            //pRootLine.StrokeThickness = STROKE_ROOT_LINE;
            //pRootLine.Data = gGrpRootLine;
            //this.Children.Add(pRootLine);

            // Setting for path curver line
            //pCurverLine.StrokeThickness = STROKE_ROOT_LINE;

            //// Get start point
            //lineGeometry = (LineGeometry)gGrpLine.Children[0];
            //startPoint = lineGeometry.StartPoint;

            //for (int i = 0; i < gGrpYellowNode.Children.Count; i = i + 2)
            //{
            //    ellipseGeometry = (EllipseGeometry)gGrpYellowNode.Children[i];
            //    endPoint = ellipseGeometry.Center;

            //    DrawLine(startPoint, endPoint, gGrpRootLine);

            //    ellipseGeometry = (EllipseGeometry)gGrpYellowNode.Children[i + 1];
            //    startPoint = ellipseGeometry.Center;
            //}

            //lineGeometry = (LineGeometry)gGrpLine.Children[gGrpLine.Children.Count - 1];
            //endPoint = lineGeometry.EndPoint;

            //DrawLine(startPoint, endPoint, gGrpRootLine);

            //this.Children.Remove(pYellowNode);
            //this.Children.Remove(pBlueNode);
            //this.Children.Remove(_startPoint);
            //this.Children.Remove(_goalPoint);
            //this.Children.Add(pYellowNode);
            //this.Children.Add(pBlueNode);
            //this.Children.Add(_startPoint);
            //this.Children.Add(_goalPoint);
        }

        /// <summary>
        /// Auto edit leght of line
        /// </summary>
        private void AutoEditLine()
        {
            double temp;
            int index = gGrpLine.Children.Count - 1;
            double centerY = Canvas.GetTop(_goalPoint) + 25;
            double centerX = Canvas.GetLeft(_goalPoint) + 25;
            LineGeometry lastLine = (LineGeometry)gGrpLine.Children[index];
            LineGeometry beforeLastLine = (LineGeometry)gGrpLine.Children[index];

            if(gGrpLine.Children.Count > 1)
            {
                beforeLastLine = (LineGeometry)gGrpLine.Children[index - 1];

                if (!IsCurverNode(beforeLastLine, lastLine))
                {
                    beforeLastLine.EndPoint = lastLine.EndPoint;
                    gGrpLine.Children.RemoveAt(index);
                    //// Remove yellow node
                    //if (gGrpYellowNode.Children.Count > 0)
                    //{
                    //    gGrpYellowNode.Children.RemoveAt(gGrpYellowNode.Children.Count - 1);
                    //    gGrpYellowNode.Children.RemoveAt(gGrpYellowNode.Children.Count - 1);
                    //}
                    //// Remove curver line
                    //if (gGrpCurverLine.Children.Count > 0)
                    //{
                    //    gGrpCurverLine.Children.RemoveAt(gGrpCurverLine.Children.Count - 1);
                    //}
                    index = index - 1;
                    lastLine = (LineGeometry)gGrpLine.Children[index];
                }
            }

            if (index == gGrpRedNode.Children.Count + 1)
            {
                AddNode(lastLine.StartPoint, gGrpRedNode);
            }

            if (lastLine.EndPoint.X == centerX && lastLine.EndPoint.Y == centerY)
                return;

            if(IsVerticalLine(lastLine))
            {
                temp = lastLine.StartPoint.Y;
                lastLine.StartPoint = new Point(centerX, temp);
                lastLine.EndPoint = new Point(centerX, centerY);

                if(gGrpLine.Children.Count > 1){
                    beforeLastLine = (LineGeometry)gGrpLine.Children[index - 1];
                    temp = beforeLastLine.EndPoint.Y;
                    beforeLastLine.EndPoint = new Point(centerX, temp);
                }
            }else
            {
                temp = lastLine.StartPoint.X;
                lastLine.StartPoint = new Point(temp, centerY);
                lastLine.EndPoint = new Point(centerX, centerY);
                if (gGrpLine.Children.Count > 1)
                {
                    beforeLastLine = (LineGeometry)gGrpLine.Children[index - 1];
                    temp = beforeLastLine.EndPoint.X;
                    beforeLastLine.EndPoint = new Point(temp, centerY);
                }
            }

            // Draw curver line
            if (IsCurverNode(beforeLastLine, lastLine))
            {
                EllipseGeometry ellipseGeometry = (EllipseGeometry)gGrpRedNode.Children[gGrpRedNode.Children.Count - 1];
                ellipseGeometry.Center = lastLine.StartPoint;
                //
                //if (GetDistance(lastLine.StartPoint, lastLine.EndPoint) > RADIUS_CURVER_LINE + 25)
                //{
                //DrawCurver(beforeLastLine, lastLine);
                //}
            }
        }

        /// <summary>
        /// Check draw curver node
        /// </summary>
        /// <param name="oldLine">Old line</param>
        /// <param name="newLine">New line</param>
        /// <returns>true:is curver, fasle: not curver</returns>
        private bool IsCurverNode(LineGeometry oldLine, LineGeometry newLine)
        {
            if (oldLine.StartPoint.Y == oldLine.EndPoint.Y && oldLine.StartPoint.Y == newLine.EndPoint.Y)
            {
                return false;
            }

            if (oldLine.StartPoint.X == oldLine.EndPoint.X && oldLine.StartPoint.X == newLine.EndPoint.X)
            {
                return false;
            }

            return true;
        }

        /// <summary>
        /// Check timming to stop draw route
        /// </summary>
        /// <param name="e"></param>
        /// <returns>true:stop; false:continue</returns>
        private bool IsStopDrawRoute(MouseEventArgs e)
        {
            if(e.RightButton == MouseButtonState.Pressed)
            {
                return true;
            }

            return false;
        }

        //2017/03/05 NAM ADD START
        private bool IsMouseLeftClick(MouseEventArgs e)
        {
            if (e.LeftButton == MouseButtonState.Pressed)
            {
                return true;
            }

            return false;
        }
        //2017/03/05 NAM ADD END

        /// <summary>
        /// Draw curver line and yellow node
        /// </summary>
        /// <param name="oldLine">Old line</param>
        /// <param name="newLine">New line</param>
        private void DrawCurver(LineGeometry oldLine, LineGeometry newLine)
        {
            double radius = RADIUS_CURVER_LINE;

            Point startPoint ;
            Point endPoint ;

            // Get postion of yellow node on old line
            if(IsVerticalLine(oldLine))
            {
                if (oldLine.StartPoint.Y > oldLine.EndPoint.Y)
                    startPoint = new Point(oldLine.EndPoint.X, oldLine.EndPoint.Y + radius);
                else
                    startPoint = new Point(oldLine.EndPoint.X, oldLine.EndPoint.Y - radius);
            }else
            {
                if (oldLine.StartPoint.X > oldLine.EndPoint.X)
                    startPoint = new Point(oldLine.EndPoint.X + radius, oldLine.EndPoint.Y);
                else
                    startPoint = new Point(oldLine.EndPoint.X - radius, oldLine.EndPoint.Y);
            }

            // Get postion of yellow node on new line
            if (IsVerticalLine(newLine))
            {
                if (newLine.StartPoint.Y > newLine.EndPoint.Y)
                    endPoint = new Point(newLine.StartPoint.X, newLine.StartPoint.Y - radius);
                else
                    endPoint = new Point(newLine.StartPoint.X, newLine.StartPoint.Y + radius);
            }
            else
            {
                if (newLine.StartPoint.X > newLine.EndPoint.X)
                    endPoint = new Point(newLine.StartPoint.X - radius, newLine.StartPoint.Y);
                else
                    endPoint = new Point(newLine.StartPoint.X + radius, newLine.StartPoint.Y);
            }

            //// Setting sweep direction
            //SweepDirection sweepDirection = SweepDirection.Clockwise;
            //if (IsVerticalLine(oldLine) && ((startPoint.Y - endPoint.Y) * (startPoint.X - endPoint.X)) > 0)
            //{
            //    sweepDirection = SweepDirection.Counterclockwise;
            //}

            //if (!IsVerticalLine(oldLine) && ((startPoint.Y - endPoint.Y) * (startPoint.X - endPoint.X)) < 0)
            //{
            //    sweepDirection = SweepDirection.Counterclockwise;
            //}
            
            //// Add curver line
            //DrawCurverLine(startPoint, endPoint, sweepDirection, gGrpCurverLine);

            // Add node to postion distance 1300mm
            if (GetDistance(oldLine.StartPoint, oldLine.EndPoint) > RADIUS_CURVER_LINE)
            {
                AddNode(startPoint, gGrpYellowNode);
            }

            if (GetDistance(newLine.StartPoint, newLine.EndPoint) > RADIUS_CURVER_LINE)
            {
                AddNode(endPoint, gGrpYellowNode);
            }
        }

        /// <summary>
        /// Init data for draw route
        /// </summary>
        private void InitDrawRoute()
        {
            //2017/03/04 NAM ADD START
            pNewLine.Stroke = new SolidColorBrush(Colors.Red);
            pNewLine.StrokeThickness = STROKE_LINE;
            pNewLine.Data = gGrpNewLine;
            //2017/03/04 NAM ADD END

            pScheduleLine.Stroke = new SolidColorBrush(Colors.Red);
            pScheduleLine.StrokeThickness = STROKE_LINE;
            pScheduleLine.Data = gGrpScheduleLine;

            // Setting for path line
            pLine.Stroke = new SolidColorBrush(Colors.Blue);
            pLine.StrokeThickness = STROKE_LINE;
            pLine.Data = gGrpLine;

            // Setting for path of curver line
            pCurverLine.Stroke = Brushes.Red;
            pCurverLine.StrokeThickness = STROKE_LINE;
            pCurverLine.Data = gGrpCurverLine;

            // Setting for path of red node
            pRedNode.Stroke = new SolidColorBrush(Colors.Blue);
            pRedNode.Fill = new SolidColorBrush(Colors.Red);
            pRedNode.StrokeThickness = STROKE_NODE;
            pRedNode.Data = gGrpRedNode;

            // Setting for path of yellow node
            pYellowNode.Stroke = new SolidColorBrush(Colors.Blue);
            pYellowNode.Fill = new SolidColorBrush(Colors.Yellow);
            pYellowNode.StrokeThickness = STROKE_NODE;
            pYellowNode.Data = gGrpYellowNode;

            // Setting for path of Blue node
            pBlueNode.Stroke = new SolidColorBrush(Colors.Blue);
            pBlueNode.Fill = new SolidColorBrush(Colors.LightBlue);
            pBlueNode.StrokeThickness = STROKE_NODE;
            pBlueNode.Data = gGrpBlueNode;

            // Add paths to canvas
            this.Children.Add(pLine);
            this.Children.Add(pCurverLine);
            this.Children.Add(pRedNode);
            this.Children.Add(pYellowNode);

            this.Children.Add(pNewLine);
            scheduleCanvas.Children.Add(pScheduleLine);
        }

        /// <summary>
        /// Clear all route
        /// </summary>
        public void ClearRoute()
        {
            isStartDrawRoute = false;
            isGoalDrawRoute = false;

            gGrpLine.Children.Clear();
            gGrpRootLine.Children.Clear();
            gGrpCurverLine.Children.Clear();
            gGrpRedNode.Children.Clear();
            gGrpYellowNode.Children.Clear();
            gGrpBlueNode.Children.Clear();

            this.Children.Remove(pLine);
            this.Children.Remove(pRootLine);
            this.Children.Remove(pCurverLine);
            this.Children.Remove(pRedNode);
            this.Children.Remove(pYellowNode);
            this.Children.Remove(pBlueNode);
        }

        /// <summary>
        /// Draw line for route
        /// </summary>
        /// <param name="startPoint">Start point</param>
        /// <param name="endPoint">End point</param>
        /// <param name="geometryGroup">Geometry Group</param>
        private void DrawLine(Point startPoint, Point endPoint, GeometryGroup geometryGroup)
        {
            LineGeometry lineGeometry = new LineGeometry();
            lineGeometry.StartPoint = startPoint;
            lineGeometry.EndPoint = endPoint;
            geometryGroup.Children.Add(lineGeometry);
        }

        /// <summary>
        /// Draw curver line
        /// </summary>
        /// <param name="startPoint">Point start curver line</param>
        /// <param name="endPoint">Point end curver line</param>
        /// <param name="radius">Radius</param>
        /// <param name="geometryGroup">Geometry Group</param>
        private void DrawCurverLine(Point startPoint, Point endPoint, SweepDirection sweepDirection, GeometryGroup geometryGroup)
        {
            PathGeometry pathGeometry = new PathGeometry();
            PathFigure figure = new PathFigure();
            figure.StartPoint = startPoint;
            figure.Segments.Add(new ArcSegment(endPoint, new Size(RADIUS_CURVER_LINE, RADIUS_CURVER_LINE), 90, false, sweepDirection, true));
            pathGeometry.Figures.Add(figure);
            geometryGroup.Children.Add(pathGeometry);
        }

        /// <summary>
        /// Setting node
        /// </summary>
        /// <param name="centerPoit">Position of center node</param>
        /// <param name="geometryGroup">Geometry Group</param>
        private void AddNode(Point centerPoit, GeometryGroup geometryGroup)
        {
            double radius = RADIUS_NODE;
            geometryGroup.Children.Add(new EllipseGeometry(centerPoit, radius, radius));
        }

        /// <summary>
        /// Check line is vertical or horizontal
        /// </summary>
        /// <param name="line"></param>
        /// <returns>true:Vertical, false:Horizontal</returns>
        private bool IsVerticalLine(LineGeometry line)
        {
            if (line.StartPoint.X == line.EndPoint.X)
            {
                // Vertical line
                return true;
            }

            // Horizontal line
            return false;
        }

        /// <summary>
        /// Get distance between two point
        /// </summary>
        /// <param name="point1">Point 1</param>
        /// <param name="point2">Point 2</param>
        /// <returns>Distance between two point</returns>
        private double GetDistance(Point point1, Point point2)
        {
            //pythagorean theorem c^2 = a^2 + b^2
            //thus c = square root(a^2 + b^2)
            double a = (double)(point2.X - point1.X);
            double b = (double)(point2.Y - point1.Y);

            return Math.Sqrt(a * a + b * b);
        }

        /// <summary>
        /// Check point is valid for draw
        /// </summary>
        /// <param name="point">Poit need check</param>
        /// <returns>true:Valid, false:Invalid</returns>
        private bool IsValidPoint(Point point)
        {
            return true;
        }

        /// <summary>
        /// Display coordinate position
        /// </summary>
        /// <param name="point">Position to display</param>
        private void DisplayCoordinate(Point point)
        {
            if (_displayAxiPosition == null)
            {
                _displayAxiPosition = new ucDisplayCoordinate();
                this.Children.Add(_displayAxiPosition);
            }
            _displayAxiPosition.Display(point);
        }

        #endregion

        #region Functions for Set Auto Nodes

        /// <summary>
        /// SetAutoNodes
        /// </summary>
        public void SetAutoNodes()
        {
            double radiusStart = DISTANCE_START_NODES;
            double radiusEnd = DISTANCE_END_NODES;
            double radiusCurver = RADIUS_CURVER_LINE;

            Point startNode;
            Point endNode;

            gGrpBlueNode.Children.Clear();

            if (gGrpLine.Children.Count == 1)
                radiusCurver = radiusEnd;

            if (gGrpLine.Children.Count > 0)
            {
                for (int i = 0; i < gGrpLine.Children.Count; i++)
                {
                    LineGeometry lineGeometry = (LineGeometry)gGrpLine.Children[i];
                    if (i == 0)
                    {
                        startNode = lineGeometry.EndPoint;
                        endNode = lineGeometry.StartPoint;
                        DrawAutoNodes(startNode, endNode, radiusCurver, radiusStart);
                    }
                    else if (i == gGrpLine.Children.Count - 1 && i > 0)
                    {
                        startNode = lineGeometry.StartPoint;
                        endNode = lineGeometry.EndPoint;
                        DrawAutoNodes(startNode, endNode, radiusCurver, radiusEnd);
                    }
                    else
                    {
                        startNode = lineGeometry.StartPoint;
                        endNode = lineGeometry.EndPoint;
                        DrawAutoNodes(startNode, endNode, radiusCurver, radiusCurver);
                    }
                }
            }
        }

        
        /// <summary>
        /// DrawAutoNodes
        /// </summary>
        /// <param name="startNode"></param>
        /// <param name="endNode"></param>
        /// <param name="radiusStart"></param>
        /// <param name="radiusEnd"></param>
        private void DrawAutoNodes(Point startNode, Point endNode, double radiusStart, double radiusEnd)
        {
            double distance = DISTANCE_AUTO_NODES;
            double i;

            Point node;

            // Get postion of blue node on line
            if (startNode.X == endNode.X)
            {
                if (startNode.Y > endNode.Y)
                {
                    i = startNode.Y - radiusStart;
                    if (i - distance > endNode.Y + radiusEnd)
                    {
                        do
                        {
                            i = i - distance;
                            node = new Point(endNode.X, i);
                            // Add node to postion distance 1000mm
                            AddNode(node, gGrpBlueNode);

                        } while (i > endNode.Y + radiusEnd + distance);
                    }
                }
                else
                {
                    i = startNode.Y + radiusStart;
                    if (i + distance < endNode.Y - radiusEnd)
                    {
                        do
                        {
                            i = i + distance;
                            node = new Point(endNode.X, i);
                            // Add node to postion distance 1000mm
                            AddNode(node, gGrpBlueNode);
                        } while (i < endNode.Y - radiusEnd - distance);
                    }
                }
            }
            else
            {
                if (startNode.X > endNode.X)
                {
                    i = startNode.X - radiusStart;
                    if (i - distance > endNode.X + radiusEnd)
                    {
                        do
                        {
                            i = i - distance;
                            node = new Point(i, endNode.Y);
                            // Add node to postion distance 1000mm
                            AddNode(node, gGrpBlueNode);
                        } while (i > endNode.X + radiusEnd + distance);
                    }
                }
                else
                {
                    i = startNode.X + radiusStart;
                    if (i + distance < endNode.X - radiusEnd)
                    {
                        do
                        {
                            i = i + distance;
                            node = new Point(i, endNode.Y);
                            // Add node to postion distance 1000mm
                            AddNode(node, gGrpBlueNode);
                        } while (i < endNode.X - radiusEnd - distance);
                    }
                }
            }

        }

        #endregion

        #region Functions for Set Free Nodes
        /// <summary>
        /// Draw Auto Blue node
        /// </summary>
        
        public void SetFreeNodes(Point FreeNode, bool RightClick)
        {
            double radiusStart = DISTANCE_START_NODES;
            double radiusEnd = DISTANCE_END_NODES;
            double radiusNode = RADIUS_NODE;
            double radiusCurver = RADIUS_CURVER_LINE + RADIUS_NODE;

            EllipseGeometry ellipseGeometry;
            Point node;
            bool deleteFlag = false;
            

            if (RightClick)
            {
                if (gGrpBlueNode.Children.Count > 0)
                {
                    for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                    {
                        ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                        node = ellipseGeometry.Center;
                        if (FreeNode.X == node.X)
                        {
                            if (CheckIsNode(FreeNode, node, radiusNode))
                            {
                                deleteFlag = true;
                            }
                        }
                        else
                        {
                            if (CheckIsNode(FreeNode, node, radiusNode))
                            {
                                deleteFlag = true;
                            }
                        }
                        if (deleteFlag)
                        {
                            MessageBoxResult result = MessageBox.Show("Do You Delete This Node?", "Delete Node", MessageBoxButton.OKCancel);
                            if (result == MessageBoxResult.OK)
                            {
                                gGrpBlueNode.Children.RemoveAt(i);

                                this.Children.Remove(NodeNo[i]);
                                return;
                            }
                            else
                            {
                                return;
                            }
                        }
                    }
                }
            }
            else
            {

                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {
                    ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                    node = ellipseGeometry.Center;
                    bool isEditNode = CheckIsNode(FreeNode, node, RADIUS_NODE);

                    if (isEditNode)
                    {
                        EditNode(node);
                        return;
                    }
                }


                MessageBoxResult result = MessageBox.Show("Do You Want To Add This Node?", "Add Node", MessageBoxButton.OKCancel);
                if (result == MessageBoxResult.OK)
                {
                    AddNode(FreeNode, gGrpBlueNode);

                    TextBlock textBlock = new TextBlock();
                    textBlock.Text = stt.ToString();
                    textBlock.FontSize = 17;
                    textBlock.Foreground = new SolidColorBrush(Colors.Red);
                    Canvas.SetLeft(textBlock, FreeNode.X - RADIUS_NODE / 2);
                    Canvas.SetTop(textBlock, FreeNode.Y - 1.5 * RADIUS_NODE);

                    this.Children.Add(textBlock);
                    NodeNo.Add(textBlock);

                    if (stt > 1)
                    {
                        int tmp = gGrpBlueNode.Children.Count;

                        EllipseGeometry elip = (EllipseGeometry)gGrpBlueNode.Children[tmp - 2];
                        Point node1 = elip.Center;
                        elip = (EllipseGeometry)gGrpBlueNode.Children[tmp - 1];
                        Point node2 = elip.Center;
                        DrawLine(node1, node2, gGrpCurverLine);
                    }


                    stt++;
                }
            }

            if (gGrpLine.Children.Count > 0)
            {
                for (int i = 0; i < gGrpLine.Children.Count; i++)
                {
                    LineGeometry lineGeometry = (LineGeometry)gGrpLine.Children[i];

                    // Get postion of  node on line
                    if (IsVerticalLine(lineGeometry))
                    {
                        if (FreeNode.X < lineGeometry.EndPoint.X + radiusNode && FreeNode.X > lineGeometry.EndPoint.X - radiusNode)
                        {
                            FreeNode.X = lineGeometry.EndPoint.X;

                            if (i == 0)
                            {
                                if (CheckFreeNodes(lineGeometry, FreeNode, radiusStart, radiusCurver))
                                {

                                    AddNode(FreeNode, gGrpBlueNode);
                                }
                            }
                            else if (i == gGrpLine.Children.Count - 1 && i > 0)
                            {
                                if (CheckFreeNodes(lineGeometry, FreeNode, radiusCurver, radiusEnd))
                                {

                                    AddNode(FreeNode, gGrpBlueNode);
                                }
                            }
                            else
                            {
                                if (CheckFreeNodes(lineGeometry, FreeNode, radiusCurver, radiusCurver))
                                {

                                    AddNode(FreeNode, gGrpBlueNode);
                                }
                            }

                        }
                    }
                    else
                    {
                        if (FreeNode.Y < lineGeometry.EndPoint.Y + radiusNode && FreeNode.Y > lineGeometry.EndPoint.Y - radiusNode)
                        {
                            FreeNode.Y = lineGeometry.EndPoint.Y;
                            if (i == 0)
                            {
                                if (CheckFreeNodes(lineGeometry, FreeNode, radiusStart, radiusCurver))
                                {

                                    AddNode(FreeNode, gGrpBlueNode);
                                }
                            }
                            else if (i == gGrpLine.Children.Count - 1 && i > 0)
                            {
                                if (CheckFreeNodes(lineGeometry, FreeNode, radiusCurver, radiusEnd))
                                {

                                    AddNode(FreeNode, gGrpBlueNode);
                                }
                            }
                            else
                            {
                                if (CheckFreeNodes(lineGeometry, FreeNode, radiusCurver, radiusCurver))
                                {

                                    AddNode(FreeNode, gGrpBlueNode);
                                }
                            }

                        }
                    }
                }
            }
        }


        public bool CheckFreeNodes(LineGeometry lineNode, Point Freenode, double radiusStart, double radiusEnd)
        {

            double distanceFreeNode = DISTANCE_FREE_NODES;
            double radiusNode = RADIUS_NODE;
            EllipseGeometry ellipseGeometry;
            Point node;

            if (IsVerticalLine(lineNode))
            {
                if (lineNode.StartPoint.Y < lineNode.EndPoint.Y)
                {
                    if (Freenode.Y < lineNode.StartPoint.Y + radiusStart || Freenode.Y > lineNode.EndPoint.Y - radiusEnd)
                    {
                        return false;
                    }
                }
                else
                {
                    if (Freenode.Y > lineNode.StartPoint.Y - radiusStart || Freenode.Y < lineNode.EndPoint.Y + radiusEnd)
                    {
                        return false;
                    }
                }
            }
            else
            {
                if (lineNode.StartPoint.X < lineNode.EndPoint.X)
                {
                    if (Freenode.X < lineNode.StartPoint.X + radiusStart || Freenode.X > lineNode.EndPoint.X - radiusEnd)
                    {
                        return false;
                    }
                }
                else
                {
                    if (Freenode.X > lineNode.StartPoint.X - radiusStart || Freenode.X < lineNode.EndPoint.X + radiusEnd)
                    {
                        return false;
                    }
                }
            }

            if (gGrpBlueNode.Children.Count > 0)
            {
                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {
                    ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                    node = ellipseGeometry.Center;
                    if (Freenode.X == node.X)
                    {
                        if (CheckIsNode(Freenode, node, radiusNode))
                        {
                            return false;
                        }
                        else if (Freenode.Y < node.Y + distanceFreeNode && Freenode.Y > node.Y - distanceFreeNode)
                        {
                            return false;
                        }
                    }
                    else if (Freenode.Y == node.Y)
                    {
                        if (CheckIsNode(Freenode, node, radiusNode))
                        {
                            return false;
                        }
                        else if (Freenode.X < node.X + distanceFreeNode && Freenode.X > node.X - distanceFreeNode)
                        {
                            return false;
                        }
                    }
                }
            }

            return true;
        }

        public bool CheckIsNode(Point FreeNode, Point Node, double radiusNode)
        {
            if (FreeNode.X < Node.X + radiusNode && FreeNode.X > Node.X - radiusNode && FreeNode.Y < Node.Y + radiusNode && FreeNode.Y > Node.Y - radiusNode)
            {
                return true;
            }
            return false;
        }

        #endregion

        #region Edit Node
        public void InitNodeInfo_List() 
        {
            //Reset List
            NodeInfo_List = new List<NodeInfo>();

            if (gGrpBlueNode.Children.Count > 0) 
            {
                // Get start point
                LineGeometry lineGeometry = (LineGeometry)gGrpNewLine.Children[0];
                Point startPoint = lineGeometry.StartPoint;

                NodeInfo n1 = new NodeInfo();
                n1.X = startPoint.X;
                n1.Y = startPoint.Y;
                n1.Mode1 = "";
                n1.Mode2 = "";
                n1.Mode3 = "";
                NodeInfo_List.Add(n1);

                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {
                    Point point;
                    EllipseGeometry eGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                    point = eGeometry.Center;
                    
                    NodeInfo Ninfo = new NodeInfo();
                    Ninfo.X = point.X;
                    Ninfo.Y = point.Y;

                    Ninfo.Mode1 = "";
                    Ninfo.Mode2 = "";
                    Ninfo.Mode3 = "";

                    NodeInfo_List.Add(Ninfo);
                }

                // Get end point
                lineGeometry = (LineGeometry)gGrpLine.Children[gGrpLine.Children.Count -1];
                Point endPoint = lineGeometry.EndPoint;
                
                NodeInfo n2 = new NodeInfo();
                n2.X = startPoint.X;
                n2.Y = startPoint.Y;
                n2.Mode1 = "";
                n2.Mode2 = "";
                n2.Mode3 = "";
                NodeInfo_List.Add(n2);

                //Resort NodeInfo_List From Start to Goal
                LineGeometry  firstLine = (LineGeometry)gGrpLine.Children[0];
                Point startNode = firstLine.StartPoint;
                Point endNode = firstLine.EndPoint;


                List<NodeInfo> tmpLst = new List<NodeInfo>();

                // Create temp List
                if(startNode.X == endNode.X)
                {
                    for (int i = 1; i < NodeInfo_List.Count; i++)
                    {
                        if (NodeInfo_List[i].X != endNode.X)
                        {
                            break;
                        }
                        else
                        {
                            tmpLst.Add(NodeInfo_List[i]);
                        }
                    }
                }

                if (startNode.Y == endNode.Y)
                {
                    for (int i = 1; i < NodeInfo_List.Count; i++)
                    {
                        if (NodeInfo_List[i].Y != endNode.Y)
                        {
                            break;
                        }
                        else
                        {
                            tmpLst.Add(NodeInfo_List[i]);
                        }
                    }
                }

                // Sort NodeInfo_List
                for (int i = 0; i < tmpLst.Count; i++) 
                {
                    NodeInfo_List[i + 1] = tmpLst[tmpLst.Count -1 - i];
                }
              
            }
        }

                
        public void EditNode(Point node_edited)
        {
            EllipseGeometry ellipseGeometry;
            Point node;
            double radiusNode = RADIUS_NODE;

            bool flag = false;
            
            if (gGrpBlueNode.Children.Count > 0)
            {
                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {
                    ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                    node = ellipseGeometry.Center;
                    
                    if (CheckIsNode(node_edited, node, radiusNode))
                    {
                        flag = true;
                    }

                    if (flag)
                    {
                        node_edited.X = node.X;
                        node_edited.Y = node.Y;

                        // show form edit node
                        EditNodeWindow edtNodeWindow = new EditNodeWindow();
                        edtNodeWindow.ShowDialog();

                        string result1 = edtNodeWindow._txtMode1;
                        string result2 = edtNodeWindow._txtMode2;
                        string result3 = edtNodeWindow._txtMode3;
                        bool exit = edtNodeWindow._ExitFlg;

                        if (!exit) {
                            SaveChanged(node_edited.X, node_edited.Y, result1, result2, result3);
                        }
                        
                        return;
                    }
                }
            }
        }

        public void SaveChanged(double x, double y, string st1, string st2, string st3)
        {
            for (int i = 0; i < NodeInfo_List.Count; i++) 
            {
                NodeInfo ni = new NodeInfo();
                ni = NodeInfo_List[i];

                if (ni.X == x && ni.Y == y)
                {
                    
                    ni.Mode1 = st1;
                    ni.Mode2 = st2;
                    ni.Mode3 = st3;

                    NodeInfo_List[i] = ni;
                    return;
                }
            }
        }

        
        #endregion

        #region Display RouteInfo

        public void DspRouteInfo()
        {
            //Clear Route Info Table
            ((RoboforkMenu)System.Windows.Application.Current.MainWindow).grdRouteInfo.Children.Clear();

            if (NodeInfo_List.Count != 0)
            {
                int _RowIdx = 0;
                string _Content = "";

                for (int i = 0; i < NodeInfo_List.Count; i++)
                {

                    NodeInfo ni = new NodeInfo();
                    ni = NodeInfo_List[i];

                    //column 1
                    if (i == 0)
                    {
                        _Content = "S";
                    }
                    else if (i == NodeInfo_List.Count - 1)
                    {
                        _Content = "G";
                    }
                    else
                    {
                        _Content = i.ToString();
                    }
                    AddLabeltoGrid(_RowIdx, 0, _Content);

                    //column 2
                    // Display Node's Position
                    _Content = ni.X + ", " + ni.Y;
                    AddLabeltoGrid(_RowIdx, 1, _Content);

                    // Display Node's Field
                    if (ni.Mode1 != "" && ni.Mode1 != null)
                    {
                        char delimiterChars = '_';
                        string[] tmp = ni.Mode1.Split(delimiterChars);
                        foreach (string s in tmp)
                        {
                            _RowIdx++;
                            delimiterChars = ':';

                            if (s.Split(delimiterChars)[0] == "Mode")
                            {
                                double distance = 0;
                                if (i == NodeInfo_List.Count - 1)
                                {
                                    distance = 0;
                                }
                                else
                                {
                                    distance = DistanceCalculate(NodeInfo_List[i].X, NodeInfo_List[i].Y, NodeInfo_List[i + 1].X, NodeInfo_List[i + 1].Y);
                                }
                                _Content = "MOVE " + distance.ToString() + "mm";
                            }
                            else
                            {
                                _Content = s.Split(delimiterChars)[0] + " " + s.Split(delimiterChars)[1];
                            }

                            AddLabeltoGrid(_RowIdx, 1, _Content);
                        }
                    }

                    if (ni.Mode2 != "" && ni.Mode2 != null)
                    {
                        char delimiterChars = '_';
                        string[] tmp = ni.Mode2.Split(delimiterChars);
                        foreach (string s in tmp)
                        {
                            _RowIdx++;
                            delimiterChars = ':';

                            if (s.Split(delimiterChars)[0] == "Mode")
                            {
                                double distance = 0;
                                if (i == NodeInfo_List.Count - 1)
                                {
                                    distance = 0;
                                }
                                else
                                {
                                    distance = DistanceCalculate(NodeInfo_List[i].X, NodeInfo_List[i].Y, NodeInfo_List[i + 1].X, NodeInfo_List[i + 1].Y);
                                }
                                _Content = "MOVE " + distance.ToString() + "mm";
                            }
                            else
                            {
                                _Content = s.Split(delimiterChars)[0] + " " + s.Split(delimiterChars)[1];
                            }

                            AddLabeltoGrid(_RowIdx, 1, _Content);
                        }
                    }

                    if (ni.Mode3 != "" && ni.Mode3 != null)
                    {
                        char delimiterChars = '_';
                        string[] tmp = ni.Mode3.Split(delimiterChars);
                        foreach (string s in tmp)
                        {
                            _RowIdx++;
                            delimiterChars = ':';

                            if (s.Split(delimiterChars)[0] == "Mode")
                            {
                                double distance = 0;
                                if (i == NodeInfo_List.Count - 1)
                                {
                                    distance = 0;
                                }
                                else
                                {
                                    distance = DistanceCalculate(NodeInfo_List[i].X, NodeInfo_List[i].Y, NodeInfo_List[i + 1].X, NodeInfo_List[i + 1].Y);
                                }
                                _Content = "MOVE " + distance.ToString() + "mm";
                            }
                            else
                            {
                                _Content = s.Split(delimiterChars)[0] + " " + s.Split(delimiterChars)[1];
                            }

                            AddLabeltoGrid(_RowIdx, 1, _Content);
                        }
                    }
                    _RowIdx++;
                }
            }
        }

        public double DistanceCalculate(double _X1, double _Y1, double _X2, double _Y2)
        {
            double dist = 0;

            if (_X1 == _X2)
            {
                dist = System.Math.Abs(_Y1 - _Y2) * DISTANCE_RATIO;
            }
            else if (_Y1 == _Y2)
            {
                dist = System.Math.Abs(_X1 - _X2) * DISTANCE_RATIO;
            }
            return dist;
        }

        public void AddLabeltoGrid(int RowIdx, int ColIdx, string Content)
        {
            //Add Row to Grid
            RowDefinition _rd = new RowDefinition();
            ((RoboforkMenu)System.Windows.Application.Current.MainWindow).grdRouteInfo.RowDefinitions.Add(_rd);
            
            // Add data to Grid
            Label dynamicLabel = new Label();

            dynamicLabel.Content = Content;
            dynamicLabel.Margin = new Thickness(0, 0, 0, 0);
            dynamicLabel.Foreground = new SolidColorBrush(Colors.Black);
            dynamicLabel.Background = new SolidColorBrush(Colors.White);
            dynamicLabel.BorderBrush = new SolidColorBrush(Colors.LightGray);
            dynamicLabel.BorderThickness = new Thickness(1);

            Grid.SetRow(dynamicLabel, RowIdx);
            Grid.SetColumn(dynamicLabel, ColIdx);
            ((RoboforkMenu)System.Windows.Application.Current.MainWindow).grdRouteInfo.Children.Add(dynamicLabel);
        }
        #endregion


        public void CreateGoalPoint()
        {
            if (isGoalDrawRoute)
            {
                return;
            }

            isStartDrawRoute = false;
            if (_goalPoint == null)
            {
                _goalPoint = new ucStartEndButton();
                _goalPoint.btnWidth = 50.0;
                _goalPoint.btnHeight = 50.0;
                _goalPoint.buttText = "G";
                Canvas.SetLeft(_goalPoint, 675);
                Canvas.SetTop(_goalPoint, 75);
                this.Children.Add(_goalPoint);
            }
        }

        public void CreateStartPoint()
        {
            if (isGoalDrawRoute)
            {
                return;
            }

            isStartDrawRoute = false;
            if (_startPoint == null)
            {
                _startPoint = new ucStartEndButton();
                _startPoint.btnWidth = 50.0;
                _startPoint.btnHeight = 50.0;
                _startPoint.buttText = "S";
                Canvas.SetLeft(_startPoint, 75);
                Canvas.SetTop(_startPoint, 675);
                this.Children.Add(_startPoint);
            }
        }


        #region Draw New FreeNode

        /// <summary>
        /// Draw Auto Blue node
        /// </summary>

        
        public void NewSetFreeNodes(Point FreeNode, bool RightClick)
        {
            double radiusNode = RADIUS_NODE;

            EllipseGeometry ellipseGeometry;
            Point node;
            bool deleteFlag = false;


            if (RightClick)
            {
                if (gGrpBlueNode.Children.Count > 0)
                {
                    for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                    {
                        ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                        node = ellipseGeometry.Center;
                        if (FreeNode.X == node.X)
                        {
                            if (CheckIsNode(FreeNode, node, radiusNode))
                            {
                                deleteFlag = true;
                            }
                        }
                        else
                        {
                            if (CheckIsNode(FreeNode, node, radiusNode))
                            {
                                deleteFlag = true;
                            }
                        }
                        if (deleteFlag)
                        {
                            MessageBoxResult result = MessageBox.Show("Do You Delete This Node?", "Delete Node", MessageBoxButton.OKCancel);
                            if (result == MessageBoxResult.OK)
                            {
                                //Call Function Delete Node
                                DeleteNode(i);

                                return;
                            }
                            else
                            {
                                return;
                            }
                        }
                    }
                }
            }
            else
            {
                //Check EditNode State
                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {
                    ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                    node = ellipseGeometry.Center;
                    bool isEditNode = CheckIsNode(FreeNode, node, RADIUS_NODE);

                    if (isEditNode)
                    {
                        NewEditNode(node);
                        NewDspRouteInfo();
                        return;
                    }
                }


                MessageBoxResult result = MessageBox.Show("Do You Want To Add This Node?", "Add Node", MessageBoxButton.OKCancel);
                if (result == MessageBoxResult.OK)
                {
                    AddNode(FreeNode, gGrpBlueNode);

                    TextBlock textBlock = new TextBlock();
                    textBlock.Text = stt.ToString();
                    textBlock.FontSize = 35;
                    textBlock.VerticalAlignment = VerticalAlignment.Center;
                    textBlock.HorizontalAlignment = HorizontalAlignment.Center;
                    textBlock.Foreground = new SolidColorBrush(Colors.Red);

                    Canvas.SetLeft(textBlock, FreeNode.X - RADIUS_NODE / 2);
                    Canvas.SetTop(textBlock, FreeNode.Y - 1.5 * RADIUS_NODE);


                    this.Children.Add(textBlock);
                    NodeNo.Add(textBlock);

                    if (stt > 1)
                    {
                        int tmp = gGrpBlueNode.Children.Count;

                        EllipseGeometry elip = (EllipseGeometry)gGrpBlueNode.Children[tmp - 2];
                        Point node1 = elip.Center;
                        elip = (EllipseGeometry)gGrpBlueNode.Children[tmp - 1];
                        Point node2 = elip.Center;
                        DrawLine(node1, node2, gGrpNewLine);


                        this.Children.Remove(pBlueNode);
                        for (int i = 0; i < tmp; i++)
                        {
                            this.Children.Remove(NodeNo[i]);
                        }

                        this.Children.Add(pBlueNode);
                        for (int i = 0; i < tmp; i++)
                        {
                            this.Children.Add(NodeNo[i]);
                        }
                    }

                    stt++;
                    NewInitNodeInfo_List();
                    NewDspRouteInfo();
                }
            }
        }

        public void DeleteNode(int i)
        {
            //DrawLine(Point startPoint, Point endPoint, GeometryGroup geometryGroup)

            LineGeometry lineGeometry = new LineGeometry();
            EllipseGeometry ellip;

            //check delete last node
            if (i == gGrpBlueNode.Children.Count - 1)
            {
                // delete line
                if (gGrpNewLine.Children.Count > 0)
                {
                    gGrpNewLine.Children.RemoveAt(i - 1);
                }

                // delete ucNode
                ucNode _ucNode = new ucNode();
                _ucNode = ucNode_Lst[i];
                this.Children.Remove(_ucNode);
                ucNode_Lst.RemoveAt(i);

                // delete node
                gGrpBlueNode.Children.RemoveAt(i);
                NewNodeInfo_List.RemoveAt(i);

            }
            else
            {
                //delete all line and remove Point at 'i' 
                gGrpNewLine.Children.Clear();
                this.Children.Remove(pNewLine);
                gGrpBlueNode.Children.RemoveAt(i);

                //redraw line
                for (int j = 0; j < gGrpBlueNode.Children.Count - 1; j++)
                {
                    ellip = (EllipseGeometry)gGrpBlueNode.Children[j];
                    Point node1 = ellip.Center;
                    ellip = (EllipseGeometry)gGrpBlueNode.Children[j + 1];
                    Point node2 = ellip.Center;
                    DrawLine(node1, node2, gGrpNewLine);
                }

                this.Children.Add(pNewLine);

                //remove ucNode
                for (int j = ucNode_Lst.Count - 1; j > i; j--)
                {
                    ucNode_Lst[j].txtNode = ucNode_Lst[j - 1].txtNode;
                }
                this.Children.Remove(ucNode_Lst[i]);
                ucNode_Lst.RemoveAt(i);

                //redraw ucNode
                for (int k = 0; k < ucNode_Lst.Count; k++)
                {
                    this.Children.Remove(ucNode_Lst[k]);
                    this.Children.Add(ucNode_Lst[k]);
                }
            }

            //NewInitNodeInfo_List();

            NewDspRouteInfo();
            stt--;

        }

        public void ReDrawAllNode()
        {
            LineGeometry lineGeometry = new LineGeometry();
            EllipseGeometry ellip;
            //delete all line
            gGrpNewLine.Children.Clear();
            this.Children.Remove(pNewLine);

            //redraw line
            for (int j = 0; j < gGrpBlueNode.Children.Count-1; j++)
            {
                ellip = (EllipseGeometry)gGrpBlueNode.Children[j];
                Point node1 = ellip.Center;
                ellip = (EllipseGeometry)gGrpBlueNode.Children[j+1];
                Point node2 = ellip.Center;
                DrawLine(node1, node2, gGrpNewLine);
            }

            this.Children.Add(pNewLine);

            
            //redraw ucNode
            for (int k = 0; k < ucNode_Lst.Count; k++)
            {
                this.Children.Remove(ucNode_Lst[k]);
                this.Children.Add(ucNode_Lst[k]);
            }
        }



        public void NewInitNodeInfo_List()
        {
            //Reset List
            //NewNodeInfo_List = new List<NewNodeInfo>();

            if (gGrpBlueNode.Children.Count > 0)
            {
                //for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                //{
                    int i = gGrpBlueNode.Children.Count - 1;
                    Point point;
                    EllipseGeometry eGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                    point = eGeometry.Center;

                    NewNodeInfo Ninfo = new NewNodeInfo();
                    Ninfo.X = point.X;
                    Ninfo.Y = point.Y;

                    Ninfo.Mode = "";

                    NewNodeInfo_List.Add(Ninfo);
                //}

            }
        }

        public void NewEditNode(Point node_edited)
        {
            EllipseGeometry ellipseGeometry;
            Point node;
            double radiusNode = RADIUS_NODE;

            bool flag = false;

            if (gGrpBlueNode.Children.Count > 0)
            {
                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {
                    ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                    node = ellipseGeometry.Center;

                    if (CheckIsNode(node_edited, node, radiusNode))
                    {
                        flag = true;
                    }

                    if (flag)
                    {
                        node_edited.X = node.X;
                        node_edited.Y = node.Y;

                        // show form edit node
                        EditNodeWindow edtNodeWindow = new EditNodeWindow();
                        edtNodeWindow.ShowDialog();

                        string result = edtNodeWindow._txtMode;
                        bool exit = edtNodeWindow._ExitFlg;

                        if (!exit)
                        {
                            NewSaveChanged(node_edited.X, node_edited.Y, result);
                        }

                        return;
                    }
                }
            }
        }

        //Save Node's Data Edited
        public void NewSaveChanged(double x, double y, string st)
        {
            for (int i = 0; i < NewNodeInfo_List.Count; i++)
            {
                NewNodeInfo ni = new NewNodeInfo();
                ni = NewNodeInfo_List[i];

                if (ni.X == x && ni.Y == y)
                {
                    ni.Mode = st;

                    NewNodeInfo_List[i] = ni;
                    return;
                }
            }
        }

        public void NewDspRouteInfo()
        {
            //Clear Route Info Table
            ((RoboforkMenu)System.Windows.Application.Current.MainWindow).grdRouteInfo.Children.Clear();

            if (NewNodeInfo_List.Count != 0)
            {
                int _RowIdx = 0;
                string _Content = "";

                for (int i = 0; i < NewNodeInfo_List.Count; i++)
                {

                    NewNodeInfo ni = new NewNodeInfo();
                    ni = NewNodeInfo_List[i];

                    //column 1
                    _Content = (i +1).ToString();
                    AddLabeltoGrid(_RowIdx, 0, _Content);

                    //column 2
                    // Display Node's Position
                    _Content = "LAT " + ni.X;
                    AddLabeltoGrid(_RowIdx, 1, _Content);
                    _RowIdx++;

                    _Content = "LOC " + ni.Y;
                    AddLabeltoGrid(_RowIdx, 1, _Content);

                    // Display Node's Field
                    if (ni.Mode != "" && ni.Mode != null)
                    {
                        char delimiterChars = '_';
                        string[] tmp = ni.Mode.Split(delimiterChars);
                        foreach (string s in tmp)
                        {
                            _RowIdx++;
                            delimiterChars = ':';

                            if (s.Split(delimiterChars)[0] == "Speed")
                            {
                                _Content = "SPD " + s.Split(delimiterChars)[1];
                                AddLabeltoGrid(_RowIdx, 1, _Content);
                            }
                        }
                    }
                    _RowIdx++;
                }
            }
        }



        //2017/03/07 tach rieng CreateNode start
        private void execDeleteNode(Point FreeNode)
        {

            double radiusNode = RADIUS_NODE;

            EllipseGeometry ellipseGeometry;
            Point node;
            bool deleteFlag = false;

            if (gGrpBlueNode.Children.Count > 0)
            {
                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {
                    ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                    node = ellipseGeometry.Center;
                    if (FreeNode.X == node.X)
                    {
                        if (CheckIsNode(FreeNode, node, radiusNode))
                        {
                            deleteFlag = true;
                        }
                    }
                    else
                    {
                        if (CheckIsNode(FreeNode, node, radiusNode))
                        {
                            deleteFlag = true;
                        }
                    }
                    if (deleteFlag)
                    {
                        MessageBoxResult result = MessageBox.Show("Do You Delete This Node?", "Delete Node", MessageBoxButton.OKCancel);
                        if (result == MessageBoxResult.OK)
                        {
                            DeleteNode(i);
                            SetScheduleRoute();

                            return;
                        }
                        else
                        {
                            return;
                        }
                    }
                }
            }
        }


        private void execEditNode(Point FreeNode)
        {
            EllipseGeometry ellipseGeometry;
            Point node;
            for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
            {
                ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                node = ellipseGeometry.Center;
                bool isEditNode = CheckIsNode(FreeNode, node, RADIUS_NODE);

                if (isEditNode)
                {
                    NewEditNode(node);
                    NewDspRouteInfo();
                    return;
                }
            }
        }


        private void execCreateNode(Point FreeNode)
        {
            //check new node in exist line
            if (gGrpNewLine.Children.Count > 0)
            {
                for (int i = 0; i < gGrpNewLine.Children.Count; i++)
                {
                    LineGeometry lineGeometry = (LineGeometry)gGrpNewLine.Children[i];
                    Point p1 = lineGeometry.StartPoint;
                    Point p2 = lineGeometry.EndPoint;

                    bool pInL = PointInLine(FreeNode, p1, p2, UCNODE_SETLEFT);

                    if (pInL)
                    {
                        MessageBoxResult result = MessageBox.Show("Do You Want To Add This Node?", "Add Node", MessageBoxButton.OKCancel);
                        if (result == MessageBoxResult.OK)
                        {
                            Point tmpPoint = ConvertNodeinLine(FreeNode, p1, p2);
                            FreeNode = tmpPoint;
                            
                            ucNode _ucNode1 = new ucNode();
                            _ucNode1.btnWidth = UCNODE_WIDTH;
                            _ucNode1.btnHeight = UCNODE_HEIGHT;
                            _ucNode1.txtNode = (i + 2).ToString();
                            _ucNode1.fillColor = "Brown";
                            _ucNode1.IsDragDelta = true;
                            Canvas.SetLeft(_ucNode1, FreeNode.X - UCNODE_SETLEFT);
                            Canvas.SetTop(_ucNode1, FreeNode.Y - UCNODE_SETTOP);
                            //this.Children.Add(_ucNode1);

                            ucNode_Lst.Insert(i + 1, _ucNode1);
                            //ucNode_Lst.Add(_ucNode1);

                            gGrpBlueNode.Children.Insert(i + 1, new EllipseGeometry(FreeNode, RADIUS_NODE, RADIUS_NODE));
                            //AddNode(FreeNode, gGrpBlueNode);


                            ////delete all line
                            //gGrpNewLine.Children.Clear();
                            //this.Children.Remove(pNewLine);
                            ////redraw line
                            //for (int j = 0; j < gGrpBlueNode.Children.Count - 1; j++)
                            //{
                            //    EllipseGeometry ellip = (EllipseGeometry)gGrpBlueNode.Children[j];
                            //    Point node1 = ellip.Center;
                            //    ellip = (EllipseGeometry)gGrpBlueNode.Children[j + 1];
                            //    Point node2 = ellip.Center;
                            //    DrawLine(node1, node2, gGrpNewLine);
                            //}

                            //this.Children.Add(pNewLine);


                            //rename Point textName
                            for (int j = 0; j < ucNode_Lst.Count; j++)
                            {
                                ucNode_Lst[j].txtNode = (j + 1).ToString();
                                this.Children.Remove(ucNode_Lst[j]);
                                this.Children.Add(ucNode_Lst[j]);
                            }


                            ReDrawAllNode();

                            stt++;
                            NewInitNodeInfo_List();
                            NewDspRouteInfo();
                            return;
                        }
                        else
                        { return; }
                    }
                }
            }


            ucNode _ucNode = new ucNode();
            _ucNode.btnWidth = UCNODE_WIDTH;
            _ucNode.btnHeight = UCNODE_HEIGHT;
            _ucNode.txtNode = stt.ToString();
            _ucNode.IsDragDelta = true;
            Canvas.SetLeft(_ucNode, FreeNode.X - UCNODE_SETLEFT);
            Canvas.SetTop(_ucNode, FreeNode.Y - UCNODE_SETTOP);
            this.Children.Add(_ucNode);
            ucNode_Lst.Add(_ucNode);

            AddNode(FreeNode, gGrpBlueNode);

            SetScheduleRoute();

            //draw line
            if (stt > 1)
            {
                //delete all line
                gGrpNewLine.Children.Clear();
                this.Children.Remove(pNewLine);
                //redraw line
                for (int j = 0; j < gGrpBlueNode.Children.Count - 1; j++)
                {
                    EllipseGeometry ellip = (EllipseGeometry)gGrpBlueNode.Children[j];
                    Point node1 = ellip.Center;
                    ellip = (EllipseGeometry)gGrpBlueNode.Children[j + 1];
                    Point node2 = ellip.Center;
                    DrawLine(node1, node2, gGrpNewLine);
                }

                this.Children.Add(pNewLine);

                //rename Point textName
                for (int j = 0; j < ucNode_Lst.Count; j++)
                {
                    ucNode_Lst[j].txtNode = (j + 1).ToString();
                    this.Children.Remove(ucNode_Lst[j]);
                    this.Children.Add(ucNode_Lst[j]);
                }
            }

            stt++;
            NewInitNodeInfo_List();
            NewDspRouteInfo();
        }

        //2017/03/07 tach CreateNode End






        private void CreateNode(Point FreeNode, bool LeftClick, bool RightClick)
        {
            double radiusNode = RADIUS_NODE;

            EllipseGeometry ellipseGeometry;
            Point node;
            bool deleteFlag = false;

            //Su kien bam chuot phai
            if (RightClick)
            {
                if (gGrpBlueNode.Children.Count > 0)
                {
                    for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                    {
                        ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                        node = ellipseGeometry.Center;
                        if (FreeNode.X == node.X)
                        {
                            if (CheckIsNode(FreeNode, node, radiusNode))
                            {
                                deleteFlag = true;
                            }
                        }
                        else
                        {
                            if (CheckIsNode(FreeNode, node, radiusNode))
                            {
                                deleteFlag = true;
                            }
                        }
                        if (deleteFlag)
                        {
                            MessageBoxResult result = MessageBox.Show("Do You Delete This Node?", "Delete Node", MessageBoxButton.OKCancel);
                            if (result == MessageBoxResult.OK)
                            {

                                DeleteNode(i);

                                SetScheduleRoute();

                                return;
                            }
                            else
                            {
                                return;
                            }
                        }
                    }
                }
            }

            // them nut
            else
            {
                //Check EditNode State
                if(LeftClick)
                { 
                    for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                    {
                        ellipseGeometry = (EllipseGeometry)gGrpBlueNode.Children[i];
                        node = ellipseGeometry.Center;
                        bool isEditNode = CheckIsNode(FreeNode, node, RADIUS_NODE);

                        if (isEditNode)
                        {

                            //NewEditNode(node);
                            //NewDspRouteInfo();
                            return;
                        }
                    }
                }
                
                // starting add node
                //check new node in exist line
                if(gGrpNewLine.Children.Count > 0)
                {
                for (int i = 0; i < gGrpNewLine.Children.Count; i++)
                {
                    LineGeometry lineGeometry = (LineGeometry)gGrpNewLine.Children[i];
                    Point p1 =  lineGeometry.StartPoint;
                    Point p2 = lineGeometry.EndPoint;

                    bool pInL = PointInLine(FreeNode, p1, p2, UCNODE_SETLEFT);
                    
                    if (pInL)
                    {
                        MessageBoxResult result = MessageBox.Show("Do You Want To Add This Node?", "Add Node", MessageBoxButton.OKCancel);
                        if (result == MessageBoxResult.OK)
                        {
                            Point tmpPoint = ConvertNodeinLine(FreeNode, p1, p2);
                            FreeNode = tmpPoint;

                            ucNode _ucNode1 = new ucNode();
                            _ucNode1.btnWidth = UCNODE_WIDTH;
                            _ucNode1.btnHeight = UCNODE_HEIGHT;
                            _ucNode1.txtNode = (i + 2).ToString();
                            _ucNode1.fillColor = "Brown";
                            _ucNode1.IsDragDelta = true;
                            Canvas.SetLeft(_ucNode1, FreeNode.X - UCNODE_SETLEFT);
                            Canvas.SetTop(_ucNode1, FreeNode.Y - UCNODE_SETTOP);
                            //this.Children.Add(_ucNode1);

                            ucNode_Lst.Insert(i + 1, _ucNode1);
                            //ucNode_Lst.Add(_ucNode1);

                            gGrpBlueNode.Children.Insert(i + 1, new EllipseGeometry(FreeNode, RADIUS_NODE, RADIUS_NODE));
                            //AddNode(FreeNode, gGrpBlueNode);


                            //delete all line
                            gGrpNewLine.Children.Clear();
                            this.Children.Remove(pNewLine);
                            //redraw line
                            for (int j = 0; j < gGrpBlueNode.Children.Count - 1; j++)
                            {
                                EllipseGeometry ellip = (EllipseGeometry)gGrpBlueNode.Children[j];
                                Point node1 = ellip.Center;
                                ellip = (EllipseGeometry)gGrpBlueNode.Children[j + 1];
                                Point node2 = ellip.Center;
                                DrawLine(node1, node2, gGrpNewLine);
                            }

                            this.Children.Add(pNewLine);


                            //rename Point textName
                            for (int j = 0; j < ucNode_Lst.Count; j++)
                            {
                                ucNode_Lst[j].txtNode = (j + 1).ToString();
                                this.Children.Remove(ucNode_Lst[j]);
                                this.Children.Add(ucNode_Lst[j]);
                            }
                            stt++;
                            NewInitNodeInfo_List();
                            NewDspRouteInfo();

                            SetScheduleRoute();

                            return;
                        }
                        else
                        { return; }
                    }
                }
                }

                ucNode _ucNode = new ucNode();
                _ucNode.btnWidth = UCNODE_WIDTH;
                _ucNode.btnHeight = UCNODE_HEIGHT;
                _ucNode.txtNode = stt.ToString();
                _ucNode.IsDragDelta = true;
                //_ucNode.selectedColor = System.Windows.Media.Brushes.Black;
                Canvas.SetLeft(_ucNode, FreeNode.X - UCNODE_SETLEFT);
                Canvas.SetTop(_ucNode, FreeNode.Y - UCNODE_SETTOP);
                this.Children.Add(_ucNode);
                ucNode_Lst.Add(_ucNode);

                AddNode(FreeNode, gGrpBlueNode);

                SetScheduleRoute();
                    
                //draw line
                if (stt > 1)
                {
                    //delete all line
                    gGrpNewLine.Children.Clear();
                    this.Children.Remove(pNewLine);
                    //redraw line
                    for (int j = 0; j < gGrpBlueNode.Children.Count - 1; j++)
                    {
                        EllipseGeometry ellip = (EllipseGeometry)gGrpBlueNode.Children[j];
                        Point node1 = ellip.Center;
                        ellip = (EllipseGeometry)gGrpBlueNode.Children[j + 1];
                        Point node2 = ellip.Center;
                        DrawLine(node1, node2, gGrpNewLine);
                    }

                    this.Children.Add(pNewLine);

                    //rename Point textName
                    for (int j = 0; j < ucNode_Lst.Count; j++)
                    {
                        ucNode_Lst[j].txtNode = (j + 1).ToString();
                        this.Children.Remove(ucNode_Lst[j]);
                        this.Children.Add(ucNode_Lst[j]);
                    }

                }

                stt++;
                NewInitNodeInfo_List();
                NewDspRouteInfo();
            }
            
        }

        public Point ConvertNodeinLine(Point point, Point l1, Point l2)
        {
            Point pointResult = new Point();
            double X = 0;
            double Y = 0;

            double distance_Line = 0;
            double distance_l1ToPoint = 0;

            distance_Line = DistanceTo(l1, l2);
            distance_l1ToPoint = DistanceTo(l1, point);


            if (l1.X == l2.X)
            {
                X = l1.X;
                Y = point.Y;
            }
            else if (l1.Y == l2.Y)
            {
                X = point.X;
                Y = l1.Y;
            }
            else
            {
                if (l1.X < l2.X)
                {
                    if (l1.Y < l2.Y)
                    {
                        Y = l1.Y + (distance_l1ToPoint * (l2.Y - l1.Y) / distance_Line);
                        X = l1.X + (distance_l1ToPoint * (l2.X - l1.X) / distance_Line);
                    }
                    else
                    {
                        Y = l1.Y - (distance_l1ToPoint * (l1.Y - l2.Y) / distance_Line);
                        X = l1.X + (distance_l1ToPoint * (l2.X - l1.X) / distance_Line);
                    }
                }
                else
                {
                    if (l1.Y < l2.Y)
                    {
                        Y = l1.Y + (distance_l1ToPoint * (l2.Y - l1.Y) / distance_Line);
                        X = l1.X - (distance_l1ToPoint * (l1.X - l2.X) / distance_Line);
                    }
                    else
                    {
                        Y = l1.Y - (distance_l1ToPoint * (l1.Y - l2.Y) / distance_Line);
                        X = l1.X - (distance_l1ToPoint * (l1.X - l2.X) / distance_Line);
                    }
                }


            }

            pointResult.X = X;
            pointResult.Y = Y;
            return pointResult;
        }

        public static double DistanceTo(Point point1, Point point2)
        {
            var a = (double)(point2.X - point1.X);
            var b = (double)(point2.Y - point1.Y);

            return Math.Sqrt(a *a + b * b);
        }

        public bool PointInLine(Point point, Point l1, Point l2, double radius)
        {
            double distance = 0;
            bool falg = false;

            if (l1.X < l2.X)
            {
                if (l1.Y < l2.Y)
                {
                    if (point.X > l1.X - radius && point.X < l2.X + radius && point.Y > l1.Y - radius && point.Y < l2.Y + radius)
                    {
                        falg = true;
                    }
                }
                else
                {
                    if (point.X > l1.X - radius && point.X < l2.X + radius && point.Y < l1.Y + radius && point.Y > l2.Y - radius)
                    {
                        falg = true;
                    }
                }
            }
            else
            {
                if (l1.Y < l2.Y)
                {
                    if (point.X < l1.X + radius && point.X > l2.X - radius && point.Y > l1.Y - radius && point.Y < l2.Y + radius)
                    {
                        falg = true;
                    }
                }
                else
                {
                    if (point.X < l1.X + radius && point.X > l2.X - radius && point.Y < l1.Y + radius && point.Y > l2.Y - radius)
                    {
                        falg = true;
                    }
                }
            }
            if (falg == false)
            {
                return false;
            }

            distance = DistanceFromPointToLine(point, l1, l2);

            if (distance > radius)
            {
                return false;
            }
            return true;
        }

        public static double DistanceFromPointToLine(Point point, Point l1, Point l2)
        {

            return Math.Abs((l2.X - l1.X)*(l1.Y - point.Y) - (l1.X - point.X)*(l2.Y - l1.Y)) /
                    Math.Sqrt(Math.Pow(l2.X - l1.X, 2) + Math.Pow(l2.Y - l1.Y, 2));
        }

        #endregion

        #region Schedule

        public void SetScheduleRoute()
        {

            EllipseGeometry ellipseGeometry_1;
            EllipseGeometry ellipseGeometry_2;
            Point node_1;
            Point node_2;
            Point node_Schedule = new Point();
            double x_1 = 50;
            double y_1 = 80;
            double Totaldistance = 1270;


            if (ucScheduleNode_Lst.Count > 0)
            {
                for (int i = 0; i < ucScheduleNode_Lst.Count; i++)
                {
                    ucNode _ucScheduleNode = new ucNode();
                    _ucScheduleNode = ucScheduleNode_Lst[i];
                    scheduleCanvas.Children.Remove(_ucScheduleNode);

                }
                ucScheduleNode_Lst.Clear();
            }

            gGrpScheduleNode.Children.Clear();
            gGrpScheduleLine.Children.Clear();


            List<double> distance = new List<double>();
            double addDistance;

            if (gGrpBlueNode.Children.Count > 0)
            {
                for (int i = 0; i < gGrpBlueNode.Children.Count; i++)
                {
                    ellipseGeometry_2 = (EllipseGeometry)gGrpBlueNode.Children[i];
                    node_2 = ellipseGeometry_2.Center;

                    if (i >= 1)
                    {
                        ellipseGeometry_1 = (EllipseGeometry)gGrpBlueNode.Children[i - 1];
                        node_1 = ellipseGeometry_1.Center;
                        if (node_1.X == node_2.X)
                        {
                            addDistance = Math.Abs(node_1.Y - node_2.Y);
                            distance.Add(addDistance);
                        }
                        else if (node_1.Y == node_2.Y)
                        {
                            addDistance = Math.Abs(node_1.X - node_2.X);
                            distance.Add(addDistance);
                        }
                        else
                        {
                            var a = (double)(node_2.X - node_1.X);
                            var b = (double)(node_2.Y - node_1.Y);
                            addDistance = Math.Sqrt(a * a + b * b);
                            distance.Add(addDistance);
                        }
                    }
                }
            }
            if (distance.Count > 0)
            {
                double total = 0;
                double distance_i;

                for (int i = 0; i < distance.Count; i++)
                {
                    total = total + distance[i];
                }

                for (int i = 0; i < distance.Count; i++)
                {
                    distance_i = distance[i] * (Totaldistance / total);
                    distance[i] = distance_i;
                }
            }

            if (gGrpBlueNode.Children.Count > 0)
            {
                node_Schedule.X = x_1;
                node_Schedule.Y = y_1;
                AddNode(node_Schedule, gGrpScheduleNode);
            }

            addDistance = 0;
            for (int i = 0; i < distance.Count; i++)
            {

                node_Schedule.Y = y_1;
                addDistance = addDistance + distance[i];
                node_Schedule.X = addDistance;
                AddNode(node_Schedule, gGrpScheduleNode);
            }

            if (gGrpScheduleNode.Children.Count > 0)
            {
                for (int i = 0; i < gGrpScheduleNode.Children.Count; i++)
                {
                    ellipseGeometry_1 = (EllipseGeometry)gGrpScheduleNode.Children[i];
                    node_1 = ellipseGeometry_1.Center;
                    if (i > 0)
                    {
                        ellipseGeometry_2 = (EllipseGeometry)gGrpScheduleNode.Children[i -1];
                        node_2 = ellipseGeometry_2.Center;
                        DrawLine(node_1, node_2, gGrpScheduleLine);
                    }
                    CreateScheduleNode(node_1, i + 1);
                }
            }
        }

        private void CreateScheduleNode(Point point, int indexNode)
        {
            ucNode _ucNode = new ucNode();
            _ucNode.btnWidth = UCNODE_WIDTH;
            _ucNode.btnHeight = UCNODE_HEIGHT;
            _ucNode.txtNode = indexNode.ToString();
            _ucNode.IsDragDelta = false;
            Canvas.SetLeft(_ucNode, point.X - UCNODE_SETLEFT);
            Canvas.SetTop(_ucNode, point.Y - UCNODE_SETTOP);
            scheduleCanvas.Children.Add(_ucNode);
            ucScheduleNode_Lst.Add(_ucNode);
        }

        #endregion

    }
}