<p>You are given an array of people, <code>people</code>, which are the attributes of some people in a queue (not necessarily in order). Each <code>people[i] = [h<sub>i</sub>, k<sub>i</sub>]</code> represents the <code>i<sup>th</sup></code> person of height <code>h<sub>i</sub></code> with <strong>exactly</strong> <code>k<sub>i</sub></code> other people in front who have a height greater than or equal to <code>h<sub>i</sub></code>.</p>

<p>Reconstruct and return <em>the queue that is represented by the input array </em><code>people</code>. The returned queue should be formatted as an array <code>queue</code>, where <code>queue[j] = [h<sub>j</sub>, k<sub>j</sub>]</code> is the attributes of the <code>j<sup>th</sup></code> person in the queue (<code>queue[0]</code> is the person at the front of the queue).</p>

<p>&nbsp;</p>
<p><strong class="example">Example 1:</strong></p>

<pre>
<strong>Input:</strong> people = [[7,0],[4,4],[7,1],[5,0],[6,1],[5,2]]
<strong>Output:</strong> [[5,0],[7,0],[5,2],[6,1],[4,4],[7,1]]
<strong>Explanation:</strong>
Person 0 has height 5 with no other people taller or the same height in front.
Person 1 has height 7 with no other people taller or the same height in front.
Person 2 has height 5 with two persons taller or the same height in front, which is person 0 and 1.
Person 3 has height 6 with one person taller or the same height in front, which is person 1.
Person 4 has height 4 with four people taller or the same height in front, which are people 0, 1, 2, and 3.
Person 5 has height 7 with one person taller or the same height in front, which is person 1.
Hence [[5,0],[7,0],[5,2],[6,1],[4,4],[7,1]] is the reconstructed queue.
</pre>

<p><strong class="example">Example 2:</strong></p>

<pre>
<strong>Input:</strong> people = [[6,0],[5,0],[4,0],[3,2],[2,2],[1,4]]
<strong>Output:</strong> [[4,0],[5,0],[2,2],[3,2],[1,4],[6,0]]
</pre>

<p>&nbsp;</p>
<p><strong>Constraints:</strong></p>

<ul>
	<li><code>1 &lt;= people.length &lt;= 2000</code></li>
	<li><code>0 &lt;= h<sub>i</sub> &lt;= 10<sup>6</sup></code></li>
	<li><code>0 &lt;= k<sub>i</sub> &lt; people.length</code></li>
	<li>It is guaranteed that the queue can be reconstructed.</li>
</ul>

class Solution {
public:
    vector<vector<int>> reconstructQueue(vector<vector<int>>& people) {
        map<int, set<int>> pos_to_ordered_height_map;
        for (const auto& person: people) {
            const int height = person[0];
            const int num_people = person[1]; 
            pos_to_ordered_height_map[num_people].insert(height);
        }

        // std::cout << "pos_to_ordered_height_map -> num_people : ordered_height_list\n";
        // for (const auto &[num_people, ordered_height_list] : pos_to_ordered_height_map) {
        //     std::cout << "\t" << num_people << ": ";
        //     for (const int height : ordered_height_list)    std::cout << height << " ";
        //     std::cout << "\n";
        // }

        vector<vector<int>> recons_queue;
        for (const auto &[num_people, ordered_height_list] : pos_to_ordered_height_map) {
            if (recons_queue.size() == 0) {
                for (const int height : ordered_height_list)    recons_queue.push_back({height, num_people});
                // std::cout << "Initial reconstruction_queue: ";
                // for (const auto& elem : recons_queue)   std::cout << "[" << elem[0] << "," << elem[1] << "] "; 
                // std::cout << "\n";
                continue;
            }

            for (const int height : ordered_height_list) {
                for (int i=0; i<recons_queue.size(); ++i) {
                    int count = 0;
                    while (i<recons_queue.size() && (recons_queue[i][0] < height || (recons_queue[i][0] >= height && count<num_people))) {
                        if (i<recons_queue.size() && recons_queue[i][0] >= height) ++count;
                        // std::cout << "\n\t\ti=" << i << "\tcount=" << count;
                        ++i;
                        if (count == num_people)    break;
                    }
                    if (count == num_people && recons_queue[i-1][0] >= height)    --i;
                    // std::cout << "\nrecons_queue.size()=" << recons_queue.size() << "\theight=" << height << "\tnum_people=" 
                    //           << num_people << "\ti=" << i << "\tcount=" << count << "\tnum_people=" << num_people << "\n";
                    if (i == recons_queue.size())   recons_queue.push_back({height, num_people});
                    else {
                        while (i+1 < recons_queue.size() && recons_queue[i+1][0] < height)   ++i;
                        if (i == recons_queue.size()-1)   recons_queue.push_back({height, num_people});
                        else recons_queue.insert(recons_queue.begin()+ i + 1, {height, num_people});
                    }

                    // std::cout << "Intermediate reconstruction_queue: ";
                    // for (const auto& elem : recons_queue)   std::cout << "[" << elem[0] << "," << elem[1] << "] "; 
                    // std::cout << "\n";
                    
                    break;
                }
            }
        }

        return recons_queue;
    }
};
18000