File content as of revision 9:97941581fe81:

#ifndef ALGO_H
#define ALGO_H
#include <cmath>
#include <stack>
#include <iostream>
#include <vector>
#include <algorithm>
#include "header.h"
struct geoCoord{
    geoCoord(int x_in, int y_in){x = x_in; y = y_in;}
    geoCoord(){x = 0; y = 0; distance = 99999.0;}
    int x, y;
    double distance;
};

bool compare(geoCoord* left, geoCoord* right){
    return left->distance > right->distance;
}

int isWall(){
    float ir_r[4]; //ir_readings, in the order of l, r, lf, rf
    FlashIRs(ir_r[0], ir_r[1], ir_r[2], ir_r[3]);
    int return_val = 0;
    if (ir_r[2] > 0.2f && ir_r[3] > 0.2f){
     return_val = return_val | 1;
    }
    if(ir_r[1] > 0.3f){
      return_val = return_val | 7;
    }
    if(ir_r[0] > 0.3f){
      return_val = return_val | 3;
    }
    return return_val; //This implementation is wrong but I got hungry.
}

void goForward(int times){
    float l,r,lf,rf;
    FlashIRs(l, r, lf, rf);
    if(l < 0.17f && r < 0.17f){
         printf("goForward is using Encoder!");
        RightEncoder.reset();
        LeftEncoder.reset();
        usePIDEncoder();    
        RightEncoder.reset();
        LeftEncoder.reset();
    }
    else{
        printf("goForward is using IR PID!");
        RightEncoder.reset();
        LeftEncoder.reset();
        usePIDBoth(); //Goes straight and uses PID using IR and the encoders
        RightEncoder.reset();
        LeftEncoder.reset();
    }
    //Should be able to detect if there aren't walls and just work on the encoders
    //And should also record the distance for just one cell.
    return;
}

void MoveTo(geoCoord* current_coord, geoCoord* next, int curr_orientation){ //Note that turning automatically sets the global orientation constant
    printf("Moving to: (%d, %d) From: (%d, %d):  \n", current_coord->x, current_coord->y, next->x, next->y);
    int diff_x = next->x - current_coord->x;
    int diff_y = next->y - current_coord->y;
    if(diff_x == 1){
        if(curr_orientation == SOUTH)
            turn_left(1); 
        else if(curr_orientation == WEST){
            turn_right(2);
        }
        else if(curr_orientation == NORTH){
            turn_right(1);
        }
    }
    else if(diff_x == -1){
        if(curr_orientation == SOUTH){
            turn_right(1);
        }
        else if(curr_orientation == NORTH){
            turn_left(1);
        }
        else if(curr_orientation == EAST){
            turn_right(2);
        }
    }
    else if(diff_y == 1){
        if(curr_orientation == SOUTH){
            turn_right(2);
        }
        else if(curr_orientation == EAST){
            turn_left(1);
        }
        else if(curr_orientation == WEST){
            turn_right(1);
        }
    }
    else if(diff_y == -1){
        if(curr_orientation == NORTH){
            turn_right(2);
        }
        else if(curr_orientation == EAST){
            turn_right(1);
        }
        else if(curr_orientation == WEST){
            turn_left(1);
        }
    }
    goForward(1);
    return;
}

geoCoord cellarray[16][16];
geoCoord Target(8,8); //Target cell was assumed to be here.
geoCoord Start(0,0);

void algorithm(){ //Implementation of floodfill algorithm
for(int i = 0; i < 16; i++){
        for(int k = 0; k < 16; k++){
         cellarray[i][k].x = i; cellarray[i][k].y = k; //Initializes coordinates of geocoord array
        }
    }
    for(int i = 0; i < 16; i++){
        for(int k = 0; k < 16; k++){
            cellarray[i][k].distance = std::sqrt((double)(cellarray[i][k].x - Start.x)*(double)(cellarray[i][k].x - Start.x) + (double)(cellarray[i][k].y - Start.y)*(double)(cellarray[i][k].y - Start.y)); //Initializes the distances of the cellarray
            printf("Cell Distance to (%d, %d): %.2f \n", i, k, cellarray[i][k].distance);
        }
    }
    std::stack<geoCoord*> cells_to_traverse;
    cells_to_traverse.push(&Start);
    while(cells_to_traverse.size() > 0){
        geoCoord* current = cells_to_traverse.top();
        printf("CurrentGeoCoord (%d, %d): %.2f \n", current->x, current->y, current->distance);
        cells_to_traverse.pop();//Theory: It's not pushing the correct cells.
        int r_val = isWall();
        printf("r_val: %d \n", r_val);
        for(int i = 1; i < 8; i+= 2) //1 for Forward, 3 for Left, 5 for South, 7 for Right. South probably isn't going to be a thing
        if(r_val&i == i){ //Potentially pushes 4 cells onto the stack
            if(i == 1 && current->y < 15)
                cells_to_traverse.push(&cellarray[current->x][current->y + 1]);
            else if(i == 3 && current->x > 0)
                cells_to_traverse.push(&cellarray[current->x - 1][current->y]);
            else if(i == 7 && current->x < 15)
                cells_to_traverse.push(&cellarray[current->x+1][current->y]);
            else if(i == 5 && current->y > 0)
                cells_to_traverse.push(&cellarray[current->x][current->y-1]); //Checks bounds for the potential cells to push.
        }
        std::vector<geoCoord*> neighboring_cells;
        unsigned int t_size = cells_to_traverse.size();
        for(int i = 0; i < t_size; i++){
            neighboring_cells.push_back(cells_to_traverse.top()); //We temporarily put neighboring cells into a vector and sort them
            cells_to_traverse.pop();
        }
        std::sort(neighboring_cells.begin(), neighboring_cells.end(), compare);
        for(int i = 0; i < t_size; i++){
            cells_to_traverse.push(neighboring_cells[i]); //Puts the sorted vector into reverse order back into the stack
        }
        MoveTo(current, cells_to_traverse.top(), global_orientation); //Moves to the cell that is the closest to the target cell
        cells_to_traverse.pop();
    }
}
#endif