#include "pledge.h"
#include "config.h"
#include "math.h"
#include "taskmanager.h"

Pledge checkPledge(int direction, bool space_left, bool space_top, bool space_right)
{
 state_t state;
    // Preferred direction
    if (direction == 0) {
        if (space_top) {
            state = straight_pref;  // Keep going forward
            std::cout << "Dir: " << direction << ", Go forward in preferred direction" << std::endl;
        }
        else if (!space_top) {
            state = turn_left;      // Turn left when obstacle in front
            direction--;
            std::cout << "Dir: " << direction << ", Obstacle ahead, turn left" << std::endl;
        }
        else if (!space_left && !space_top && !space_right) {
            state = turn_180;       // Dead end detected
            direction--;
            direction--;
            std::cout << "Dir: " << direction << ", Dead end detected, rotate 180 deg" << std::endl;
        }
    }
    // Other than preferred direction
    else {
        if (space_right) {
            state = turn_right;     // Always turn right when there is space
            direction++;
            std::cout << "Dir: " << direction << ", Turn right, unpreferred direction" << std::endl;
        }
        else if (!space_right && space_top) {
            state = straight;
            std::cout << "Dir: " << direction << ", Go straight, unpreferred direction" << std::endl;
        }
        else if (!space_right && !space_top && space_left) {
            state = turn_left;
            direction--;
            std::cout << "Dir: " << direction << ", Go left, unpreferred direction" << std::endl;
        }
        else if (!space_right && !space_top && !space_left) {
            state = turn_180;
            direction--;
            direction--;
            std::cout << "Dir: " << direction << ", Dead end detected, rotate 180 deg, unpreferred direction" << std::endl;
        }
    }

    return Pledge(direction, state);
}