File content as of revision 22:8f726dc175cd:

#include <cstdlib>
#include <cstring>
#include <cstdio>
#include <sstream>
#include <string>
#include <iostream>
#include <vector>
#include "navcontroller.h"

NavController::NavController(int mapSize) : map(mapSize), orient(NORTH)
{
    end.x = mapSize-1;
    end.y = mapSize-1; 
}

bool NavController::isDone() { return map.getPosition() == map.getFinish(); } 

void NavController::prepReturn()
{
    stack<R5Map::direction> temp;

    while(!path.empty())
    {
        temp.push(path.top());
        path.pop();
    }

    path = temp;
}

R5Map::direction NavController::nextTraceMove()
{
    R5Map::position newPos = map.getPosition();
    R5Map::nextMove;
    
    nextMove = path.top();
    path.pop();
    
    switch(nextMove)
    {
        case NORTH : newPos.y++;
            break;
        case SOUTH : newPos.y--;
            break;
        case EAST : newPos.x++;
            break;
        case WEST : newPos.x--;
    }
        
    map.setPosition(newPos);
    return nextMove;
}

R5Map::direction NavController::nextExploreMove()
{
    R5Map::position current = map.getPosition();
    R5Map::position dest = map.getFinish();
    
    int northWgt = dest.y - current.y;
    int southWgt = current.y;
    int eastWgt = current.x;
    int westWgt = current.x - dest.x;

    R5Map::direction nextMove;
    
    if(map.getVisited(NORTH) || map.getCurWall(NORTH) == CLOSED)
        northWgt = 0;

    if(map.getVisited(SOUTH) || map.getCurWall(SOUTH) == CLOSED)
        southWgt = 0;

    if(map.getVisited(EAST) || map.getCurWall(EAST) == CLOSED)
        eastWgt = 0;

    if(map.getVisited(WEST) || map.getCurWall(WEST) == CLOSED)
        westWgt = 0;

    if(northWgt != 0 || westWgt != 0)
    {
        if(northWgt > westWgt)
            nextMove = NORTH;
        else
            nextMove = WEST;

        path.push(nextMove);
    }
    else if(southWgt != 0 || eastWgt != 0)
    {
        if(southWgt > eastWgt)
            nextMove = SOUTH;
        else
            nextMove = EAST;

        path.push(nextMove);
    }
    else
    {
        R5Map::direction lastMove = path.top();
        path.pop();

        switch(lastMove)
        {
            case NORTH : nextMove = SOUTH;
                break;
            case SOUTH : nextMove = NORTH;
                break;
            case EAST : nextMove = WEST;
                break;
            case WEST : nextMove = EAST;
        }
    }

    return nextMove;
}

void NavController::updateMap()
{
    if(!map.getCurVisited())
    {
        readSensor(NORTH) ? map.setCurWall(NORTH, CLOSED) : map.setCurWall(NORTH, OPEN);
        readSensor(SOUTH) ? map.setCurWall(SOUTH, CLOSED) : map.setCurWall(SOUTH, OPEN);
        readSensor(EAST) ? map.setCurWall(EAST, CLOSED) : map.setCurWall(EAST, OPEN);
        readSensor(WEST) ? map.setCurWall(WEST, CLOSED) : map.setCurWall(WEST, OPEN);
    }
}

bool NavController::readSensor(R5Map::direction wall)
{
    int diff = wall - orient;
    
    switch(diff)
    {
        case 0:
            digitalRead(SENS_FORWARD);
        case 1:
            digitalRead(SENS_RIGHT);
        case 2:
            digitalRead(SENS_BACK);
        case 3:
            digitalRead(SENS_LEFT);
    }
}