Собрана но пока не ворк верно

#include <iostream>
#include <vector>
#include <deque>

using namespace std;

class Heap
{
    std::vector<int> h;
    std::vector<int> ID_to_HeapIdx;
    int heap_size;
    int curr_ID;

public:
    Heap();
    void siftup(int ID, std::vector<int>& HeapIdx_to_ID);
    void siftdown(int ID, std::vector<int>& HeapIdx_to_ID);
    void add(int vertex, int ID, std::vector<int>& HeapIdx_to_ID);
    void delete_vertex(int ID, std::vector<int>& HeapIdx_to_ID);
    bool isempty();
    void out();
    int get_root();
};

Heap::Heap()
{
    std::vector<int> h;
    heap_size = 0;
    curr_ID = 0;
    std::vector<int> ID_to_HeapIdx;
}

int Heap::get_root()
{
    if (heap_size > 0)
        return h[0];
    return -1;
}

bool Heap::isempty()
{
    if (heap_size == 0)
        return true;
    return false;
}

void Heap::siftup(int ID, std::vector<int>& HeapIdx_to_ID)
{
    int curr, parent, tmp;
    curr = heap_size - 1;
    parent = (curr - 1);
    for (int i = 0; i < HeapIdx_to_ID.size(); i++)

        for (int i = 0; i < ID_to_HeapIdx.size(); i++)

            while (parent >= 0 && curr > 0)
            {

                if (h[parent] < h[curr])
                {

                    int buff = h[curr];
                    h[curr] = h[parent];
                    h[parent] = buff;

                    tmp = HeapIdx_to_ID[parent];
                    HeapIdx_to_ID[parent] = HeapIdx_to_ID[curr];
                    HeapIdx_to_ID[curr] = tmp;


                    tmp = ID_to_HeapIdx[HeapIdx_to_ID[parent]];
                    ID_to_HeapIdx[HeapIdx_to_ID[parent]] = ID_to_HeapIdx[HeapIdx_to_ID[curr]];
                    ID_to_HeapIdx[HeapIdx_to_ID[curr]] = tmp;
                }
                curr = parent;
                parent = (curr - 1);
            }
}

void Heap::add(int vertex, int ID, std::vector<int>& HeapIdx_to_ID)
{

    h.push_back(vertex);
    ID_to_HeapIdx.push_back(heap_size);
    HeapIdx_to_ID.push_back(ID);
    heap_size++;
    siftup(ID, HeapIdx_to_ID);
}

void Heap::siftdown(int ID, std::vector<int>& HeapIdx_to_ID)
{
    int parent, max_child, tmp, buff;

    int curr = ID_to_HeapIdx[ID];
    int child_l = 2 * curr + 1;
    int child_r = 2 * curr + 2;

    if (h[child_r] < h[child_l])
        max_child = child_l;
    else
        max_child = child_r;

    while (child_l < heap_size)
    {
        if (child_l == heap_size - 1)
            max_child = child_l;
        else if (h[child_r] < h[child_l])
            max_child = child_l;
        else
            max_child = child_r;

        if (h[curr] < h[max_child])
        {

            buff = h[curr];
            h[curr] = h[max_child];
            h[max_child] = buff;

            tmp = HeapIdx_to_ID[max_child];
            HeapIdx_to_ID[max_child] = HeapIdx_to_ID[curr];
            HeapIdx_to_ID[curr] = tmp;

            tmp = ID_to_HeapIdx[HeapIdx_to_ID[max_child]];
            ID_to_HeapIdx[HeapIdx_to_ID[max_child]] = ID_to_HeapIdx[HeapIdx_to_ID[curr]];
            ID_to_HeapIdx[HeapIdx_to_ID[curr]] = tmp;
        }
        curr = max_child;
        child_l = 2 * curr + 1;
        child_r = 2 * curr + 2;
    }
}


void Heap::delete_vertex(int ID, std::vector<int>& HeapIdx_to_ID)
{
    int pos = ID_to_HeapIdx[ID];
    h[pos] = h[heap_size - 1];
    h.pop_back();
    heap_size--;
    siftdown(ID, HeapIdx_to_ID);
}


void Heap::out(void)
{

    for (int i = 0; i < heap_size; i++)
    {
        cout << h[i] << " ";
    }
    cout << endl;
}

int main()
{
    Heap heap1;
    Heap heap2;
    std::vector<int> HeapIdx1_to_ID;
    std::vector<int> HeapIdx2_to_ID;
    char c;

    int count = 0;
    int ID1 = -1;
    int ID2 = -1;
    deque<int> deq1;
    deque<int> deq2;

    int N, M, K, ID, root1_ID, root2_ID;
    N = 7;
    M = 4;
    K = 2;


    int ID1toID2[N];
    int ID2toID1[N];

    int R = 0, L = 0;
    int a[N] = { 4, 2, 1, 3, 6, 5, 7 };

    for (int i = 0; i < M; ++i)
    {
        cin >> c;
        if (c == 'R')
        {
            count++;
            R++;

            if (count < K)
            {
                ID1++;
                heap1.add(a[R], ID1, HeapIdx1_to_ID);
                deq1.push_back(ID1);
                cout << "-1" << endl;
            }
            if (count == K)
            {
                ID1++;
                heap1.add(a[R], ID1, HeapIdx1_to_ID);
                deq1.push_back(ID1);
                cout << heap1.get_root() << endl;
            }
            if (count > K)
            {

                if (a[R] > heap1.get_root())
                {
                    ID2++;
                    heap2.add(a[R], ID2, HeapIdx2_to_ID);
                    deq2.push_back(ID2);
                    cout << heap1.get_root() << endl;
                }

                else

                {

                    root1_ID = HeapIdx1_to_ID[0];
                    heap1.delete_vertex(root1_ID, HeapIdx1_to_ID);


                    ID1++;
                    heap1.add(a[R], ID1, HeapIdx1_to_ID);
                    deq1.push_back(ID1);

                    ID2++;
                    int root1ID = heap1.get_root();
                    heap2.add(root1ID, ID2, HeapIdx2_to_ID);
                    deq2.push_back(ID2);
                    deq1[root1ID] = -ID2;
                    ID1toID2[root1ID] = ID2;
                    ID2toID1[ID2] = root1ID;
                }
            }
        }
        else
        {
            L--;
            count--;
            if (count < K)
            {
                ID = deq1[0];
                if (ID < 0)
                {
                    deq1.pop_front();

                    heap2.delete_vertex(-ID, HeapIdx2_to_ID);
                    cout << "-1" << endl;
                }
                else
                {
                    deq1.pop_front();
                    heap1.delete_vertex(ID, HeapIdx1_to_ID);
                    cout << "-1" << endl;
                }
            }
            if (count >= K)
            {
                ID = deq1[0];

                if (ID < 0)
                {
                    deq1.pop_front();

                    heap2.delete_vertex(-ID, HeapIdx2_to_ID);
                }
                else
                {
                    deq1.pop_front();
                    heap1.delete_vertex(ID, HeapIdx1_to_ID);
                }

                ID1++;
                heap1.add(heap2.get_root(), ID1, HeapIdx1_to_ID);


                int ID_top2 = HeapIdx2_to_ID[0];
                heap2.delete_vertex(ID_top2, HeapIdx2_to_ID);


                deq1[ID2toID1[ID_top2]] = ID2toID1[ID_top2];
                cout << heap1.get_root() << endl;
            }
        }
    }

return 0;
}