Decision_Making_Wang.cpp

int solver()
 {
   // use history to record each behavior (just for debugginng)

     // initialize = 0; left = 1; right = 2; turn_back = 3;

     // Add two more integers to vector

     // when it is not turning

     /*------------------------New Codes Now!----------------------------*/

     /// get the history size and determine which side to follow

     history_size = history.size();
    if(openspace) openspace_counter++;
     if ((history_size <= 20)&&(openspace_counter<6))
        {
         if (((history_size / 4) % 2) == 0)
         {
              WallFollower = RIGHT;
         }

         else if (((history_size / 4) % 2) == 1) {

              WallFollower = LEFT;

         }

         else std::cout << "sth spacial happened in wall follower counter, Hilf" << std::endl;

     }

     else

     {

         if (forwarddetect) {

              direction = NONE;

              WallFollower = NONE;

         }

         else {
                  srand(time(0));

                  WallFollower = random_pool[rand() % 10 / 2];
         }

         std::cout << "I will try to take a random walk" << std::endl;

     }

     if (turndetect) {

         if (WallFollower == LEFT) {

              std::cout << "Left Follower" << '\n';
              if((rightturndetect&&!leftturndetect&&!forwarddetect)||(!rightturndetect&&leftturndetect&&!forwarddetect)||(!rightturndetect&&!leftturndetect&&forwarddetect))
              {
                          route_flag =0;

                          std::cout<<"Pico follow the route ->from LEFT"<<std::endl;
              }
              else{
              if (leftturndetect) route_flag = 4;

              else if (forwarddetect) route_flag = 5;

              else if (rightturndetect) route_flag = 6;

              else if (doordetect) route_flag = 7;
                else std::cout << "Conflicts found" << '\n';

                    }

              //    else if((leftturndetect&&!forwarddetect&&!rightturndetect)||(!leftturndetect&&!forwarddetect&&rightturndetect)){

              //        route_flag ==10;

              //        std::cout<<"Pico follow the route "<<std::endl;

              //    }

                  if (route_flag == 4) // when left route available

                  {

                      direction = LEFT; // Here assume that motion include go ahead, rotate, go ahead again

                                       //Thus, after turning the route flag can be changed
                      if(initial_history == 0){

                      history.push_back(LEFT);
                      initial_history =1;
            }


                      std::cout << "Pico turns left" << '\n';

                  }

                  else if (route_flag == 5)// when straight route available

                  {

                      direction = STRAIGHT;

                  }

                  //          std::cout << "Pico goes straight" << '\n';

                  else if (route_flag == 6)// when right route available

                  {

                      direction = RIGHT;
            if(initial_history == 0){
                      history.push_back(RIGHT);
                      initial_history =1;
                        }


                      std::cout << "Pico turns right" << '\n';

                  }

                  else if (route_flag == 7)// dead end, need to turn back

                  {

                      direction = RETURN; // The alarm function shoule be imbedded into turn_back
                    if(initial_history ==0){
                      history.push_back(RETURN);
                        initial_history=1;
                    }


                      std::cout << "Pico turns back from LEFT" << '\n';

                  }

                  else{
                      direction = NONE;
                      std::cout << "Pico follow the route from->LEFT" << '\n';
                  }

         }

         else if (WallFollower == RIGHT) {

              std::cout << "Right Follower" << '\n';
        if((rightturndetect&&!leftturndetect&&!forwarddetect)||(!rightturndetect&&leftturndetect&&!forwarddetect)||(!rightturndetect&&!leftturndetect&&forwarddetect))
        {
                    route_flag =20;

                    std::cout<<"Pico follow the route "<<std::endl;
        }
        else{
        if (rightturndetect) route_flag = 8;

              else if (forwarddetect) route_flag = 9;

              else if (leftturndetect) route_flag = 10;

              else if (doordetect) route_flag = 11;
         else std::cout << "Conflicts found" << '\n';
        }


              //    else if((leftturndetect&&!forwarddetect&&!rightturndetect)||(!leftturndetect&&!forwarddetect&&rightturndetect)){

              //        route_flag ==10;

              //        std::cout<<"Pico follow the route "<<std::endl;

              //    }

              if (route_flag == 10) // when only left route available

              {

                  direction = LEFT; // Here assume that motion include go ahead, rotate, go ahead again

                                     //Thus, after turning the route flag can be changed
            if(initial_history == 0){
                  history.push_back(LEFT);
                  initial_history =1;
            }

                  std::cout << "Pico turns the only left" << '\n';

              }

              else if (route_flag == 9)// when straight route available

              {

                  direction = STRAIGHT;

              }

              //          std::cout << "Pico goes straight" << '\n';

              else if (route_flag == 8)// when right route available

              {

                  direction = RIGHT;
            if(initial_history == 0){
                  history.push_back(RIGHT);
                initial_history =1;
            }

                  std::cout << "Pico turns right" << '\n';

              }

              else if (route_flag == 11)// dead end, need to turn back

              {

                  direction = RETURN; // The alarm function shoule be imbedded into turn_back
            if(initial_history == 0){
                  history.push_back(RETURN);
                initial_history =1;
                                     }

                  std::cout << "Pico go back From RIGHT" << '\n';

              }

              else {direction = NONE;
                std::cout << "I will follow the line" << '\n';}

         }

     }

     else
     {
       if(doordetect)direction = RETURN;
        else direction = NONE;



         std::cout << "direction is none" << std::endl;

     }

     return direction;