File content as of revision 2:5343f19381c8:

#include "Lees_Algorithm.h"
#include "mbed.h"

Lees_Algorithm::Lees_Algorithm(unsigned char size_): mazeSize(size_) {
    
    map = new _cell*[mazeSize];
    for(int i = 0; i < mazeSize; i++)
    {
        map[i] = new _cell[mazeSize];
    }

    map[0][0].weight = 0;
    map[0][0].weight -= 1;
    maxWeight = map[0][0].weight;

    clearWeights();
    
    for(int i = 0; i < mazeSize; i++)
    {
        for(int j = 0; j < mazeSize; j++)
        {
            if(i == 0) map[0][j].north = true;
            else map[i][j].north = false;
            if(j == 0) map[i][0].west = true;
            else map[i][j].west = false;
        }
    }
    
    nextRead = 0;
    nextWrite = 0;
    stackSize = 0;

    return;
}

Lees_Algorithm::~Lees_Algorithm(void) {
    for(int i = 0; i < mazeSize; i++)
    {
        delete[] map[i];
    }
    delete[] map;
}


void Lees_Algorithm::updateMap(_location position, bool wNorth, bool wWest, bool wSouth, bool wEast) {
    _location buffer, newLoc;
    unsigned char minimumWeight;
    
    // copy new walls to database
    if(position.x == 0) wWest = true; // force some walls to true
    if(position.y == 0) wNorth = true;
    if(position.x == (mazeSize - 1)) wEast = true;
    if(position.y == (mazeSize - 1)) wSouth = true;
    
    map[position.y][position.x].north = wNorth;
    map[position.y][position.x].west = wWest;
    map[((position.y + 1) % mazeSize)][position.x].north = wSouth;
    map[position.y][((position.x + 1) % mazeSize)].west = wEast;
   
    // run lees algorthim
    push(position);
    
    while(stackSize > 0)
    {
        buffer = pull();        
        minimumWeight = 255;        
        
        if(!map[buffer.y][buffer.x].north && map[(buffer.y - 1) % mazeSize][buffer.x].weight < minimumWeight) minimumWeight = map[(buffer.y - 1) % mazeSize][buffer.x].weight;
        if(!map[buffer.y][buffer.x].west && map[buffer.y][(buffer.x - 1) % mazeSize].weight < minimumWeight) minimumWeight = map[buffer.y][(buffer.x - 1) % mazeSize].weight;
        if(!map[(buffer.y + 1) % mazeSize][buffer.x].north && map[(buffer.y + 1) % mazeSize][buffer.x].weight < minimumWeight) minimumWeight = map[(buffer.y + 1) % mazeSize][buffer.x].weight;
        if(!map[buffer.y][(buffer.x + 1) % mazeSize].west && map[buffer.y][(buffer.x + 1) % mazeSize].weight < minimumWeight) minimumWeight = map[buffer.y][(buffer.x + 1) % mazeSize].weight;
        
        if(map[buffer.y][buffer.x].weight != (minimumWeight + 1))
        {
            if(map[buffer.y][buffer.x].weight != 0) map[buffer.y][buffer.x].weight = minimumWeight + 1;
            
            if(map[buffer.y][buffer.x].north == false)
            {
                newLoc.y = buffer.y - 1;
                newLoc.x = buffer.x;
                push(newLoc);
            }
            if(map[buffer.y][buffer.x].west == false)
            {
                newLoc.y = buffer.y;
                newLoc.x = buffer.x - 1;
                push(newLoc);
            }
            if(map[(buffer.y + 1) % mazeSize][buffer.x].north == false)
            {
                newLoc.y = (buffer.y + 1) % mazeSize;
                newLoc.x = buffer.x;
                push(newLoc);
            }
            if(map[buffer.y][(buffer.x + 1) % mazeSize].west == false)
            {
                newLoc.y = buffer.y;
                newLoc.x = (buffer.x + 1) % mazeSize;
                push(newLoc);
            }
        }        
    }
    
    return;
}

char Lees_Algorithm::getDirection(_location position, char facing) {
    char buffer;
    unsigned char minimumWeight = 255;
    bool direction[4] = {false, false, false, false};
    
    if(!map[position.y][position.x].north && map[(position.y - 1) % mazeSize][position.x].weight < minimumWeight) minimumWeight = map[(position.y - 1) % mazeSize][position.x].weight;
    if(!map[position.y][position.x].west && map[position.y][(position.x - 1) % mazeSize].weight < minimumWeight) minimumWeight = map[position.y][(position.x - 1) % mazeSize].weight;
    if(!map[(position.y + 1) % mazeSize][position.x].north && map[(position.y + 1) % mazeSize][position.x].weight < minimumWeight) minimumWeight = map[(position.y + 1) % mazeSize][position.x].weight;
    if(!map[position.y][(position.x + 1) % mazeSize].west && map[position.y][(position.x + 1) % mazeSize].weight < minimumWeight) minimumWeight = map[position.y][(position.x + 1) % mazeSize].weight;
    
    if(!map[position.y][position.x].north && map[(position.y - 1) % mazeSize][position.x].weight == minimumWeight) direction[0] = true;
        if(!map[position.y][position.x].west && map[position.y][(position.x - 1) % mazeSize].weight == minimumWeight) direction[1] = true;
        if(!map[(position.y + 1) % mazeSize][position.x].north && map[(position.y + 1) % mazeSize][position.x].weight == minimumWeight) direction[2] = true;
        if(!map[position.y][(position.x + 1) % mazeSize].west && map[position.y][(position.x + 1) % mazeSize].weight == minimumWeight) direction[3] = true;
    
    switch(facing) {
        case 'n':
            if(direction[3] == true) buffer = 's'; 
            if(direction[2] == true) buffer = 'e'; 
            if(direction[1] == true) buffer = 'w';
            if(direction[0] == true) buffer = 'n';
            break;
        case 'w':
            if(direction[2] == true) buffer = 'e';
            if(direction[3] == true) buffer = 's'; 
            if(direction[0] == true) buffer = 'n';
            if(direction[1] == true) buffer = 'w';
            break;
        case 's':
            if(direction[0] == true) buffer = 'n';
            if(direction[2] == true) buffer = 'e';
            if(direction[1] == true) buffer = 'w';
            if(direction[3] == true) buffer = 's';
            break;
        case 'e':
            if(direction[1] == true) buffer = 'w';
            if(direction[0] == true) buffer = 'n';
            if(direction[3] == true) buffer = 's';
            if(direction[2] == true) buffer = 'e';
            break;
        default:
            break;
    }
    if(map[position.y][position.x].weight == 0) buffer = 'x';
    return buffer;
}

void Lees_Algorithm::initialFill(void) {

    int i,j;
    bool finished = false;
    
    while(finished == false)
    {
        
        for(i = 0; i < mazeSize; i++)
        {
            for(j = 0; j < mazeSize; j++)
            {
                if(map[i][j].weight == 0)
                {
                    for(int k = j - 1; k >= 0; k--)
                    {
                        if(map[i][k].weight != 0) map[i][k].weight = map[i][k+1].weight + 1;
                    }
                    
                    for(int k = j + 1; k < mazeSize; k++)
                    {
                        if(map[i][k].weight != 0) map[i][k].weight = map[i][k-1].weight + 1;
                    }
                }
                if(i == (mazeSize - 1) && j == (mazeSize -1)) finished = true;
            }
        }
    }
    
    for(int i = 0; i < mazeSize; i++)
    {
        if(map[i][0].weight < maxWeight)
        {
            for(int k = i - 1; k >= 0; k--)
            {
                if(map[k][0].weight == maxWeight)
                {
                    for(int j = 0; j < mazeSize; j++)
                    {
                        map[k][j].weight = map[k+1][j].weight + 1;
                    }
                }
            }
            for(int k = i + 1; k < mazeSize; k++)
            {
                if(map[k][0].weight == maxWeight)
                {
                    for(int j = 0; j < mazeSize; j++)
                    {
                        map[k][j].weight = map[k-1][j].weight + 1;
                    }
                }
            }
        }
    }
    
    
    return;
}

void Lees_Algorithm::printMaze(Serial output) {

    for(int i = 0; i < mazeSize; i ++)
    {
        for(int j = 0; j < mazeSize; j++)
        {
            if(map[i][j].north == true) output.printf("+---");
            else output.printf("+   ");
        }
        output.printf("+\r\n");

        for(int j = 0; j < mazeSize; j++)
        {
            if(map[i][j].west == true) output.printf("|");
            else output.printf(" ");
            output.printf("%3i", map[i][j].weight);
        }
        if(map[i][0].west == true) output.printf("|\r\n");
        else output.printf(" \r\n");
    }
    for(int j = 0; j < mazeSize; j ++)
    {
        if(map[0][j].north == 1) output.printf("+---");
        else output.printf("+   ");
    }
    output.printf("+\r\n");

    return;
}

void Lees_Algorithm::setDestination(_location target) {
    map[target.y][target.x].weight = 0;
}

void Lees_Algorithm::clearWeights(void) {
    for (int j = 0; j < mazeSize; j++) 
    {
        for (int i = 0; i < mazeSize; i++) 
        {
            //map[0][0].weight = 0; 
            //map[0][0].weight -= 1; // should auto-set to 255
            map[j][i].weight = maxWeight;
        }
    }
    return;
}