using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace LowestCommonAncestorWithParent
{
    class Program
    {
        static void Main(string[] args)
        {
            var node1 = new Node(1);
            node1.left = new Node(2);
            node1.left.left = new Node(3);
            node1.left.right = new Node(4);
            node1.left.right.right = new Node(5);

            node1.left.parent = node1;
            node1.left.left.parent = node1.left;
            node1.left.right.parent = node1.left;
            node1.left.right.right.parent = node1.left.right;

            var test = new Program();
            var LCA = test.LowestCommonAncestor(node1.left.left, node1.left.right.right);
            Debug.Assert(LCA.val == 2);
        }

        public class Node
        {
            public int val;
            public Node left;
            public Node right;
            public Node parent;

            public Node(int value)
            {
                val = value;
            }
        }
        
        public Node LowestCommonAncestor(Node p, Node q) {
            if(p == null || q == null)
                return null;
        
            var start = p;
            var stepP = 0;
            while(start != null)
            {
                start = start.parent;
                stepP++;
            }        
        
            var stepQ = 0;
            start = q;
            while(start != null)
            {
                start = start.parent;
                stepQ++;
            }
        
            if(stepP <= stepQ)
            {
                // swap p and q
                var tmp = p;
                p = q;
                q = tmp;
            }
        
            // q is close to root always. stepP is bigger
            // advance p first
            start = p;
            var count = Math.Abs(stepQ - stepP);   // negative -> absolute 
            while(count > 0)
            {
                start = start.parent;
                count--;
            }
        
            while( start != null && q != null)
            {
                if(start == q)
                    return start;
            
                start = start.parent;
                q = q.parent;
            }        
        
            return null;
        }
    }
}