#include "robot.h"
#include <vector>
std::vector<int> states_round1 = {-2, -1, 0, 2, 3, 4, 5},
states_wo0_round1 = {-2, -1, 2, 3, 4, 5},
states_round2 = {-2, -1, 0, 2, 3, 4, 5, 6};
// Iterative Deepening Search (IDS)
// first round: perform IDS from start to finish
// second round: mark shortest path from finish to start and erase states of other visited cells
// third round: mark final shortest path from start to finish and erase all other states
// cell states
// -2 ... boundary cell
// -1 ... obstacle
// 0 ... unvisited cell
// 1 ... shortest path from start to finish / eraser (3rd round)
// states 2, 3, 4, 5: the last direction chosen from this cell (1st round)
// 2 ... west (W)
// 3 ... south (S)
// 4 ... east (E)
// 5 ... north (N)
// 6 ... shortest path from finish to start / eraser (2nd round)
void program_pulibot()
{
/************************ round 1 ***********************/
// start
set_instruction({0, -2, 0, 0, -2}, 4, 'E'); // begin towards east
set_instruction({0, -2, -1, 0, -2}, 4, 'E'); // begin towards east
set_instruction({0, -2, 0, -1, -2}, 3, 'S'); // obstacle on the east; begin towards south
// return to parent from the leaf
// leaf is a cell visited for the first time in IDS
for (auto const &i : states_round1)
{
for (auto const &j : states_round1)
{
for (auto const &k : states_round1)
{
if (!(i == -2 && j == -2 && k == -2)) // impossible position
{
if (i != 5 && j != 2 && k != 3 && !(i == -2 && j == -2)) // finish at i == j == -2
{
set_instruction({0, 4, i, j, k}, 2, 'W');
}
if (i != 4 && j != 2 && k != 3)
{
set_instruction({0, i, 5, j, k}, 3, 'S');
}
if (i != 4 && j != 5 && k != 3)
{
set_instruction({0, i, j, 2, k}, 4, 'E');
}
if (i != 4 && j != 5 && k != 2 && !(j == -2 && k == -2)) // finish at j == k == -2
{
set_instruction({0, i, j, k, 3}, 5, 'N');
}
}
}
}
}
// rotate counterclockwise and continue in selected direction
// IDS: go to the next child or return to parent
// next is yet unvisited,
// or a child (last move was returning from the child to next),
// or a parent (last move was moving from the parent to next)
// rotate by a quarter if possible
// new state = (state - 2 + 1) mod 4 + 2
for (auto const &i : states_round1)
{
for (auto const &j : states_round1)
{
for (auto const &k : states_round1)
{
if (!(i == -2 && j == -2 && k == -2)) // impossible position
{
set_instruction({2, i, 0, j, k}, 3, 'S');
set_instruction({2, i, 5, j, k}, 3, 'S');
set_instruction({3, i, j, 0, k}, 4, 'E');
set_instruction({3, i, j, 2, k}, 4, 'E');
set_instruction({4, i, j, k, 0}, 5, 'N');
set_instruction({4, i, j, k, 3}, 5, 'N');
set_instruction({5, 0, i, j, k}, 2, 'W');
set_instruction({5, 4, i, j, k}, 2, 'W');
}
}
}
}
// rotate by a half if possible
// new state = (state - 2 + 2) mod 4 + 2
for (auto const &i : states_round1)
{
for (auto const &j : states_round1)
{
for (auto const &k : states_wo0_round1)
{
if (!(i == -2 && j == -2 && k == -2)) // impossible position
{
if (k != 5)
{
set_instruction({2, i, k, 0, j}, 4, 'E');
set_instruction({2, i, k, 2, j}, 4, 'E');
}
if (k != 2)
{
set_instruction({3, i, j, k, 0}, 5, 'N');
set_instruction({3, i, j, k, 3}, 5, 'N');
}
if (k != 3)
{
set_instruction({4, 0, i, j, k}, 2, 'W');
set_instruction({4, 4, i, j, k}, 2, 'W');
}
if (k != 4)
{
set_instruction({5, k, 0, j, i}, 3, 'S');
set_instruction({5, k, 5, j, i}, 3, 'S');
}
}
}
}
}
// rotate by 3 quarters if possible
// new state = (state - 2 + 3) mod 4 + 2
for (auto const &i : states_round1)
{
for (auto const &j : states_wo0_round1)
{
for (auto const &k : states_wo0_round1)
{
if (!(i == -2 && j == -2 && k == -2)) // impossible position
{
if (k != 5 && j != 2)
{
set_instruction({2, i, k, j, 0}, 5, 'N');
set_instruction({2, i, k, j, 3}, 5, 'N');
}
if (k != 2 && j != 3)
{
set_instruction({3, 0, i, k, j}, 2, 'W');
set_instruction({3, 4, i, k, j}, 2, 'W');
}
if (k != 3 && j != 4)
{
set_instruction({4, j, 0, i, k}, 3, 'S');
set_instruction({4, j, 5, i, k}, 3, 'S');
}
if (k != 4 && j != 5)
{
set_instruction({5, k, j, 0, i}, 4, 'E');
set_instruction({5, k, j, 2, i}, 4, 'E');
}
}
}
}
}
// dead end; return to parent
for (auto const &i : states_wo0_round1)
{
for (auto const &j : states_wo0_round1)
{
for (auto const &k : states_wo0_round1)
{
if (!(i == -2 && j == -2 && k == -2)) // impossible position
{
if (i != 5 && j != 2 && k != 3)
{
set_instruction({2, 4, i, j, k}, 2, 'W');
}
if (i != 4 && j != 2 && k != 3)
{
set_instruction({3, i, 5, j, k}, 3, 'S');
}
if (i != 4 && j != 5 && k != 3)
{
set_instruction({4, i, j, 2, k}, 4, 'E');
}
if (i != 4 && j != 5 && k != 2)
{
set_instruction({5, i, j, k, 3}, 5, 'N');
}
}
}
}
}
// IDS found finish (1st -> 2nd round)
for (auto i = -1; i <= 5; i++)
{
set_instruction({0, 4, -2, -2, i}, 6, 'W');
if (i != 4)
{
set_instruction({0, i, -2, -2, 3}, 6, 'N');
}
}
/************************ round 2 ***********************/
// return to start and mark shortest path
// from this cell return is possible to exactly one eraser
// return is deterministic (no cycle)
for (auto const &i : states_round2)
{
for (auto const &j : states_round2)
{
for (auto const &k : states_round2)
{
if (((i == 6 && j < 6 && k < 6) ||
(i < 6 && j == 6 && k < 6) ||
(i < 6 && j < 6 && k == 6)) &&
!(i == -2 && j == -2 && k == -2)) // impossible position
{
for (auto state = 2; state <= 6; state++)
{
set_instruction({state, 4, i, j, k}, 6, 'W');
if (i != 4)
{
set_instruction({state, i, 5, j, k}, 6, 'S');
}
if (i != 4 && j != 5)
{
set_instruction({state, i, j, 2, k}, 6, 'E');
}
if (i != 4 && j != 5 && k != 2)
{
set_instruction({state, i, j, k, 3}, 6, 'N');
}
}
}
}
}
}
// from this cell return would be possible to more than one eraser
// break the cycle:
// transfer the processing of this cell to previous adjacent eraser
// and return to current adjacent eraser
for (auto const &i : states_round2)
{
for (auto const &j : states_round2)
{
if (i == 2 || j == 3)
{
set_instruction({2, 6, 6, i, j}, 3, 'W');
set_instruction({3, 6, 6, i, j}, 2, 'S');
}
if (i != 6 && (i == 5 || j == 3))
{
set_instruction({2, 6, i, 6, j}, 4, 'W');
}
if (i != 6 && j != 6 && (i == 5 || j == 2))
{
set_instruction({2, 6, i, j, 6}, 5, 'W');
}
if (i != 6 && (i == 4 || j == 3))
{
set_instruction({3, i, 6, 6, j}, 4, 'S');
set_instruction({4, i, 6, 6, j}, 3, 'E');
}
if (i != 6 && j != 6 && (i == 4 || j == 2))
{
set_instruction({3, i, 6, j, 6}, 5, 'S');
}
if (i == 5 || j == 3)
{
set_instruction({4, 6, i, 6, j}, 2, 'E');
}
if (i != 6 && j != 6 && (i == 4 || j == 5))
{
set_instruction({4, i, j, 6, 6}, 5, 'E');
set_instruction({5, i, j, 6, 6}, 4, 'N');
}
if (i == 5 || j == 2)
{
set_instruction({5, 6, i, j, 6}, 2, 'N');
}
if (i != 6 && (i == 4 || j == 2))
{
set_instruction({5, i, 6, j, 6}, 3, 'N');
}
}
}
// found a leaf; erase state and return to the parent
for (auto const &i : states_round2)
{
for (auto const &j : states_round2)
{
for (auto const &k : states_round2)
{
if (!(i == -2 && j == -2 && k == -2)) // impossible position
{
if (i != 5 && j != 2 && k != 3)
{
set_instruction({2, 6, i, j, k}, 0, 'W');
set_instruction({6, 6, i, j, k}, 0, 'W');
}
if (i != 4 && j != 2 && k != 3 && (i != -2 || k != -2)) // start at i == k == -2
{
set_instruction({3, i, 6, j, k}, 0, 'S');
if (i != 6)
{
set_instruction({6, i, 6, j, k}, 0, 'S');
}
}
if (i != 4 && j != 5 && k != 3 && (i != -2 || k != -2)) // start at i == k == -2
{
set_instruction({4, i, j, 6, k}, 0, 'E');
if (i != 6 && j != 6)
{
set_instruction({6, i, j, 6, k}, 0, 'E');
}
}
if (i != 4 && j != 5 && k != 2)
{
set_instruction({5, i, j, k, 6}, 0, 'N');
if (i != 6 && j != 6 && k != 6)
{
set_instruction({6, i, j, k, 6}, 0, 'N');
}
}
}
}
}
}
// backtracking found start (2nd -> 3rd round)
for (auto const &i : {0, 3, 4})
{
set_instruction({6, -2, i, 6, -2}, 1, 'E');
set_instruction({6, -2, 6, i, -2}, 1, 'S');
}
set_instruction({4, -2, -1, 6, -2}, 1, 'E');
set_instruction({3, -2, 6, -1, -2}, 1, 'S');
/************************ round 3 ***********************/
// no branches to erase
// mark final shortest path and continue towards finish
for (auto i = -2; i <= 6; i++)
{
for (auto j = -2; j <= 6; j++)
{
for (auto k = -2; k <= 6; k++)
{
if (((i == 1 && j != 1 && k != 1) ||
(i != 1 && j == 1 && k != 1) ||
(i != 1 && j != 1 && k == 1)) &&
!(i == -2 && j == -2 && k == -2)) // impossible position
{
if (i != 5 && j != 2 && k != 3 && !(i == -2 && j == -2)) // finish at i == j == -2
{
set_instruction({6, 6, i, j, k}, 1, 'W');
}
if (i != 6)
{
if (i != 4 && j != 2 && k != 3)
{
set_instruction({6, i, 6, j, k}, 1, 'S');
}
if (j != 6)
{
if (i != 4 && j != 5 && k != 3)
{
set_instruction({6, i, j, 6, k}, 1, 'E');
}
if (k != 6)
{
if (i != 4 && j != 5 && k != 2 && !(j == -2 && k == -2)) // finish at j == k == -2
{
set_instruction({6, i, j, k, 6}, 1, 'N');
}
}
}
}
}
}
}
}
// erase branch
for (auto i = -2; i <= 6; i++)
{
for (auto j = -2; j <= 6; j++)
{
for (auto k = -2; k <= 6; k++)
{
if (((i == 6 && j < 6 && k < 6) ||
(i < 6 && j == 6 && k < 6) ||
(i < 6 && j < 6 && k == 6)) &&
((i == 1 && j != 1 && k != 1) ||
(i != 1 && j == 1 && k != 1) ||
(i != 1 && j != 1 && k == 1)) &&
!(i == -2 && j == -2 && k == -2)) // impossible position
{
set_instruction({6, 4, i, j, k}, 6, 'W');
if (i != 4)
{
set_instruction({6, i, 5, j, k}, 6, 'S');
}
if (i != 4 && j != 5)
{
set_instruction({6, i, j, 2, k}, 6, 'E');
}
if (i != 4 && j != 5 && k != 2)
{
set_instruction({6, i, j, k, 3}, 6, 'N');
}
}
}
}
}
for (auto i = -2; i <= 6; i++)
{
for (auto j = -2; j <= 6; j++)
{
for (auto k = -2; k <= 6; k++)
{
if (((i == 1 && j != 1 && k != 1) ||
(i != 1 && j == 1 && k != 1) ||
(i != 1 && j != 1 && k == 1)) &&
!(i == -2 && j == -2 && k == -2)) // impossible position
{
set_instruction({1, 4, i, j, k}, 1, 'W');
if (i != 4)
{
set_instruction({1, i, 5, j, k}, 1, 'S');
}
if (i != 4 && j != 5)
{
set_instruction({1, i, j, 2, k}, 1, 'E');
}
if (i != 4 && j != 5 && k != 2)
{
set_instruction({1, i, j, k, 3}, 1, 'N');
}
}
}
}
}
for (auto i = -2; i <= 5; i++)
{
for (auto j = -2; j <= 5; j++)
{
for (auto k = -2; k <= 5; k++)
{
if (((i == 1 && j != 1 && k != 1) ||
(i != 1 && j == 1 && k != 1) ||
(i != 1 && j != 1 && k == 1)) &&
!(i == -2 && j == -2 && k == -2)) // impossible position
{
for (auto state = 2; state <= 5; state++)
{
set_instruction({state, 4, i, j, k}, 1, 'W');
if (i != 4)
{
set_instruction({state, i, 5, j, k}, 1, 'S');
}
if (i != 4 && j != 5)
{
set_instruction({state, i, j, 2, k}, 1, 'E');
}
if (i != 4 && j != 5 && k != 2)
{
set_instruction({state, i, j, k, 3}, 1, 'N');
}
}
}
}
}
}
for (auto i = -2; i <= 5; i++)
{
for (auto j = -2; j <= 5; j++)
{
if (i == 1 || j == 1)
{
set_instruction({3, 4, 6, i, j}, 1, 'W');
set_instruction({4, 4, i, 6, j}, 1, 'W');
set_instruction({5, 4, i, j, 6}, 1, 'W');
set_instruction({2, 6, 5, i, j}, 1, 'S');
if (i != 4)
{
set_instruction({4, i, 5, 6, j}, 1, 'S');
set_instruction({5, i, 5, j, 6}, 1, 'S');
set_instruction({3, i, 6, 2, j}, 1, 'E');
if (j != 5)
{
set_instruction({5, i, j, 2, 6}, 1, 'E');
set_instruction({4, i, j, 6, 3}, 1, 'N');
}
if (j != 2)
{
set_instruction({3, i, 6, j, 3}, 1, 'N');
}
}
if (i != 5)
{
set_instruction({2, 6, i, 2, j}, 1, 'E');
if (j != 2)
{
set_instruction({2, 6, i, j, 3}, 1, 'N');
}
}
}
}
}
for (auto i = -2; i <= 5; i++)
{
for (auto j = -2; j <= 5; j++)
{
if (i == 2 || j == 3)
{
set_instruction({2, 1, 6, i, j}, 3, 'W');
set_instruction({3, 6, 1, i, j}, 2, 'S');
}
if (i == 5 || j == 3)
{
set_instruction({2, 1, i, 6, j}, 4, 'W');
set_instruction({4, 6, i, 1, j}, 2, 'E');
}
if (i == 5 || j == 2)
{
set_instruction({2, 1, i, j, 6}, 5, 'W');
set_instruction({5, 6, i, j, 1}, 2, 'N');
}
if (i == 4 || j == 3)
{
set_instruction({3, i, 1, 6, j}, 4, 'S');
set_instruction({4, i, 6, 1, j}, 3, 'E');
}
if (i == 4 || j == 2)
{
set_instruction({3, i, 1, j, 6}, 5, 'S');
set_instruction({5, i, 6, j, 1}, 3, 'N');
}
if (i == 4 || j == 5)
{
set_instruction({4, i, j, 1, 6}, 5, 'E');
set_instruction({5, i, j, 6, 1}, 4, 'N');
}
}
}
// found a leaf; erase state and return to the parent
for (auto const &i : states_round2)
{
for (auto const &j : states_round2)
{
for (auto const &k : states_round2)
{
if (!(i == -2 && j == -2 && k == -2)) // impossible position
{
for (auto state = 2; state <= 5; state++)
{
if (i != 5 && j != 2 && k != 3)
{
set_instruction({state, 1, i, j, k}, 0, 'W');
}
if (i != 4 && j != 2 && k != 3)
{
set_instruction({state, i, 1, j, k}, 0, 'S');
}
if (i != 4 && j != 5 && k != 3)
{
set_instruction({state, i, j, 1, k}, 0, 'E');
}
if (i != 4 && j != 5 && k != 2)
{
set_instruction({state, i, j, k, 1}, 0, 'N');
}
}
}
}
}
}
for (auto i = -2; i <= 5; i++)
{
for (auto j = -2; j <= 5; j++)
{
for (auto k = -2; k <= 5; k++)
{
if (((i == 1 && j != 1 && k != 1) ||
(i != 1 && j == 1 && k != 1) ||
(i != 1 && j != 1 && k == 1)) &&
!(i == -2 && j == -2 && k == -2)) // impossible position
{
if (i != 5 && j != 2 && k != 3)
{
set_instruction({1, 6, i, j, k}, 0, 'W');
}
if (i != 6)
{
if (i != 4 && j != 2 && k != 3)
{
set_instruction({1, i, 6, j, k}, 0, 'S');
}
if (j != 6)
{
if (i != 4 && j != 5 && k != 3)
{
set_instruction({1, i, j, 6, k}, 0, 'E');
}
if (k != 6)
{
if (i != 4 && j != 5 && k != 2)
{
set_instruction({1, i, j, k, 6}, 0, 'N');
}
}
}
}
}
}
}
}
for (auto const &i : states_round1)
{
for (auto const &j : states_round1)
{
for (auto const &k : states_round1)
{
if (!(i == -2 && j == -2 && k == -2)) // impossible position
{
if (i != 5 && j != 2 && k != 3)
{
set_instruction({1, 1, i, j, k}, 0, 'W');
}
if (i != 4 && j != 2 && k != 3)
{
set_instruction({1, i, 1, j, k}, 0, 'S');
}
if (i != 4 && j != 5 && k != 3)
{
set_instruction({1, i, j, 1, k}, 0, 'E');
}
if (i != 4 && j != 5 && k != 2)
{
set_instruction({1, i, j, k, 1}, 0, 'N');
}
}
}
}
}
// finish
set_instruction({6, -1, -2, -2, 1}, 1, 'T');
set_instruction({6, 0, -2, -2, 1}, 1, 'T');
set_instruction({6, 1, -2, -2, -1}, 1, 'T');
set_instruction({6, 1, -2, -2, 0}, 1, 'T');
}
