#include <bits/stdc++.h>
#include "robot.h"
using namespace std;
int TO_WEST = 2;
int TO_SOUTH = 3;
int TO_EAST = 4;
int TO_NORTH = 5;
vector<char> Directions = {'_', '_', 'W', 'S', 'E', 'N'};
int Counter(int dir) {
assert(2 <= dir && dir <= 5);
return ((dir - 2) ^ 2) + 2;
}
int Direction_to_pointed_back(vector<int> z) {
for (int adj_state_id = 1; adj_state_id <= 4; adj_state_id++) {
if (z[adj_state_id] < 2) continue;
int point_back_dir = Counter(adj_state_id + 1);
if (z[adj_state_id] == point_back_dir) return adj_state_id + 1;
}
return -1;
}
int Direction_to_pointed_out(vector<int> z) {
for (int adj_state_id = 1; adj_state_id <= 4; adj_state_id++) {
if (z[adj_state_id] < 2) continue;
int point_back_dir = Counter(adj_state_id + 1);
if (z[adj_state_id] != point_back_dir) return adj_state_id + 1;
}
return -1;
}
int which_state(vector<int> z) {
if (z[0] == 0) return 1;
if (z[0] == 1) return 2;
int dir_id = z[0] - 1;
return z[dir_id] == Counter(z[0]) ? 1 : 2;
}
bool is_beg(vector<int> z) {
return z[1] == -2 && z[4] == -2;
}
bool is_goal(vector<int> z) {
return z[3] == -2 && z[2] == -2;
}
pair<int, char> state_one_next_step(vector<int> z) {
assert(which_state(z) == 1);
if (is_goal(z)) {
int pointed_back_direction = Direction_to_pointed_back(z);
if (pointed_back_direction == -1) return make_pair(-1, '_');
return make_pair(3, Directions[pointed_back_direction]);
}
if (z[0] == 0) {
int pointed_back_dir = Direction_to_pointed_back(z);
if (pointed_back_dir != -1) {
int BFS_parent_pointing = pointed_back_dir;
return make_pair(BFS_parent_pointing, Directions[BFS_parent_pointing]);
} else if (is_beg(z)) {
for (int adj_state_id = 1; adj_state_id <= 4; adj_state_id++) if (z[adj_state_id] == 0) return make_pair(adj_state_id + 1, Directions[adj_state_id + 1]);
}
} else if (z[0] >= 2 && z[0] <= 5) {
int pointed_to_adj_state_id = z[0] - 1;
for (int shift_cw_id = 1; shift_cw_id <= 4; shift_cw_id++) {
int nearest_state_id = ((pointed_to_adj_state_id - 1 + shift_cw_id) % 4) + 1;
int state = z[nearest_state_id];
int pointing_direction = nearest_state_id + 1;
if (state == 0 || (state >= 2 && Counter(state) == pointing_direction)) return make_pair(pointing_direction, Directions[pointing_direction]);
}
}
return make_pair(-1, '_');
}
bool state_two_dead_end(vector<int> z) {
if (is_goal(z)) return false;
if (is_beg(z)) return false;
int dir = Direction_to_pointed_back(z);
if (dir != -1) return false;
bool one_min_path_cant_adj_two_skip_nodes = false;
for (int i = 1; i <= 4; i++) one_min_path_cant_adj_two_skip_nodes |= z[i] == 1;
return !one_min_path_cant_adj_two_skip_nodes;
}
pair<int, char> state_two_next_step(vector<int> z) {
assert(which_state(z) == 2);
if (state_two_dead_end(z)) {
if (z[0] >= 2) {
return make_pair(0, Directions[z[0]]);
}
} else if (2 <= z[0] && z[0] <= 5) {
int To_unfinished_direction = Direction_to_pointed_back(z);
if (To_unfinished_direction != -1) return make_pair(z[0], Directions[To_unfinished_direction]);
else {
if (is_goal(z)) return make_pair(1, 'T');
return make_pair(1, Directions[z[0]]);
}
}
return make_pair(-1, '_');
}
void program_pulibot() {
for (int z0 = 0; z0 <= 5; z0++) {
for (int z1 = -2; z1 <= 5; z1++) {
for (int z2 = -2; z2 <= 5; z2++) {
for (int z3 = -2; z3 <= 5; z3++) {
for (int z4 = -2; z4 <= 5; z4++) {
vector<int> z = {z0, z1, z2, z3, z4};
if (which_state(z) == 1) {
auto [st, to] = state_one_next_step(z);
if (st != -1) set_instruction(z, st, to);
} else {
auto [st, to] = state_two_next_step(z);
if (st != -1) set_instruction(z, st, to);
}
}
}
}
}
}
}
