File content as of revision 57:aba1296e51b1:

#include "mbed.h"           //Include the mbed libraries
#include "ALGORITHM.hpp"          //Include the header file, this acts like a series of forward declarations
#include "SERIAL_COMMANDS.hpp"

//Constructor
ALGORITHM::ALGORITHM() //Constructor
{
    _State = 0;
}

ALGORITHM::~ALGORITHM(){}   //Destructor

void ALGORITHM::ALGORITHM_INIT()
{
    _State = 0;
    /*
    Needs to know what colour all of the positions of the cube are in so that it can enact upon this data.
    
    */
} 

void ALGORITHM::Find_edges()//Sets the internal variable to the location of all the pieces and the positions of them
{  
//Reset the Pointer Values
    _White_Pointer_Edge  = 0;
    _Orange_Pointer_Edge = 0;
    _Blue_Pointer_Edge   = 0;
    _Red_Pointer_Edge    = 0;
    _Green_Pointer_Edge  = 0;
    _Yellow_Pointer_Edge = 0;
//Reset the 
    
    _Find_edges_White();
    _Find_edges_Orange();
    _Find_edges_Blue();
    _Find_edges_Red();
    _Find_edges_Green();
    _Find_edges_Yellow();
}   

void ALGORITHM::Find_Corners()//Sets the internal variable to the location of all the pieces and the positions of them
{  
//Reset the Pointer Values
    _White_Pointer_Corner  = 0;
    _Orange_Pointer_Corner = 0;
    _Blue_Pointer_Corner   = 0;
    _Red_Pointer_Corner    = 0;
    _Green_Pointer_Corner  = 0;
    _Yellow_Pointer_Corner = 0;
//Reset the 
    
    _Find_Corners_White();
    _Find_Corners_Orange();
    _Find_Corners_Blue();
    _Find_Corners_Red();
    _Find_Corners_Green();
    _Find_Corners_Yellow();
}   
void ALGORITHM::_Find_Corners_White()//Identify White edges
{
    //Check White side 
    if(CubeMap[0][0][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =1;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][0] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =3;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][2] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =7;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[0][2][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =9;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =1;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][0] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =3;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][2] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =7;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[1][2][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =9;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =1;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][0] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =3;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][2] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =7;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[2][2][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =9;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =1;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][0] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =3;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][2] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =7;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[3][2][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =9;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =1;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][0] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =3;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][2] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =7;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[4][2][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =9;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =1;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][0] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =3;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][2] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =7;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
    if(CubeMap[5][2][1] == White)
    {
     _White_List_Corner[_White_Pointer_Corner] =9;//Store this internal variable  
     _White_Pointer_Corner = _White_Pointer_Corner + 1; 
    }
}


void ALGORITHM::_Find_Corners_Orange()//Identify Orange edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =1;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][0] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =3;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][2] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =7;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[0][2][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =9;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =1;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][0] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =3;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][2] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =7;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[1][2][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =9;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =1;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][0] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =3;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][2] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =7;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[2][2][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =9;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =1;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][0] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =3;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][2] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =7;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[3][2][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =9;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =1;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][0] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =3;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][2] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =7;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[4][2][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =9;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =1;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][0] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =3;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][2] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =7;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
    if(CubeMap[5][2][1] == Orange)
    {
     _Orange_List_Corner[_Orange_Pointer_Corner] =9;//Store this internal variable  
     _Orange_Pointer_Corner = _Orange_Pointer_Corner + 1; 
    }
}
void ALGORITHM::_Find_Corners_Blue()//Identify White edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =1;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][0] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =3;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][2] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =7;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[0][2][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =9;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =1;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][0] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =3;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][2] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =7;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[1][2][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =9;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =1;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][0] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =3;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][2] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =7;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[2][2][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =9;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =1;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][0] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =3;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][2] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =7;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[3][2][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =9;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =1;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][0] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =3;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][2] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =7;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[4][2][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =9;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =1;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][0] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =3;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][2] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =7;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
    if(CubeMap[5][2][1] == Blue)
    {
     _Blue_List_Corner[_Blue_Pointer_Corner] =9;//Store this internal variable  
     _Blue_Pointer_Corner = _Blue_Pointer_Corner + 1; 
    }
}
void ALGORITHM::_Find_Corners_Red()//Identify Red edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =1;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][0] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =3;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][2] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =7;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[0][2][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =9;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =1;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][0] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =3;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][2] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =7;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[1][2][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =9;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =1;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][0] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =3;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][2] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =7;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[2][2][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =9;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =1;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][0] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =3;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][2] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =7;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[3][2][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =9;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =1;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][0] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =3;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][2] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =7;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[4][2][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =9;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =1;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][0] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =3;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][2] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =7;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
    if(CubeMap[5][2][1] == Red)
    {
     _Red_List_Corner[_Red_Pointer_Corner] =9;//Store this internal variable  
     _Red_Pointer_Corner = _Red_Pointer_Corner + 1; 
    }
}
void ALGORITHM::_Find_Corners_Green()//Identify Green edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =1;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][0] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =3;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][2] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =7;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[0][2][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =9;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =1;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][0] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =3;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][2] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =7;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[1][2][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =9;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =1;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][0] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =3;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][2] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =7;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[2][2][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =9;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =1;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][0] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =3;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][2] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =7;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[3][2][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =9;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =1;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][0] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =3;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][2] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =7;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[4][2][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =9;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =1;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][0] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =3;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][2] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =7;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
    if(CubeMap[5][2][1] == Green)
    {
     _Green_List_Corner[_Green_Pointer_Corner] =9;//Store this internal variable  
     _Green_Pointer_Corner = _Green_Pointer_Corner + 1; 
    }
}
void ALGORITHM::_Find_Corners_Yellow()//Identify Yellow edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =1;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][0] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =3;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[0][1][2] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =7;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[0][2][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =9;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =1;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][0] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =3;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[1][1][2] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =7;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[1][2][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =9;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =1;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][0] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =3;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[2][1][2] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =7;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[2][2][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =9;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =1;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][0] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =3;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[3][1][2] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =7;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[3][2][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =9;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =1;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][0] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =3;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[4][1][2] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =7;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[4][2][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =9;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =1;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][0] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =3;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[5][1][2] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =7;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
    if(CubeMap[5][2][1] == Yellow)
    {
     _Yellow_List_Corner[_Yellow_Pointer_Corner] =9;//Store this internal variable  
     _Yellow_Pointer_Corner = _Yellow_Pointer_Corner + 1; 
    }
}      

void ALGORITHM::_Find_edges_White()//Identify White edges
{
    //Check White side 
    if(CubeMap[0][0][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 2;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][0] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 4;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][2] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 6;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[0][2][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 8;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 2;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][0] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 4;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][2] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 6;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[1][2][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 8;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 2;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][0] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 4;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][2] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 6;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[2][2][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 8;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 2;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][0] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 4;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][2] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 6;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[3][2][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 8;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 2;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][0] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 4;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][2] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 6;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[4][2][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 8;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 2;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][0] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 4;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][2] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 6;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
    if(CubeMap[5][2][1] == White)
    {
     _White_List_Edge[_White_Pointer_Edge] = 8;//Store this internal variable  
     _White_Pointer_Edge = _White_Pointer_Edge + 1; 
    }
}


void ALGORITHM::_Find_edges_Orange()//Identify Orange edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 2;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][0] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 4;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][2] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 6;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[0][2][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 8;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 2;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][0] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 4;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][2] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 6;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[1][2][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 8;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 2;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][0] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 4;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][2] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 6;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[2][2][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 8;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 2;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][0] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 4;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][2] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 6;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[3][2][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 8;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 2;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][0] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 4;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][2] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 6;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[4][2][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 8;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 2;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][0] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 4;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][2] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 6;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
    if(CubeMap[5][2][1] == Orange)
    {
     _Orange_List_Edge[_Orange_Pointer_Edge] = 8;//Store this internal variable  
     _Orange_Pointer_Edge = _Orange_Pointer_Edge + 1; 
    }
}
void ALGORITHM::_Find_edges_Blue()//Identify White edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 2;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][0] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 4;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][2] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 6;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[0][2][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 8;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 2;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][0] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 4;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][2] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 6;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[1][2][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 8;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 2;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][0] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 4;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][2] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 6;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[2][2][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 8;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 2;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][0] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 4;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][2] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 6;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[3][2][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 8;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 2;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][0] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 4;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][2] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 6;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[4][2][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 8;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 2;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][0] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 4;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][2] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 6;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
    if(CubeMap[5][2][1] == Blue)
    {
     _Blue_List_Edge[_Blue_Pointer_Edge] = 8;//Store this internal variable  
     _Blue_Pointer_Edge = _Blue_Pointer_Edge + 1; 
    }
}
void ALGORITHM::_Find_edges_Red()//Identify Red edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 2;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][0] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 4;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][2] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 6;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[0][2][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 8;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 2;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][0] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 4;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][2] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 6;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[1][2][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 8;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 2;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][0] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 4;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][2] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 6;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[2][2][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 8;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 2;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][0] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 4;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][2] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 6;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[3][2][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 8;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 2;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][0] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 4;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][2] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 6;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[4][2][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 8;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 2;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][0] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 4;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][2] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 6;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
    if(CubeMap[5][2][1] == Red)
    {
     _Red_List_Edge[_Red_Pointer_Edge] = 8;//Store this internal variable  
     _Red_Pointer_Edge = _Red_Pointer_Edge + 1; 
    }
}
void ALGORITHM::_Find_edges_Green()//Identify Green edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 2;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][0] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 4;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][2] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 6;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[0][2][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 8;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 2;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][0] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 4;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][2] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 6;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[1][2][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 8;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 2;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][0] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 4;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][2] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 6;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[2][2][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 8;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 2;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][0] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 4;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][2] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 6;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[3][2][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 8;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 2;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][0] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 4;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][2] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 6;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[4][2][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 8;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 2;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][0] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 4;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][2] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 6;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
    if(CubeMap[5][2][1] == Green)
    {
     _Green_List_Edge[_Green_Pointer_Edge] = 8;//Store this internal variable  
     _Green_Pointer_Edge = _Green_Pointer_Edge + 1; 
    }
}
void ALGORITHM::_Find_edges_Yellow()//Identify Yellow edges
{
    //Check White side 
    if(CubeMap[0][0][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 2;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][0] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 4;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[0][1][2] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 6;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[0][2][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 8;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    //Check Orange side
    if(CubeMap[1][0][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 2;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][0] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 4;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[1][1][2] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 6;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[1][2][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 8;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    //Check Blue side
    if(CubeMap[2][0][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 2;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][0] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 4;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[2][1][2] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 6;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[2][2][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 8;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    //Check Red side
    if(CubeMap[3][0][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 2;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][0] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 4;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[3][1][2] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 6;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[3][2][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 8;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    //Check Green side
    if(CubeMap[4][0][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 2;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][0] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 4;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[4][1][2] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 6;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[4][2][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 8;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    //Check Yellow side
    if(CubeMap[5][0][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 2;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][0] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 4;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[5][1][2] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 6;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
    if(CubeMap[5][2][1] == Yellow)
    {
     _Yellow_List_Edge[_Yellow_Pointer_Edge] = 8;//Store this internal variable  
     _Yellow_Pointer_Edge = _Yellow_Pointer_Edge + 1; 
    }
}
 
      
//Forward Declaration of the functions
SIDE::SIDE()//Constructor
{
    _Cubelet_1_position = 1;
    _Cubelet_2_position = 2;
    _Cubelet_3_position = 3;
    _Cubelet_4_position = 4;
    _Cubelet_5_position = 5;
    _Cubelet_6_position = 6;
    _Cubelet_7_position = 7;
    _Cubelet_8_position = 8;
    _Cubelet_9_position = 9;
    
    _Cubelet_1_colour = 0;
    _Cubelet_2_colour = 0;
    _Cubelet_3_colour = 0;
    _Cubelet_4_colour = 0;
    _Cubelet_5_colour = 0;
    _Cubelet_6_colour = 0;
    _Cubelet_7_colour = 0;
    _Cubelet_8_colour = 0;
    _Cubelet_9_colour = 0;
}
SIDE::~SIDE()//Destructor
{
    
}


int SIDE::SIDE_type()
{
    return _Cubelet_5_colour;
}
void SIDE::Set_Colours(int CP1,int CP2,int CP3,int CP4,int CP5,int CP6,int CP7,int CP8,int CP9)
{
    _Cubelet_1_colour = CP1;
    _Cubelet_2_colour = CP2;
    _Cubelet_3_colour = CP3;
    _Cubelet_4_colour = CP4;
    _Cubelet_5_colour = CP5;
    _Cubelet_6_colour = CP6;
    _Cubelet_7_colour = CP7;
    _Cubelet_8_colour = CP8;
    _Cubelet_9_colour = CP9;
}