求二叉树的给定两个结点之间的距离

#include <iostream>
#include <vector>
#include <queue>
#include <stack>
#include <string>
#include <algorithm>

using namespace std;

struct TreeNode{
    int val;
    TreeNode* left;
    TreeNode* right;
    TreeNode(int val_ = 0):val(val_),left(NULL),right(NULL){}
};

struct TreeHeightNode{
    int height;
    TreeNode* node;
    TreeHeightNode* parent;
    TreeHeightNode(TreeNode* node_ = NULL,int height_ = 0,TreeHeightNode* parent_ =NULL): node(node_), height(height_),parent(parent_){}
};

int getDistanceBetweenNode(TreeNode* root,TreeNode* a, TreeNode* b){
    if(root == NULL ) return -1;
    queue<TreeHeightNode *> que;
    que.push(new TreeHeightNode(root,0));
    bool flagA = false, flagB = false;
   // int heightA = 0, heightB = 0;
    TreeHeightNode* heightA =NULL, *heightB = NULL;
    while(!que.empty()){
        TreeHeightNode* tmp = que.front(); que.pop();
        TreeNode *node = tmp->node;
        if(node == a) {flagA = true;heightA = tmp;}
        if(node == b) {flagB = true;heightB = tmp;}
        if(flagA && flagB) break;
        if(node->left){
            if(node->val == 0) que.push(new TreeHeightNode(node->left,1,tmp));
            else if(node->val > 0) que.push(new TreeHeightNode(node->left,tmp->height+1,tmp));
            else if(node->val < 0) que.push(new TreeHeightNode(node->left,tmp->height-1,tmp));
        }
        if(node->right){
            if(node->val == 0) que.push(new TreeHeightNode(node->right,-1,tmp));
            else if(node->val > 0) que.push(new TreeHeightNode(node->right,tmp->height+1,tmp));
            else if(node->val < 0) que.push(new TreeHeightNode(node->right,tmp->height-1,tmp));
        }
    }
    
    if(!flagA || !flagB) return -1;
    else{
        int ha = heightA->height, hb =heightB->height;
       if(ha*hb <=0)  return ha-hb;
       else{
           if((ha >= hb && hb > 0) || (ha < hb && hb < 0)){
               int cnt = ha-hb;
               while(cnt-->0){
                   heightA=heightA->parent;
               }
               if(heightA == heightB) return ha-hb;
               else return ha-hb+2;
           }else{
               int cnt = hb - ha;
               while(cnt-- > 0){
                   heightB=heightB->parent;
               }
               if(heightB == heightA) return hb-ha;
               else return hb-ha+2;
           }
       }  
    }
}

int main(){
    TreeNode *root = new TreeNode(1);
    TreeNode *left = new TreeNode(2);
    TreeNode *right = new TreeNode(3);
    root->left = left;
    root->right = right;
    TreeNode *left1 = new TreeNode(4);
    TreeNode *right1 = new TreeNode(5);
    left->left = left1;
    left->right = right1;
    TreeNode *left2 = new TreeNode(4);
    TreeNode *right2 = new TreeNode(5);
    left1->left = left2;
    left1->right = right2;
    cout<<getDistanceBetweenNode(root,left2,right)<<endl;
    return 0;
}