Hola buenas tengo una duda tengo una cola de prioridad heap donde cada prioridad es unica y tengo que buscar un elemento de la cola donde la prioridad coincida con la buscada

//Busca en cp la prioridad prio y devuelve el nodo asociado a es prioridad
nodo SearchNodeByPriority(unsigned int prio, heap cp){
//cp es un arreglo con punteros a nodos con la prioridad y otros datos
 
    int prio = 1;
    while(cp[prio] -> prioridad < p){
      prio = (cp[prio * 2]->prioridad < cp[prio * 2 + 1]->prioridad) ? (prio * 2) : ((prio * 2) + 1);
    }
 
    if (cp -> arre[prio] == p) {
      return cp[prio];
    }      
    return NULL;
}
 

Se me ocurrio este codigo el problema es que me sirve solamente si el elemento que busco esta en la primera o segunda posicion. Como podria solucionarlo?
Estoy tratando de que esta busquedaq quede en ordel log n siendo n el largo de cp