<p>A robot on an infinite XY-plane starts at point <code>(0, 0)</code> facing north. The robot receives an array of integers <code>commands</code>, which represents a sequence of moves that it needs to execute. There are only three possible types of instructions the robot can receive:</p>

<ul>
	<li><code>-2</code>: Turn left <code>90</code> degrees.</li>
	<li><code>-1</code>: Turn right <code>90</code> degrees.</li>
	<li><code>1 &lt;= k &lt;= 9</code>: Move forward <code>k</code> units, one unit at a time.</li>
</ul>

<p>Some of the grid squares are <code>obstacles</code>. The <code>i<sup>th</sup></code> obstacle is at grid point <code>obstacles[i] = (x<sub>i</sub>, y<sub>i</sub>)</code>. If the robot runs into an obstacle, it will stay in its current location (on the block adjacent to the obstacle) and move onto the next command.</p>

<p>Return the <strong>maximum squared Euclidean distance</strong> that the robot reaches at any point in its path (i.e. if the distance is <code>5</code>, return <code>25</code>).</p>

<p><strong>Note:</strong></p>

<ul>
	<li>There can be an obstacle at <code>(0, 0)</code>. If this happens, the robot will ignore the obstacle until it has moved off the origin. However, it will be unable to return to <code>(0, 0)</code> due to the obstacle.</li>
	<li>North means +Y direction.</li>
	<li>East means +X direction.</li>
	<li>South means -Y direction.</li>
	<li>West means -X direction.</li>
</ul>

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

<div class="example-block">
<p><strong>Input:</strong> <span class="example-io">commands = [4,-1,3], obstacles = []</span></p>

<p><strong>Output:</strong> <span class="example-io">25</span></p>

<p><strong>Explanation: </strong></p>

<p>The robot starts at <code>(0, 0)</code>:</p>

<ol>
	<li>Move north 4 units to <code>(0, 4)</code>.</li>
	<li>Turn right.</li>
	<li>Move east 3 units to <code>(3, 4)</code>.</li>
</ol>

<p>The furthest point the robot ever gets from the origin is <code>(3, 4)</code>, which squared is <code>3<sup>2</sup> + 4<sup>2 </sup>= 25</code> units away.</p>
</div>

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

<div class="example-block">
<p><strong>Input:</strong> <span class="example-io">commands = [4,-1,4,-2,4], obstacles = [[2,4]]</span></p>

<p><strong>Output:</strong> <span class="example-io">65</span></p>

<p><strong>Explanation:</strong></p>

<p>The robot starts at <code>(0, 0)</code>:</p>

<ol>
	<li>Move north 4 units to <code>(0, 4)</code>.</li>
	<li>Turn right.</li>
	<li>Move east 1 unit and get blocked by the obstacle at <code>(2, 4)</code>, robot is at <code>(1, 4)</code>.</li>
	<li>Turn left.</li>
	<li>Move north 4 units to <code>(1, 8)</code>.</li>
</ol>

<p>The furthest point the robot ever gets from the origin is <code>(1, 8)</code>, which squared is <code>1<sup>2</sup> + 8<sup>2</sup> = 65</code> units away.</p>
</div>

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

<div class="example-block">
<p><strong>Input:</strong> <span class="example-io">commands = [6,-1,-1,6], obstacles = [[0,0]]</span></p>

<p><strong>Output:</strong> <span class="example-io">36</span></p>

<p><strong>Explanation:</strong></p>

<p>The robot starts at <code>(0, 0)</code>:</p>

<ol>
	<li>Move north 6 units to <code>(0, 6)</code>.</li>
	<li>Turn right.</li>
	<li>Turn right.</li>
	<li>Move south 5 units and get blocked by the obstacle at <code>(0,0)</code>, robot is at <code>(0, 1)</code>.</li>
</ol>

<p>The furthest point the robot ever gets from the origin is <code>(0, 6)</code>, which squared is <code>6<sup>2</sup> = 36</code> units away.</p>
</div>

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

<ul>
	<li><code>1 &lt;= commands.length &lt;= 10<sup>4</sup></code></li>
	<li><code>commands[i]</code> is either <code>-2</code>, <code>-1</code>, or an integer in the range <code>[1, 9]</code>.</li>
	<li><code>0 &lt;= obstacles.length &lt;= 10<sup>4</sup></code></li>
	<li><code>-3 * 10<sup>4</sup> &lt;= x<sub>i</sub>, y<sub>i</sub> &lt;= 3 * 10<sup>4</sup></code></li>
	<li>The answer is guaranteed to be less than <code>2<sup>31</sup></code>.</li>
</ul>

class Solution {
public:
    vector<int> position{0,0};  //initial pos
    vector<int> direction{0,1}; //facing north
    int maxDistance = 0;
    // set<vector<int>> obstacleSet;
    // vector<set<int>> obstacleSet;


    void rotateCW(){
        int x = direction[0];
        direction[0] = direction[1];
        direction[1] = -x;
    }

    void rotateCCW(){
        int x = direction[0];
        direction[0] = -direction[1];
        direction[1] = x;
    }

    int getDistance(){
        return position[0]*position[0] + position[1]*position[1];
    }

    // bool obstacle(vector<vector<int>>& obstacles, vector<int> position, vector<int> direction){
        
    //     position[0]+=direction[0];
    //     position[1]+=direction[1];

    //     if(obstacleSet.find(position) != obstacleSet.end()) return true;

    //     // for(const vector<int>& obstacle : obstacles){
    //     //     if(obstacle[0] == position[0] 
    //     //     && obstacle[1] == position[1]) return true;
    //     // }

    //     return false;
    // }

    // //1st try
    // int robotSim(vector<int>& commands, vector<vector<int>>& obstacles) {
        
    //     for(const vector<int>& obstacle : obstacles){
    //         obstacleSet.insert(obstacle);
    //     }

    //     for(int i = 0; i<commands.size(); i++){
    //         if(commands[i] == -1) {
    //             rotateCW();
    //         } else if(commands[i] == -2) {
    //             rotateCCW();
    //         } else if(commands[i] > 0 && commands[i] < 10){
    //             for(int j = 0; j<commands[i]; j++){
    //                 // cout << position[0] << ", " << position[1] << endl;
    //                 if(obstacle(obstacles, position, direction)) break;
    //                 position[0] += direction[0];
    //                 position[1] += direction[1];
    //                 maxDistance = max(maxDistance, getDistance());
    //             }
    //         };
    //     }

    //     return maxDistance;
    // }

    map<int, set<int>> obstacleMapH;
    map<int, set<int>> obstacleMapV;

    template <typename T> int sgn(T val) {
        return (T(0) < val) - (val < T(0));
    }


    int getObstacle(set<int> set, int begin, int end){
        if(set.empty()) return end;
        // cout << "end: " << end << endl;
        // set<int>::iterator it = set.lower_bound(begin);
        for(int i = 1; i<abs(end-begin)+1; i++){
            int p = begin+i*sgn(end-begin);
        // cout << "p: " << p << endl;

            if (set.contains(p)) return p-sgn(end-begin);
        }
        return end;
    }

    bool obstacle(vector<vector<int>>& obstacles, vector<int>& position, vector<int> direction, int steps){
        
        int x = position[0];
        int y = position[1];
        int finalX, finalY;
        if(direction[0] == 0){ //vertical
            finalX = x;
            finalY = getObstacle(obstacleMapV[x], y, y+direction[1]*steps);
            
            // if(!obstacleMapV.contains(x)){
            //     finalY = y+direction[1]*steps;
            // }
            // else 
            // if(direction[1] == 1) {
            //     finalY = getObstacle(obstacleMapV[x], y, y+direction[1]*steps);
            //     // set<int>::iterator it = obstacleMapV[x].lower_bound(y);
            //     // if(*it < y) finalY = y+steps;
            //     // // cout << *(it) << endl;
            //     // else finalY = min(*(it)-1,y+steps);
            // }
            // else if(direction[1] == -1) {
            //     set<int>::iterator it = obstacleMapV[x].upper_bound(y);
            //     if(it != obstacleMapV[x].begin()){
            //         finalY = max(*(--it)+1, y-steps);
            //     }else{
            //         finalY = y-steps;
            //     }

            // };
        }

        if(direction[1] == 0){ //horizontal

            finalY = y;
            finalX = getObstacle(obstacleMapH[y], x, x+direction[0]*steps);

            // finalY = y;
            // if(!obstacleMapH.contains(y)){
            //     finalX = x+direction[0]*steps;
            // }
            // else if(direction[0] == 1) {
            //     set<int>::iterator it = obstacleMapH[y].lower_bound(x);
            //     finalX = min(*(it)-1,x+steps);
            // }
            // else if(direction[0] == -1) {
            //     set<int>::iterator it = obstacleMapH[y].upper_bound(x);
            //     if(it != obstacleMapH[y].begin()){
            //         finalX = max(*(--it)+1, x-steps);
            //     }else{
            //         finalX = x-steps;
            //     }

            // };
        }

        // if(direction[1] == 0){ //horizontal
        //     if(direction[0] == 1) finalX = *(obstacleMapH[y].lower_bound(x));
        //     if(direction[0] == -1) finalX = *(--obstacleMapH[y].upper_bound(x));
        //     finalY = y;
        // }

        position[0]=finalX;
        position[1]=finalY;

        // if(obstacleSet.find(position) != obstacleSet.end()) return true;

        return false;
    }


    //2nd try
    int robotSim(vector<int>& commands, vector<vector<int>>& obstacles) {
        
        for(const vector<int>& obstacle : obstacles){
            int x = obstacle[0];
            int y = obstacle[1];
            
            if(obstacleMapH.contains(y)) obstacleMapH[y].insert(x);
            else obstacleMapH[y] = set<int>{x};

            if(obstacleMapV.contains(x)) obstacleMapV[x].insert(y);
            else obstacleMapV[x] = set<int>{y};
        }

        for(int i = 0; i<commands.size(); i++){
            if(commands[i] == -1) {
                rotateCW();
            } else if(commands[i] == -2) {
                rotateCCW();
            } else if(commands[i] > 0 && commands[i] < 10){
                // for(int j = 0; j<commands[i]; j++){
                    // cout << position[0] << ", " << position[1] << endl;
                    // cout << "direction" << direction[0] << ", " << direction[1] << endl;
                    obstacle(obstacles, position, direction, commands[i]);
                    // cout << position[0] << ", " << position[1] << endl;
                    // position[0] += direction[0];
                    // position[1] += direction[1];

                    maxDistance = max(maxDistance, getDistance());
                // }
            };
        }

        return maxDistance;
    }
};
61201