maze runner on ice arduino (in development)

#define map_size 64
#define traceback_buf_size 16
//char traceback_buf[traceback_buf_size];
int freeRam () {
  extern int __heap_start, *__brkval;
  int v;
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}
class robot {



  public:

    char head_dir (char num) {
      while (num < 0)num += 4;
      while (num > 3)num -= 4;
      return num;
    }
    char dx_dir(char _dir) { //cosine
      _dir =  head_dir(_dir);
      if (_dir == 0)return 1;
      if (_dir == 2)return -1;
      return 0;
    }
    char dy_dir(char _dir) { //sine
      _dir =  head_dir(_dir);
      if (_dir == 1)return 1;
      if (_dir == 3)return -1;
      return 0;
    }
    class state {
        char head_dir (char num) {
          while (num < 0)num += 4;
          while (num > 3)num -= 4;
          return num;
        }
      public:
        char pos = 0;
        char head = 0;//initial heading
        char front() {
          return head;
        }
        char left() {
          return head_dir(head + 1);
        }
        char right() {
          return head_dir(head - 1);
        }
        char back() {
          return head_dir(head + 2);
        }
        bool operator==(state &comp) {
          if (pos == comp.pos && head == comp.head)return true;
          else return false;
        }
        bool operator!=(state &comp) {
          if (pos == comp.pos && head == comp.head)return false;
          else return true;
        }

    };

    state crr_state;

    class maping {
      public:

        class map_node {
          public:
            char x = -128;
            char y = -128;

            char connect[4] =  { -1, -1, -1, -1}; // -1 refer to null
            unsigned int weight[4] =  {65535, 65535, 65535, 65535};
            void weight_reset() {
              for (int i = 0; i < 4; i++) {
                weight [i] = 65535;
              }
            }
            bool is_unvisited() {
              return connect[0] == -1 || connect[1] == -1 || connect[2] == -1 || connect[3] == -1;
            }

        };

        char maplength = 0;

        map_node map_arr[map_size];

        
        char length () {
          int size_all = sizeof(map_arr);
          int size_each = sizeof(map_node);
          return (size_all / size_each);
        }
        char _new_ () {
          char new_node = maplength;
          maplength++;

          return new_node;
        }
        char find_xy (char _x, char _y) {
          for (char i = 0; i < map_size; i++) {
            if (map_arr[i].x == _x && map_arr[i].y == _y) {
              return i;
            }
          }
          return -1;
        }

    };

    maping mazemap;


    char occ[4] = { -1, -1, -1, -1}; //right,up,left,down
    char gfocc() {//put front sensor read hear // return 0 = clear ,return 1 = occupied
      Serial.print('F');//demo for simulator
      while (!Serial.available());
      char gdat = Serial.read();//get data
      return gdat == '1';
    }
    char glocc() {
      Serial.print('L');
      while (!Serial.available());
      char gdat = Serial.read();//get data
      return gdat == '1';
    }
    char grocc() {
      Serial.print('R');
      while (!Serial.available());
      char gdat = Serial.read();//get data
      return gdat == '1';
    }



    void action (char cmd) {
    
      if (cmd == 'W') {
        crr_state.pos = mazemap.map_arr[crr_state.pos].connect[crr_state.head];
      }
      else  if (cmd == 'A') {
        crr_state.head = crr_state.left();
      }
      else  if (cmd == 'D') {
        crr_state.head = crr_state.right();
      }
     else{
      Serial.println("err in action");
     }
     Serial.print(cmd);
    }
    state new_state(char _pos, char _head) {
      state result;
      result.pos = _pos;
      result.head = _head;
      return result;

    }
    state action_result (char cmd, state _crr) {
      if (cmd == 'W') {
        _crr.pos = mazemap.map_arr[_crr.pos].connect[_crr.front()];
      }
      else  if (cmd == 'A') {
        _crr.head = _crr.left();
      }
      else  if (cmd == 'D') {
        _crr.head = _crr.right();
      }

      return _crr;
    }
    state back_tracking_result (char cmd, state _crr) {//invert from action result
      if (cmd == 'W') {
        _crr.pos = mazemap.map_arr[_crr.pos].connect[_crr.back()];
      }
      else  if (cmd == 'D') {
        _crr.head = _crr.left();
      }
      else  if (cmd == 'A') {
        _crr.head = _crr.right();
      }

      return _crr;
    }
    char get_action(state prov, state next) {
      if (action_result('W', prov) == next)return 'W';
      else if (action_result('A', prov) == next)return 'A';
      else if (action_result('D', prov) == next)return 'D';
      else return '\0';
    }
    void tl() {
     // Serial.print('A');
      action('A');
    }
    void tr() {
      //Serial.print('D');
      action('D');
    }
    void gh() {
     // Serial.print('W');
      action('W');

    }


    void occ_update() {
      for (char i = 0; i < 4; i++) {
        occ[i] = -1;
      }
      occ[crr_state.front()] = gfocc();
      occ[crr_state.right()] = grocc();
      occ[crr_state.left()] = glocc();
    }

    char f_occ () {
      return  occ[crr_state.front()];
    }
    char l_occ () {
      return occ[crr_state.left()];
    }
    char r_occ () {
      return occ[crr_state.right()];
    }
    void printocc() {
      for (char i = 0; i < 4; i++) {
        Serial.print(int(occ[i]));
        if (i < 3)  Serial.print(",");
      }
      Serial.println();
    }
    void map_update() {
      //Serial.println("\nmap update.. at node " + String(int(crr_state.pos)));
      occ_update();
      for (char i = 0; i < 4; i++) {
        if (occ[i] != -1) {
          char new_x = mazemap.map_arr[crr_state.pos].x + dx_dir(i);
          char new_y = mazemap.map_arr[crr_state.pos].y + dy_dir(i);
          if (occ[i] == 1) { //occupied
            mazemap.map_arr[crr_state.pos].connect[i] = crr_state.pos;
          }
          else if (occ[i] == 0 && mazemap.map_arr[crr_state.pos].connect[i] == -1 ) {
            char new_node;
            if ( mazemap.find_xy(new_x, new_y) == -1) {

              new_node = mazemap._new_();
            }
            else new_node = mazemap.find_xy(new_x, new_y);
            mazemap.map_arr[crr_state.pos].connect[i] = new_node;
            mazemap.map_arr[new_node].connect[head_dir(i + 2)] = crr_state.pos;
            mazemap.map_arr[new_node].x = new_x;
            mazemap.map_arr[new_node].y = new_y;
            Serial.println("create node " + String(int(new_node)) + " , x = " + String(int(new_x)) + " , y = " + String(int(new_y)));


          }
          else {
            //Serial.println("We have some Error here!!");
          }
        }
      }
    }
    void startup() {
      crr_state.pos = 0;//init
      crr_state.head = 0;
      char new_node = mazemap._new_ ();
      mazemap.map_arr[crr_state.pos].x = 0;
      mazemap.map_arr[crr_state.pos].y = 0;

      map_update();
      if (glocc() == '0') { //left is clear
        tl();
      }
      else tr();

      map_update();

    }
    unsigned int get_weight_state(state _crr) {
      return mazemap.map_arr[_crr.pos].weight[_crr.head];
    }
    void set_weight_state(state _crr, unsigned int _weight) {
      mazemap.map_arr[_crr.pos].weight[_crr.head] = _weight;
    }
    void run() {
      const unsigned int W_weight = 5;
      const unsigned int A_weight = 3;
      const unsigned int D_weight = 3;
      //Serial.println("\nrun");
      map_update();
      
        Serial.println("now on node = " + String(int(crr_state.pos)) + " , x = " + String(int(mazemap.map_arr[crr_state.pos].x)) + " ,y = " + String(int(mazemap.map_arr[crr_state.pos].y)));
        //Serial.print("that connect to");
        //for (char i = 0; i < 4; i++) {
        //Serial.print((int) mazemap.map_arr[crr_state.pos].connect[i]);
        //if (i < 3)Serial.print(",");
        //}
        //Serial.println();
      
      if (mazemap.map_arr[mazemap.map_arr[crr_state.pos].connect[crr_state.front()]].is_unvisited()  ) {
        /*
          Serial.print("on node = " + String( (int)mazemap.map_arr[crr_state.pos].connect[crr_state.front()]) + " , we found ");
          for (char i = 0; i < 4; i++) {
          Serial.print((int)mazemap.map_arr[  mazemap.map_arr[crr_state.pos].connect[crr_state.front()]].connect[i]);
          if (i < 3)Serial.print(",");
          }
          Serial.print(" and ");
          Serial.println((int)mazemap.map_arr[mazemap.map_arr[crr_state.pos].connect[crr_state.front()]].is_unvisited());
        */
        gh();

      }
      else if (mazemap.map_arr[mazemap.map_arr[crr_state.pos].connect[crr_state.left()]].is_unvisited() ) {
        tl();
      }
      else if (mazemap.map_arr[mazemap.map_arr[crr_state.pos].connect[crr_state.right()]].is_unvisited() ) {
        tr();
      }

      else {
        Serial.println("start searching!!");
        state goal_state = new_state(-1, 0);
        char unvisited = 0;
        for (char i = 0; i < mazemap.maplength; i++ ) {
          mazemap.map_arr[i].weight_reset();
          if(mazemap.map_arr[i].is_unvisited())unvisited++;
         
        }
        Serial.println("found "+String(int(unvisited))+" unvisited nodes");
        if(unvisited == 0){
          Serial.println("exploration complete, stopprogram");
          while(1);
        }
        mazemap.map_arr[crr_state.pos].weight[crr_state.head] = 0;

        unsigned int base_weight = 0;
        ///some loop here
        bool search_loop = true;
        while (1) {
          //Serial.println("start serch at weight = " + String(base_weight));

          for (char _pos = 0; _pos < mazemap.maplength; _pos++ ) {
            if (goal_state.pos != -1)break;
            for (char _head = 0; _head < 4; _head++) {
              state base_state = new_state(_pos, _head);
              //Serial.println("searching at state ,pos = "+String(int(_pos)) + " ,head = "+String(int(_head))+ " ,weight = "+String(get_weight_state(base_state)));
              if (get_weight_state(base_state) == base_weight) {

                //Serial.println("found base weight = " + String(get_weight_state(base_state)) + "base state , node = " + String(int(base_state.pos)) + " , head = " + String(int(base_state.head)));
                state w_state = action_result ('W', base_state) ;
                state a_state = action_result ('A', base_state) ;
                state d_state = action_result ('D', base_state) ;

                if (W_weight + base_weight < get_weight_state(w_state)) {
                  set_weight_state(w_state, W_weight + base_weight);
                  if (mazemap.map_arr[w_state.pos].is_unvisited()) {
                    goal_state = w_state;
                    break;
                  }
                }
                if (A_weight + base_weight < get_weight_state(a_state)) {
                  set_weight_state(a_state, A_weight + base_weight);
                  if (mazemap.map_arr[a_state.pos].is_unvisited()) {
                    goal_state = a_state;
                    break;
                  }
                }
                if (D_weight + base_weight < get_weight_state(d_state)) {
                  set_weight_state(d_state, D_weight + base_weight);
                  if (mazemap.map_arr[d_state.pos].is_unvisited()) {
                    goal_state = d_state;
                    break;
                  }
                }

              }//"if (mazemap.map_arr[_pos].weight[_head] == base_weight)" 's statement

            }
          }//find match weight with base weight statement
          if (goal_state.pos == -1) {
            unsigned int minimum_weight = 65535;
            
            for (char _pos = 0; _pos < mazemap.maplength; _pos++ ) {

              for (char _head = 0; _head < 4; _head++) {
                state base_state = new_state(_pos, _head);
                unsigned int _weight = get_weight_state(base_state) ;
                if (_weight > base_weight && _weight < minimum_weight) {
                   //Serial.println("debug : update state, pos = "+String(int(base_state.pos))+" head = "+String(int(base_state.head)));
                  minimum_weight = _weight;
                }

              }
            }
              Serial.println("debug : minimum weight = "+String(minimum_weight));
            if (minimum_weight == base_weight) {
              Serial.println("\n!!!!!!stop!!!!!!");
              while (1);
            }
            else if(minimum_weight == 65535){
              Serial.println("stop, exploration complete!");
               while (1);
            }
            else {
              base_weight = minimum_weight;
            }
          }
          else {//next pos != -1 , found next pos to go.

            //Serial.println("\nwholaa ,we found next target at node = " + String(int( goal_state.pos)) + " , head = " + String(int(goal_state.head)) + " , at weight = " + String(base_weight));
            //begin backtracking
            Serial.println("ram:" + String(freeRam()));
            char  traceback_buf_index = 0;
            char traceback_buf[traceback_buf_size];
            Serial.println("ram:" + String(freeRam()));
            for (char i = 0; i < traceback_buf_size; i++) {
              traceback_buf[i] = '\0';
            }

            state backtracking = goal_state;
            //Serial.println("back-tracking start ,node = " + String(int(backtracking.pos)) +" ,head =  "+ String(int(backtracking.head)));
            while ( backtracking != crr_state) {
              state b_w = back_tracking_result('W', backtracking);
              state b_a = back_tracking_result('A', backtracking);
              state b_d = back_tracking_result('D', backtracking);
              unsigned int w_b_w = get_weight_state(b_w);
              unsigned int w_b_a = get_weight_state(b_a);
              unsigned int w_b_d = get_weight_state(b_d);

              if ( w_b_w <= w_b_a && w_b_w <= w_b_d) {
                backtracking = b_w;
                traceback_buf[traceback_buf_index] = 'W';
              }
              else if (w_b_a <= w_b_d) {
                backtracking = b_a;
                traceback_buf[traceback_buf_index] = 'A';
              }
              else {
                backtracking = b_d;
                traceback_buf[traceback_buf_index] = 'D';
              }
             // Serial.println("back-tracking node = " + String(int(backtracking.pos)) +" ,head =  "+ String(int(backtracking.head)));
              //Serial.println("back-tracking cmd = "+ String(char(traceback_buf[traceback_buf_index]+32)));
              traceback_buf_index ++;
              if (traceback_buf_index >= traceback_buf_size)traceback_buf_index -= traceback_buf_size;
              
            }
            
            for (char i = 0; i < traceback_buf_size; i++) {
              traceback_buf_index--;
              if (traceback_buf_index <0)traceback_buf_index += traceback_buf_size;
              if(traceback_buf[traceback_buf_index]!= '\0'){
             
                action(traceback_buf[traceback_buf_index]);
       
                delay(200);
              }
              else break;
            }
           // map_update();
            break;

            //Serial.println("temporary stop!! 000");

            //while (1);
          }

        }//while(true) statement
      }


    }
};

robot mazerunner;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("robot start!");
  Serial.println(sizeof(robot));
  while (!Serial.available());
  Serial.read();
  mazerunner.startup();
}

void loop() {
  // put your main code here, to run repeatedly:
  mazerunner.run();
}