#include "robot.h"
#include <iostream>
#include <vector>
#include <algorithm>
#include <map>
using namespace std;
#define ll int
#define sz(x) (ll)x.size()
#define MID ((l+r)/2)
#define pb push_back
#define dbg(x) cout<<#x<<": "<<x<<endl;
typedef vector <ll> vi;
typedef pair <ll,ll> ii;
typedef vector <ii> vii;
void printVct(vi &v){
for (ll i=0; i<sz(v); i++){
cout<<v[i]<<" ";
}
cout<<endl;
}
/*
-2: Edge
-1: Wall
0: Empty
1: Final Correct
2: Initial Correct
3: Up
4: Right
5: Down
6: Left
*/
#define N 3
#define E 4
#define S 5
#define W 6
// ll mpri[4];
map <ll, char> mp;
// map <ll,char> mpr;
void init_map(){
mp[N] = 'N';
mp[E] = 'E';
mp[S] = 'S';
mp[W] = 'W';
// mpr[N] = 'S';
// mpr[E] = 'W';
// mpr[S] = 'N';
// mpr[W] = 'E';
// mpi[N] = S;
// mpi[E] = W;
// mpi[S] = N;
// mpi[W] = E;
}
ll i,j,k,z,c;
void goToAnyNeighbourWithColor(ll color){
if (i == 2){
set_instruction({c, i, j, k, z}, W, 'W');
}
else if (j == 2){
set_instruction({c, i, j, k, z}, S, 'S');
}
else if (k == 2){
set_instruction({c, i, j, k, z}, E, 'E');
}
else if (z == 2){
set_instruction({c, i, j, k, z}, N, 'N');
}
}
bool goToNeighbourColorPriority(vi p, vi setColor){
for (ll x = 0; x<sz(p); x++){
if (i == p[x]){
set_instruction({c, i, j, k, z}, setColor[x], 'W');
}
else if (j == p[x]){
set_instruction({c, i, j, k, z}, setColor[x], 'S');
}
else if (k == p[x]){
set_instruction({c, i, j, k, z}, setColor[x], 'E');
}
else if (z == p[x]){
set_instruction({c, i, j, k, z}, setColor[x], 'N');
}
else{
continue;
}
return true;
}
return false;
}
/*
if there is neighbour with correct colour, go
else go back to the square
*/
bool goToDirection(ll color, bool goAnyway){
if (i == E){
set_instruction({c,i,j,k,z}, color, 'W');
}
else if (j == N){
set_instruction({c,i,j,k,z}, color, 'S');
}
else if (k == W){
set_instruction({c,i,j,k,z}, color, 'E');
}
else if (z == S){
set_instruction({c,i,j,k,z}, color, 'N');
}
else{
if (goAnyway)
set_instruction({c,i,j,k,z}, 0, mp[color]);
return false;
}
return true;
}
void program_pulibot()
{
init_map();
for (c = 0; c<=6; c++){
for (i = -2; i<=6; i++){
for (j =-2; j<=6; j++){
for (k = -2; k<=6; k++){
for (z = -2; z<=6; z++){
if (c >= 3){
goToDirection(c, true);
}
else if (i == 1 || j == 1 || k == 1 || z == 1){
if (i == -2 && z == -2){
if (!goToNeighbourColorPriority({3,4,5,6}, {2,2,2,2}))
set_instruction({c, i, j, k, z}, 1, 'T');
}
else{
if (!goToDirection(c, false)){
goToNeighbourColorPriority({2,1}, {1,0});
}
// goToNeighbourColorPriority({6,5,4,3,2,1}, {1,1,1,1,1,0})
// goToNeighbourColorPriority({3,2,1}, {1,1,0});
}
}
else{
if (k == -2 && j == -2){
goToNeighbourColorPriority({2}, {1});
}
else if (k == 0){
set_instruction({c, i, j, k, z}, 2, 'E');
}
else if (j == 0){
set_instruction({c, i, j, k, z}, 2, 'S');
}
else{
goToAnyNeighbourWithColor(2);
}
}
}
}
}
}
}
}
