9_12_LCA_with_parent_pointers

#include <iostream>
#include <vector>
#include <cmath>

using namespace std;

#include <iostream>
#include <vector>
#include <cmath>

using namespace std;

template <typename T>
class node {
public:
	T data;
	node *left, *right, *parent;
	
	node() {
		left = right = parent = NULL;
	}
};

template <typename T>
class BinaryTree {
public:
	node<T> *root;

	BinaryTree() {
		root = NULL;
	}

	void _insert(T a, node<T> *n) {
		if (root) {
			if (a < n->data) {
				if (n->left)
					_insert(a, n->left);
				else {
					node<T> *leaf = new node<T>();
					leaf->data = a;
                    leaf->parent = n;
					n->left = leaf;
				}
			}
			else {
				if (n->right)
					_insert(a, n->right);
				else {
					node<T> *leaf = new node<T>();
					leaf->data = a;
                    leaf->parent = n;
					n->right = leaf;
				}
			}
		}
		else {
			node<T> *leaf = new node<T>();
			leaf->data = a;
			root = leaf;
		}

	}

	void insert(T a) {
		_insert(a, root);
	}
    
    node<T>* leastCommonAncestor(node<T>* a, node<T>* b) {
        int h1 = 0, h2 = 0;
        node<T> *ancestorA = a->parent, *ancestorB = b->parent;
        while (ancestorA) {
            h1++;
            ancestorA = ancestorA->parent;
        }
        while (ancestorB) {
            h2++;
            ancestorB = ancestorB->parent;
        }
        while (h1 > h2) {
            a = a->parent;
            h1--;
        }
        while (h2 > h1) {
            b = b->parent;
            h2--;
        }
        while (a != b) {
            a = a->parent;
            b = b->parent;
        }
        return a;
    }
    
    void Print (node<T> * x, int & id)
    {
        if (!x) return;

        Print (x->left,id);

        id++;
        cout << id << ' ' << x->data << endl;

        Print (x->right,id);
    }
};

int main() {
    
    BinaryTree<int> B;
    B.insert(10);
    B.insert(5);
    B.insert(13);
    B.insert(16);
    B.insert(11);
    B.insert(7);
    B.insert(3);
    B.insert(6);
    B.insert(8);
    B.insert(9);
    cout << "Find LCA of " << B.root->left->right->left->data << " and " << B.root->left->right->right->right->data << endl;
    node<int>* n = B.leastCommonAncestor(B.root->left->right->left, B.root->left->right->right->right);
    cout << n->data << endl;
	return 0;
}