<commit_before>#ifndef DEDICATED_ONLY
#include "player_input.h"

#include "gusgame.h"
#include "CWormHuman.h"
#include "gconsole.h"
#include "LuaCallbacks.h"
#include "luaapi/context.h"
#include "util/text.h"
#include "util/log.h"
#include "util/stringbuild.h"
#include "game/Game.h"
#include "CClientNetEngine.h"
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>

#include <string>
#include <list>
#include <vector>
#include <assert.h>
#include "StaticAssert.h"

static_assert( C_LocalPlayer_ActionCount == CWormHumanInputHandler::ACTION_COUNT, actioncount_mismatch );

using namespace std;

std::string _event(size_t index, CWormHumanInputHandler::Actions action, bool start) {
	using namespace boost::lambda;

	assert((int)action >= 0 && (int)action < CWormHumanInputHandler::ACTION_COUNT);

	if ( index < game.localPlayers.size() )
	{
		CWormHumanInputHandler& player = *game.localPlayers[index];

		bool ignore = false;

		if(!player.worm()->bWeaponsReady)
			ignore = true;

		LuaCallbackProxy::PostHandler f =
				(var(ignore) |= (_2 > 0 && bind(&LuaContext::tobool, _1, -1)));

		LUACALLBACK(localplayerEvent+action).call(1, f)(player.getLuaReference())(start)();
		LUACALLBACK(localplayerEventAny).call(1, f)(player.getLuaReference())((int)action)(start)();

		if(!ignore) {
			if(start)
				player.actionStart(action);
			else
				player.actionStop(action);
		}
	}
	return "";
}

std::string eventStart(size_t index, CWormHumanInputHandler::Actions action, std::list<std::string> const&)
{
	return _event(index, action, true);
}

std::string eventStop(size_t index, CWormHumanInputHandler::Actions action, std::list<std::string> const&)
{
	return _event(index, action, false);
}

void registerPlayerInput()
{
	using namespace boost::lambda;

	for ( size_t i = 0; i < GusGame::MAX_LOCAL_PLAYERS; ++i)
	{
		static char const* actionNames[] =
		{
			"_LEFT", "_RIGHT", "_UP", "_DOWN", "_FIRE", "_JUMP", "_CHANGE", "_NINJAROPE"
		};
		static_assert( sizeof(actionNames)/sizeof(char*) == CWormHumanInputHandler::ACTION_COUNT, actioncount_mismatch );

		for(int action = CWormHumanInputHandler::LEFT; action < CWormHumanInputHandler::ACTION_COUNT; ++action)
		{
			console.registerCommands()
				((S_("+P") << i << actionNames[action]), bind(eventStart, i, (CWormHumanInputHandler::Actions)action, _1))
				((S_("-P") << i << actionNames[action]), bind(eventStop, i, (CWormHumanInputHandler::Actions)action, _1))
			;
		}
	}
	console.registerCommands()
		("SAY", say);
}

string say( const list<string> &args )
{

	if ( !args.empty() )
	{
		if ( !game.localPlayers.empty() )
		{
			CWorm* w = game.localPlayers[0]->worm();
			if(w)
				cClient->getNetEngine()->SendText(*(args.begin()), w->getName());
		}
	}
	else
	{
		return "SAY <MESSAGE> : SENDS A MESSAGE TO THE OTHER PLAYERS ON THE SERVER";
	}
	return "";
}

#endif
<commit_msg>MSVC fix. this was annoying. see <http://superuser.com/questions/406362/install-win7-sp1-with-bcdedit-failing><commit_after>#ifndef DEDICATED_ONLY
#include "player_input.h"

#include "gusgame.h"
#include "CWormHuman.h"
#include "gconsole.h"
#include "LuaCallbacks.h"
#include "luaapi/context.h"
#include "util/text.h"
#include "util/log.h"
#include "util/stringbuild.h"
#include "game/Game.h"
#include "CClientNetEngine.h"
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>

#include <string>
#include <list>
#include <vector>
#include <assert.h>
#include "StaticAssert.h"

static_assert( C_LocalPlayer_ActionCount == CWormHumanInputHandler::ACTION_COUNT, actioncount_mismatch );

std::string _event(size_t index, CWormHumanInputHandler::Actions action, bool start) {
	using namespace boost::lambda;

	assert((int)action >= 0 && (int)action < CWormHumanInputHandler::ACTION_COUNT);

	if ( index < game.localPlayers.size() )
	{
		CWormHumanInputHandler& player = *game.localPlayers[index];

		bool ignore = false;

		if(!player.worm()->bWeaponsReady)
			ignore = true;

		LuaCallbackProxy::PostHandler f =
				(var(ignore) |= (_2 > 0 && bind(&LuaContext::tobool, _1, -1)));

		LUACALLBACK(localplayerEvent+action).call(1, f)(player.getLuaReference())(start)();
		LUACALLBACK(localplayerEventAny).call(1, f)(player.getLuaReference())((int)action)(start)();

		if(!ignore) {
			if(start)
				player.actionStart(action);
			else
				player.actionStop(action);
		}
	}
	return "";
}

std::string eventStart(size_t index, CWormHumanInputHandler::Actions action, std::list<std::string> const&)
{
	return _event(index, action, true);
}

std::string eventStop(size_t index, CWormHumanInputHandler::Actions action, std::list<std::string> const&)
{
	return _event(index, action, false);
}

void registerPlayerInput()
{
	using namespace boost::lambda;

	for ( size_t i = 0; i < GusGame::MAX_LOCAL_PLAYERS; ++i)
	{
		static char const* actionNames[] =
		{
			"_LEFT", "_RIGHT", "_UP", "_DOWN", "_FIRE", "_JUMP", "_CHANGE", "_NINJAROPE"
		};
		static_assert( sizeof(actionNames)/sizeof(char*) == CWormHumanInputHandler::ACTION_COUNT, actioncount_mismatch );

		for(int action = CWormHumanInputHandler::LEFT; action < CWormHumanInputHandler::ACTION_COUNT; ++action)
		{
			console.registerCommands()
				((S_("+P") << i << actionNames[action]), boost::lambda::bind(&eventStart, i, (CWormHumanInputHandler::Actions)action, _1))
				((S_("-P") << i << actionNames[action]), boost::lambda::bind(&eventStop, i, (CWormHumanInputHandler::Actions)action, _1))
			;
		}
	}
	console.registerCommands()
		("SAY", say);
}

std::string say( const std::list<std::string> &args )
{

	if ( !args.empty() )
	{
		if ( !game.localPlayers.empty() )
		{
			CWorm* w = game.localPlayers[0]->worm();
			if(w)
				cClient->getNetEngine()->SendText(*(args.begin()), w->getName());
		}
	}
	else
	{
		return "SAY <MESSAGE> : SENDS A MESSAGE TO THE OTHER PLAYERS ON THE SERVER";
	}
	return "";
}

#endif
<|endoftext|>


<commit_before>#include <iostream>
#include <vector>
#include <HFO.hpp>
#include <cstdlib>
#include <thread>
#include "SarsaAgent.h"
#include "CMAC.h"
#include <unistd.h>

// Before running this program, first Start HFO server:
// $./bin/HFO --offense-agents numAgents

void printUsage() {
  std::cout<<"Usage:123 ./high_level_sarsa_agent [Options]"<<std::endl;
  std::cout<<"Options:"<<std::endl;
  std::cout<<"  --numAgents <int>        Number of SARSA agents"<<std::endl;
  std::cout<<"                           Default: 0"<<std::endl;
  std::cout<<"  --numEpisodes <int>      Number of episodes to run"<<std::endl;
  std::cout<<"                           Default: 10"<<std::endl;
  std::cout<<"  --basePort <int>         SARSA agent base port"<<std::endl;
  std::cout<<"                           Default: 6000"<<std::endl;
  std::cout<<"  --learnRate <float>      Learning rate of SARSA agents"<<std::endl;
  std::cout<<"                           Range: [0.0, 1.0]"<<std::endl;
  std::cout<<"                           Default: 0.1"<<std::endl;
  std::cout<<"  --suffix <int>           Suffix for weights files"<<std::endl;
  std::cout<<"                           Default: 0"<<std::endl;
  std::cout<<"  --noOpponent             Sets opponent present flag to false"<<std::endl;
  std::cout<<"  --step                   Sets the persistent step size"<<std::endl;
  std::cout<<"  --eps                    Sets the exploration rate"<<std::endl;
  std::cout<<"  --numOpponents           Sets the number of opponents"<<std::endl;
  std::cout<<"  --weightId               Sets the given Id for weight File"<<std::endl;
  std::cout<<"  --help                   Displays this help and exit"<<std::endl;
}

// Returns the reward for SARSA based on current state
double getReward(hfo::status_t status) {
  double reward;
  if (status==hfo::GOAL) reward = -1;
  else if (status==hfo::CAPTURED_BY_DEFENSE) reward = 1;
  else if (status==hfo::OUT_OF_BOUNDS) reward = 1;
  else reward = 0;
  return reward;
}

// Fill state with only the required features from state_vec
void purgeFeatures(double *state, const std::vector<float>& state_vec,
                   int numTMates, int numOpponents, bool oppPres) {

  int stateIndex = 0;

  // If no opponents ignore features Distance to Opponent
  // and Distance from Teammate i to Opponent are absent
  int tmpIndex = oppPres ? (9 + 3 * numTMates) : (9 + 2 * numTMates);

  for(int i = 0; i < state_vec.size(); i++) {

    // Ignore first six featues
    if(i == 5 || i==8) continue;
    if(i>9 && i<= 9+numTMates) continue; // Ignore Goal Opening angles, as invalid
    if(i<= 9+3*numTMates && i > 9+2*numTMates) continue; // Ignore Pass Opening angles, as invalid
    // Ignore Uniform Number of Teammates and opponents
    int temp =  i-tmpIndex;
    if(temp > 0 && (temp % 3 == 0) )continue;
    //if (i > 9+6*numTMates) continue;
    state[stateIndex] = state_vec[i];
    stateIndex++;
  }
}

// Convert int to hfo::Action
hfo::action_t toAction(int action, const std::vector<float>& state_vec) {
hfo::action_t a;
  switch (action) {
        case 0: a = hfo::MOVE; break;
        case 1: a = hfo::REDUCE_ANGLE_TO_GOAL; break;
        case 2: a = hfo::GO_TO_BALL; break;
        case 3: a = hfo::NOOP; break;
        case 4: a = hfo::DEFEND_GOAL; break;
        default : a = hfo::MARK_PLAYER; break;
  }
    return a;
}

void offenseAgent(int port, int numTMates, int numOpponents, int numEpi, double learnR,
                  int suffix, bool oppPres, double eps, int step, std::string weightid) {

  // Number of features
  int numF = oppPres ? (8 + 3 * numTMates + 2*numOpponents) : (3 + 3 * numTMates);
  // Number of actions
  int numA = 5 + numOpponents; //DEF_GOAL+MOVE+GTB+NOOP+RATG+MP(unum)

  // Other SARSA parameters
  eps = 0.01;
  double discFac = 1;
  double lambda=0.9375;
  // Tile coding parameter
  double resolution = 0.1;

  double range[numF];
  double min[numF];
  double res[numF];
  for(int i = 0; i < numF; i++) {
      min[i] = -1;
      range[i] = 2;
      res[i] = resolution;
  }

  // Weights file
  char *wtFile;
  std::string s = "weights_" + std::to_string(port) +
                  "_" + std::to_string(numTMates + 1) +
                  "_" + std::to_string(suffix) +
                  "_" + std::to_string(step) +
                   "_" + weightid;
  wtFile = &s[0u];

  CMAC *fa = new CMAC(numF, numA, range, min, res);
  SarsaAgent *sa = new SarsaAgent(numF, numA, learnR, eps, lambda, fa, wtFile, wtFile);

  hfo::HFOEnvironment hfo;
  hfo::status_t status;
  hfo::action_t a;
  double state[numF];
  int action = -1;
  double reward;
  int no_of_offense = numTMates + 1;
  hfo.connectToServer(hfo::HIGH_LEVEL_FEATURE_SET,"../../bin/teams/base/config/formations-dt",port,"localhost","base_right",false,"");

  for (int episode=0; episode < numEpi; episode++) {
    int count = 0;
    status = hfo::IN_GAME;
    action = -1;
    int count_steps = 0;
    double unum = -1;
    const std::vector<float>& state_vec = hfo.getState();
    int num_steps_per_epi = 0;
    while (status == hfo::IN_GAME) {
          num_steps_per_epi++;
      //std::cout << "::::::"<< hfo::ActionToString(a) <<" "<<count_steps <<std::endl;
      if (count_steps != step && action >=0 && (a != hfo :: MARK_PLAYER ||  unum>0)) {
          count_steps ++;
          if (a == hfo::MARK_PLAYER) {
              hfo.act(a,unum);
              //std::cout << "MARKING" << unum <<"\n";
          } else {
              hfo.act(a);
          }
          status = hfo.step();
          continue;

      } else {
          count_steps = 0;
      }

      if(action != -1) {
        reward = getReward(status);
        sa->update(state, action, reward, discFac);
      }

      // Fill up state array
      purgeFeatures(state, state_vec, numTMates, numOpponents, oppPres);

          // Get raw action
      action = sa->selectAction(state);

      // Get hfo::Action
      a = toAction(action, state_vec);
      if (a== hfo::MARK_PLAYER) {
           unum = state_vec[(state_vec.size()-1 - (action-5)*3)];
           hfo.act(a,unum);
        } else {
           hfo.act(a);
        }
        std::string s = std::to_string(action);
        for (int state_vec_fc=0; state_vec_fc < state_vec.size(); state_vec_fc++) {
            s+=std::to_string(state_vec[state_vec_fc]) + ",";
        }
      s+="UNUM" +std::to_string(unum) +"\n";;
      status = hfo.step();
     // std::cout <<s;
    }
    //std :: cout <<":::::::::::::" << num_steps_per_epi<< " "<<step << " "<<"\n";
    // End of episode
    if(action != -1) {
      reward = getReward(status);
      sa->update(state, action, reward, discFac);
      sa->endEpisode();
    }
  }

  delete sa;
  delete fa;
}

int main(int argc, char **argv) {

  int numAgents = 0;
  int numEpisodes = 10;
  int basePort = 6000;
  double learnR = 0.1;
  int suffix = 0;
  bool opponentPresent = true;
  int numOpponents = 0;
  double eps = 0.01;
  int step = 10;
  std::string weightid;
  for (int i = 0; i<argc; i++) {
      std::string param = std::string(argv[i]);
      std::cout<<param<<"\n";
  }
  for(int i = 1; i < argc; i++) {
    std::string param = std::string(argv[i]);
    if(param == "--numAgents") {
      numAgents = atoi(argv[++i]);
    }else if(param == "--numEpisodes") {
      numEpisodes = atoi(argv[++i]);
    }else if(param == "--basePort") {
      basePort = atoi(argv[++i]);
    }else if(param == "--learnRate") {
      learnR = atof(argv[++i]);
      if(learnR < 0 || learnR > 1) {
        printUsage();
        return 0;
      }
    }else if(param == "--suffix") {
      suffix = atoi(argv[++i]);
    }else if(param == "--noOpponent") {
      opponentPresent = false;
    }else if(param=="--eps"){
        eps=atoi(argv[++i]);
    }else if(param=="--numOpponents"){
        numOpponents=atoi(argv[++i]);
    }else if(param=="--step"){
        step=atoi(argv[++i]);
    }else if(param=="--weightId"){
        weightid=std::string(argv[++i]);
    }else {
      printUsage();
      return 0;
    }
  }
  int numTeammates = numAgents; //using goalie npc
  std::thread agentThreads[numAgents];
  for (int agent = 0; agent < numAgents; agent++) {
    agentThreads[agent] = std::thread(offenseAgent, basePort + agent,
                                      numTeammates, numOpponents, numEpisodes, learnR,
                                      suffix, opponentPresent, eps, step, weightid);
    usleep(500000L);
  }
  for (int agent = 0; agent < numAgents; agent++) {
    agentThreads[agent].join();
  }
  return 0;
}

<commit_msg>Corrections in getting unum and purgeFeatures<commit_after>#include <iostream>
#include <vector>
#include <HFO.hpp>
#include <cstdlib>
#include <thread>
#include "SarsaAgent.h"
#include "CMAC.h"
#include <unistd.h>

// Before running this program, first Start HFO server:
// $./bin/HFO --offense-agents numAgents

void printUsage() {
  std::cout<<"Usage:123 ./high_level_sarsa_agent [Options]"<<std::endl;
  std::cout<<"Options:"<<std::endl;
  std::cout<<"  --numAgents <int>        Number of SARSA agents"<<std::endl;
  std::cout<<"                           Default: 0"<<std::endl;
  std::cout<<"  --numEpisodes <int>      Number of episodes to run"<<std::endl;
  std::cout<<"                           Default: 10"<<std::endl;
  std::cout<<"  --basePort <int>         SARSA agent base port"<<std::endl;
  std::cout<<"                           Default: 6000"<<std::endl;
  std::cout<<"  --learnRate <float>      Learning rate of SARSA agents"<<std::endl;
  std::cout<<"                           Range: [0.0, 1.0]"<<std::endl;
  std::cout<<"                           Default: 0.1"<<std::endl;
  std::cout<<"  --suffix <int>           Suffix for weights files"<<std::endl;
  std::cout<<"                           Default: 0"<<std::endl;
  std::cout<<"  --noOpponent             Sets opponent present flag to false"<<std::endl;
  std::cout<<"  --step                   Sets the persistent step size"<<std::endl;
  std::cout<<"  --eps                    Sets the exploration rate"<<std::endl;
  std::cout<<"  --numOpponents           Sets the number of opponents"<<std::endl;
  std::cout<<"  --weightId               Sets the given Id for weight File"<<std::endl;
  std::cout<<"  --help                   Displays this help and exit"<<std::endl;
}

// Returns the reward for SARSA based on current state
double getReward(hfo::status_t status) {
  double reward;
  if (status==hfo::GOAL) reward = -1;
  else if (status==hfo::CAPTURED_BY_DEFENSE) reward = 1;
  else if (status==hfo::OUT_OF_BOUNDS) reward = 1;
  else reward = 0;
  return reward;
}

// Fill state with only the required features from state_vec
void purgeFeatures(double *state, const std::vector<float>& state_vec,
                   int numTMates, int numOpponents, bool oppPres) {

  int stateIndex = 0;

  // Features[0 - 9] - {5,8}=8
  // Features[9+T+1 - 9+2T]: teammates dists to closest opps=T
  // Features [9+3T+1 - 9+6T]: x, y, unum of teammates ignoring %3 -> unum of team mates=2T
  // Features  [9+6T+1 - 9+6T+3*O]: x, y, unum of opponents ignoring %3 -> unum of opponents=20
  // Ignored: Feature [ 9+6T+3O+1, 9+6T+3O+2]: last_action_status,stamina->2

  // If no opponents ignore features Distance to Opponent
  // and Distance from Teammate i to Opponent are absent
  int tmpIndex = oppPres ? (9 + 3 * numTMates) : (9 + 2 * numTMates);

  for(int i = 0; i < state_vec.size()-2; i++) {

    // Ignore first six featues
    if(i == 5 || i==8) continue;
    if(i>9 && i<= 9+numTMates) continue; // Ignore Goal Opening angles, as invalid
    if(i<= 9+3*numTMates && i > 9+2*numTMates) continue; // Ignore Pass Opening angles, as invalid
    // Ignore Uniform Number of Teammates and opponents
    int temp =  i-tmpIndex;
    if(temp > 0 && (temp % 3 == 0) )continue;
    //if (i > 9+6*numTMates) continue;
    state[stateIndex] = state_vec[i];
    stateIndex++;
  }
}

// Convert int to hfo::Action
hfo::action_t toAction(int action, const std::vector<float>& state_vec) {
hfo::action_t a;
  switch (action) {
        case 0: a = hfo::MOVE; break;
        case 1: a = hfo::REDUCE_ANGLE_TO_GOAL; break;
        case 2: a = hfo::GO_TO_BALL; break;
        case 3: a = hfo::NOOP; break;
        case 4: a = hfo::DEFEND_GOAL; break;
        default : a = hfo::MARK_PLAYER; break;
  }
    return a;
}

void offenseAgent(int port, int numTMates, int numOpponents, int numEpi, double learnR,
                  int suffix, bool oppPres, double eps, int step, std::string weightid) {

  // Number of features
  int numF = oppPres ? (8 + 3 * numTMates + 2*numOpponents) : (3 + 3 * numTMates);
  // Number of actions
  int numA = 5 + numOpponents; //DEF_GOAL+MOVE+GTB+NOOP+RATG+MP(unum)

  // Other SARSA parameters
  eps = 0.01;
  double discFac = 1;
  double lambda=0.9375;
  // Tile coding parameter
  double resolution = 0.1;

  double range[numF];
  double min[numF];
  double res[numF];
  for(int i = 0; i < numF; i++) {
      min[i] = -1;
      range[i] = 2;
      res[i] = resolution;
  }

  // Weights file
  char *wtFile;
  std::string s = "weights_" + std::to_string(port) +
                  "_" + std::to_string(numTMates + 1) +
                  "_" + std::to_string(suffix) +
                  "_" + std::to_string(step) +
                   "_" + weightid;
  wtFile = &s[0u];

  CMAC *fa = new CMAC(numF, numA, range, min, res);
  SarsaAgent *sa = new SarsaAgent(numF, numA, learnR, eps, lambda, fa, wtFile, wtFile);

  hfo::HFOEnvironment hfo;
  hfo::status_t status;
  hfo::action_t a;
  double state[numF];
  int action = -1;
  double reward;
  int no_of_offense = numTMates + 1;
  hfo.connectToServer(hfo::HIGH_LEVEL_FEATURE_SET,"../../bin/teams/base/config/formations-dt",port,"localhost","base_right",false,"");

  for (int episode=0; episode < numEpi; episode++) {
    int count = 0;
    status = hfo::IN_GAME;
    action = -1;
    int count_steps = 0;
    double unum = -1;
    const std::vector<float>& state_vec = hfo.getState();
    int num_steps_per_epi = 0;
    while (status == hfo::IN_GAME) {
          num_steps_per_epi++;
      //std::cout << "::::::"<< hfo::ActionToString(a) <<" "<<count_steps <<std::endl;
      if (count_steps != step && action >=0 && (a != hfo :: MARK_PLAYER ||  unum>0)) {
          count_steps ++;
          if (a == hfo::MARK_PLAYER) {
              hfo.act(a,unum);
              //std::cout << "MARKING" << unum <<"\n";
          } else {
              hfo.act(a);
          }
          status = hfo.step();
          continue;

      } else {
          count_steps = 0;
      }

      if(action != -1) {
        reward = getReward(status);
        sa->update(state, action, reward, discFac);
      }

      // Fill up state array
      purgeFeatures(state, state_vec, numTMates, numOpponents, oppPres);

          // Get raw action
      action = sa->selectAction(state);

      // Get hfo::Action
      a = toAction(action, state_vec);
      if (a== hfo::MARK_PLAYER) {
           unum = state_vec[(state_vec.size()-1 -2 - (action-5)*3)];
           hfo.act(a,unum);
        } else {
           hfo.act(a);
        }
        std::string s = std::to_string(action);
        for (int state_vec_fc=0; state_vec_fc < state_vec.size(); state_vec_fc++) {
            s+=std::to_string(state_vec[state_vec_fc]) + ",";
        }
      s+="UNUM" +std::to_string(unum) +"\n";;
      status = hfo.step();
     // std::cout <<s;
    }
    //std :: cout <<":::::::::::::" << num_steps_per_epi<< " "<<step << " "<<"\n";
    // End of episode
    if(action != -1) {
      reward = getReward(status);
      sa->update(state, action, reward, discFac);
      sa->endEpisode();
    }
  }

  delete sa;
  delete fa;
}

int main(int argc, char **argv) {

  int numAgents = 0;
  int numEpisodes = 10;
  int basePort = 6000;
  double learnR = 0.1;
  int suffix = 0;
  bool opponentPresent = true;
  int numOpponents = 0;
  double eps = 0.01;
  int step = 10;
  std::string weightid;
  for (int i = 0; i<argc; i++) {
      std::string param = std::string(argv[i]);
      std::cout<<param<<"\n";
  }
  for(int i = 1; i < argc; i++) {
    std::string param = std::string(argv[i]);
    if(param == "--numAgents") {
      numAgents = atoi(argv[++i]);
    }else if(param == "--numEpisodes") {
      numEpisodes = atoi(argv[++i]);
    }else if(param == "--basePort") {
      basePort = atoi(argv[++i]);
    }else if(param == "--learnRate") {
      learnR = atof(argv[++i]);
      if(learnR < 0 || learnR > 1) {
        printUsage();
        return 0;
      }
    }else if(param == "--suffix") {
      suffix = atoi(argv[++i]);
    }else if(param == "--noOpponent") {
      opponentPresent = false;
    }else if(param=="--eps"){
        eps=atoi(argv[++i]);
    }else if(param=="--numOpponents"){
        numOpponents=atoi(argv[++i]);
    }else if(param=="--step"){
        step=atoi(argv[++i]);
    }else if(param=="--weightId"){
        weightid=std::string(argv[++i]);
    }else {
      printUsage();
      return 0;
    }
  }
  int numTeammates = numAgents; //using goalie npc
  std::thread agentThreads[numAgents];
  for (int agent = 0; agent < numAgents; agent++) {
    agentThreads[agent] = std::thread(offenseAgent, basePort + agent,
                                      numTeammates, numOpponents, numEpisodes, learnR,
                                      suffix, opponentPresent, eps, step, weightid);
    usleep(500000L);
  }
  for (int agent = 0; agent < numAgents; agent++) {
    agentThreads[agent].join();
  }
  return 0;
}

<|endoftext|>


<commit_before>//===-- GDBRemoteCommunicationClientTest.cpp --------------------*- C++ -*-===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#if defined(_MSC_VER) && (_HAS_EXCEPTIONS == 0)
// Workaround for MSVC standard library bug, which fails to include <thread>
// when
// exceptions are disabled.
#include <eh.h>
#endif
#include <future>

#include "GDBRemoteTestUtils.h"

#include "Plugins/Process/gdb-remote/GDBRemoteCommunicationClient.h"
#include "lldb/Core/DataBuffer.h"
#include "lldb/Core/ModuleSpec.h"

#include "llvm/ADT/ArrayRef.h"

using namespace lldb_private::process_gdb_remote;
using namespace lldb_private;
using namespace lldb;

namespace {

typedef GDBRemoteCommunication::PacketResult PacketResult;

struct TestClient : public GDBRemoteCommunicationClient {
  TestClient() { m_send_acks = false; }
};

void Handle_QThreadSuffixSupported(MockServer &server, bool supported) {
  StringExtractorGDBRemote request;
  ASSERT_EQ(PacketResult::Success, server.GetPacket(request));
  ASSERT_EQ("QThreadSuffixSupported", request.GetStringRef());
  if (supported)
    ASSERT_EQ(PacketResult::Success, server.SendOKResponse());
  else
    ASSERT_EQ(PacketResult::Success, server.SendUnimplementedResponse(nullptr));
}

void HandlePacket(MockServer &server, llvm::StringRef expected,
                  llvm::StringRef response) {
  StringExtractorGDBRemote request;
  ASSERT_EQ(PacketResult::Success, server.GetPacket(request));
  ASSERT_EQ(expected, request.GetStringRef());
  ASSERT_EQ(PacketResult::Success, server.SendPacket(response));
}

uint8_t all_registers[] = {'@', 'A', 'B', 'C', 'D', 'E', 'F', 'G',
                           'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O'};
std::string all_registers_hex = "404142434445464748494a4b4c4d4e4f";
uint8_t one_register[] = {'A', 'B', 'C', 'D'};
std::string one_register_hex = "41424344";

} // end anonymous namespace

class GDBRemoteCommunicationClientTest : public GDBRemoteTest {};

TEST_F(GDBRemoteCommunicationClientTest, WriteRegister) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  const uint32_t reg_num = 4;
  std::future<bool> write_result = std::async(std::launch::async, [&] {
    return client.WriteRegister(tid, reg_num, one_register);
  });

  Handle_QThreadSuffixSupported(server, true);

  HandlePacket(server, "P4=" + one_register_hex + ";thread:0047;", "OK");
  ASSERT_TRUE(write_result.get());

  write_result = std::async(std::launch::async, [&] {
    return client.WriteAllRegisters(tid, all_registers);
  });

  HandlePacket(server, "G" + all_registers_hex + ";thread:0047;", "OK");
  ASSERT_TRUE(write_result.get());
}

TEST_F(GDBRemoteCommunicationClientTest, WriteRegisterNoSuffix) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  const uint32_t reg_num = 4;
  std::future<bool> write_result = std::async(std::launch::async, [&] {
    return client.WriteRegister(tid, reg_num, one_register);
  });

  Handle_QThreadSuffixSupported(server, false);
  HandlePacket(server, "Hg47", "OK");
  HandlePacket(server, "P4=" + one_register_hex, "OK");
  ASSERT_TRUE(write_result.get());

  write_result = std::async(std::launch::async, [&] {
    return client.WriteAllRegisters(tid, all_registers);
  });

  HandlePacket(server, "G" + all_registers_hex, "OK");
  ASSERT_TRUE(write_result.get());
}

TEST_F(GDBRemoteCommunicationClientTest, ReadRegister) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  const uint32_t reg_num = 4;
  std::future<bool> async_result = std::async(
      std::launch::async, [&] { return client.GetpPacketSupported(tid); });
  Handle_QThreadSuffixSupported(server, true);
  HandlePacket(server, "p0;thread:0047;", one_register_hex);
  ASSERT_TRUE(async_result.get());

  std::future<DataBufferSP> read_result = std::async(
      std::launch::async, [&] { return client.ReadRegister(tid, reg_num); });
  HandlePacket(server, "p4;thread:0047;", "41424344");
  auto buffer_sp = read_result.get();
  ASSERT_TRUE(bool(buffer_sp));
  ASSERT_EQ(0,
            memcmp(buffer_sp->GetBytes(), one_register, sizeof one_register));

  read_result = std::async(std::launch::async,
                           [&] { return client.ReadAllRegisters(tid); });
  HandlePacket(server, "g;thread:0047;", all_registers_hex);
  buffer_sp = read_result.get();
  ASSERT_TRUE(bool(buffer_sp));
  ASSERT_EQ(0,
            memcmp(buffer_sp->GetBytes(), all_registers, sizeof all_registers));
}

TEST_F(GDBRemoteCommunicationClientTest, SaveRestoreRegistersNoSuffix) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  uint32_t save_id;
  std::future<bool> async_result = std::async(std::launch::async, [&] {
    return client.SaveRegisterState(tid, save_id);
  });
  Handle_QThreadSuffixSupported(server, false);
  HandlePacket(server, "Hg47", "OK");
  HandlePacket(server, "QSaveRegisterState", "1");
  ASSERT_TRUE(async_result.get());
  EXPECT_EQ(1u, save_id);

  async_result = std::async(std::launch::async, [&] {
    return client.RestoreRegisterState(tid, save_id);
  });
  HandlePacket(server, "QRestoreRegisterState:1", "OK");
  ASSERT_TRUE(async_result.get());
}

TEST_F(GDBRemoteCommunicationClientTest, SyncThreadState) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  std::future<bool> async_result = std::async(
      std::launch::async, [&] { return client.SyncThreadState(tid); });
  HandlePacket(server, "qSyncThreadStateSupported", "OK");
  HandlePacket(server, "QSyncThreadState:0047;", "OK");
  ASSERT_TRUE(async_result.get());
}

TEST_F(GDBRemoteCommunicationClientTest, GetModulesInfo) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  llvm::Triple triple("i386-pc-linux");

  FileSpec file_specs[] = {FileSpec("/foo/bar.so", false),
                           FileSpec("/foo/baz.so", false)};
  std::future<llvm::Optional<std::vector<ModuleSpec>>> async_result =
      std::async(std::launch::async,
                 [&] { return client.GetModulesInfo(file_specs, triple); });
  HandlePacket(
      server, "jModulesInfo:["
              R"({"file":"/foo/bar.so","triple":"i386-pc-linux"},)"
              R"({"file":"/foo/baz.so","triple":"i386-pc-linux"}])",
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f","triple":"i386-pc-linux",)"
      R"("file_path":"/foo/bar.so","file_offset":0,"file_size":1234}]])");

  auto result = async_result.get();
  ASSERT_TRUE(result.hasValue());
  ASSERT_EQ(1u, result->size());
  EXPECT_EQ("/foo/bar.so", result.getValue()[0].GetFileSpec().GetPath());
  EXPECT_EQ(triple, result.getValue()[0].GetArchitecture().GetTriple());
  EXPECT_EQ(UUID("@ABCDEFGHIJKLMNO", 16), result.getValue()[0].GetUUID());
  EXPECT_EQ(0u, result.getValue()[0].GetObjectOffset());
  EXPECT_EQ(1234u, result.getValue()[0].GetObjectSize());
}

TEST_F(GDBRemoteCommunicationClientTest, GetModulesInfoInvalidResponse) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  llvm::Triple triple("i386-pc-linux");
  FileSpec file_spec("/foo/bar.so", false);

  const char *invalid_responses[] = {
      "OK", "E47", "[]",
      // no UUID
      R"([{"triple":"i386-pc-linux",)"
      R"("file_path":"/foo/bar.so","file_offset":0,"file_size":1234}])",
      // no triple
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f",)"
      R"("file_path":"/foo/bar.so","file_offset":0,"file_size":1234}])",
      // no file_path
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f","triple":"i386-pc-linux",)"
      R"("file_offset":0,"file_size":1234}])",
      // no file_offset
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f","triple":"i386-pc-linux",)"
      R"("file_path":"/foo/bar.so","file_size":1234}])",
      // no file_size
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f","triple":"i386-pc-linux",)"
      R"("file_path":"/foo/bar.so","file_offset":0}])",
  };

  for (const char *response : invalid_responses) {
    std::future<llvm::Optional<std::vector<ModuleSpec>>> async_result =
        std::async(std::launch::async,
                   [&] { return client.GetModulesInfo(file_spec, triple); });
    HandlePacket(
        server,
        R"(jModulesInfo:[{"file":"/foo/bar.so","triple":"i386-pc-linux"}])",
        response);

    ASSERT_FALSE(async_result.get().hasValue()) << "response was: " << response;
  }
}
<commit_msg>Fix new gdb-remote client unit test for windows<commit_after>//===-- GDBRemoteCommunicationClientTest.cpp --------------------*- C++ -*-===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#if defined(_MSC_VER) && (_HAS_EXCEPTIONS == 0)
// Workaround for MSVC standard library bug, which fails to include <thread>
// when
// exceptions are disabled.
#include <eh.h>
#endif
#include <future>

#include "GDBRemoteTestUtils.h"

#include "Plugins/Process/gdb-remote/GDBRemoteCommunicationClient.h"
#include "lldb/Core/DataBuffer.h"
#include "lldb/Core/ModuleSpec.h"

#include "llvm/ADT/ArrayRef.h"

using namespace lldb_private::process_gdb_remote;
using namespace lldb_private;
using namespace lldb;

namespace {

typedef GDBRemoteCommunication::PacketResult PacketResult;

struct TestClient : public GDBRemoteCommunicationClient {
  TestClient() { m_send_acks = false; }
};

void Handle_QThreadSuffixSupported(MockServer &server, bool supported) {
  StringExtractorGDBRemote request;
  ASSERT_EQ(PacketResult::Success, server.GetPacket(request));
  ASSERT_EQ("QThreadSuffixSupported", request.GetStringRef());
  if (supported)
    ASSERT_EQ(PacketResult::Success, server.SendOKResponse());
  else
    ASSERT_EQ(PacketResult::Success, server.SendUnimplementedResponse(nullptr));
}

void HandlePacket(MockServer &server, llvm::StringRef expected,
                  llvm::StringRef response) {
  StringExtractorGDBRemote request;
  ASSERT_EQ(PacketResult::Success, server.GetPacket(request));
  ASSERT_EQ(expected, request.GetStringRef());
  ASSERT_EQ(PacketResult::Success, server.SendPacket(response));
}

uint8_t all_registers[] = {'@', 'A', 'B', 'C', 'D', 'E', 'F', 'G',
                           'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O'};
std::string all_registers_hex = "404142434445464748494a4b4c4d4e4f";
uint8_t one_register[] = {'A', 'B', 'C', 'D'};
std::string one_register_hex = "41424344";

} // end anonymous namespace

class GDBRemoteCommunicationClientTest : public GDBRemoteTest {};

TEST_F(GDBRemoteCommunicationClientTest, WriteRegister) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  const uint32_t reg_num = 4;
  std::future<bool> write_result = std::async(std::launch::async, [&] {
    return client.WriteRegister(tid, reg_num, one_register);
  });

  Handle_QThreadSuffixSupported(server, true);

  HandlePacket(server, "P4=" + one_register_hex + ";thread:0047;", "OK");
  ASSERT_TRUE(write_result.get());

  write_result = std::async(std::launch::async, [&] {
    return client.WriteAllRegisters(tid, all_registers);
  });

  HandlePacket(server, "G" + all_registers_hex + ";thread:0047;", "OK");
  ASSERT_TRUE(write_result.get());
}

TEST_F(GDBRemoteCommunicationClientTest, WriteRegisterNoSuffix) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  const uint32_t reg_num = 4;
  std::future<bool> write_result = std::async(std::launch::async, [&] {
    return client.WriteRegister(tid, reg_num, one_register);
  });

  Handle_QThreadSuffixSupported(server, false);
  HandlePacket(server, "Hg47", "OK");
  HandlePacket(server, "P4=" + one_register_hex, "OK");
  ASSERT_TRUE(write_result.get());

  write_result = std::async(std::launch::async, [&] {
    return client.WriteAllRegisters(tid, all_registers);
  });

  HandlePacket(server, "G" + all_registers_hex, "OK");
  ASSERT_TRUE(write_result.get());
}

TEST_F(GDBRemoteCommunicationClientTest, ReadRegister) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  const uint32_t reg_num = 4;
  std::future<bool> async_result = std::async(
      std::launch::async, [&] { return client.GetpPacketSupported(tid); });
  Handle_QThreadSuffixSupported(server, true);
  HandlePacket(server, "p0;thread:0047;", one_register_hex);
  ASSERT_TRUE(async_result.get());

  std::future<DataBufferSP> read_result = std::async(
      std::launch::async, [&] { return client.ReadRegister(tid, reg_num); });
  HandlePacket(server, "p4;thread:0047;", "41424344");
  auto buffer_sp = read_result.get();
  ASSERT_TRUE(bool(buffer_sp));
  ASSERT_EQ(0,
            memcmp(buffer_sp->GetBytes(), one_register, sizeof one_register));

  read_result = std::async(std::launch::async,
                           [&] { return client.ReadAllRegisters(tid); });
  HandlePacket(server, "g;thread:0047;", all_registers_hex);
  buffer_sp = read_result.get();
  ASSERT_TRUE(bool(buffer_sp));
  ASSERT_EQ(0,
            memcmp(buffer_sp->GetBytes(), all_registers, sizeof all_registers));
}

TEST_F(GDBRemoteCommunicationClientTest, SaveRestoreRegistersNoSuffix) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  uint32_t save_id;
  std::future<bool> async_result = std::async(std::launch::async, [&] {
    return client.SaveRegisterState(tid, save_id);
  });
  Handle_QThreadSuffixSupported(server, false);
  HandlePacket(server, "Hg47", "OK");
  HandlePacket(server, "QSaveRegisterState", "1");
  ASSERT_TRUE(async_result.get());
  EXPECT_EQ(1u, save_id);

  async_result = std::async(std::launch::async, [&] {
    return client.RestoreRegisterState(tid, save_id);
  });
  HandlePacket(server, "QRestoreRegisterState:1", "OK");
  ASSERT_TRUE(async_result.get());
}

TEST_F(GDBRemoteCommunicationClientTest, SyncThreadState) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  const lldb::tid_t tid = 0x47;
  std::future<bool> async_result = std::async(
      std::launch::async, [&] { return client.SyncThreadState(tid); });
  HandlePacket(server, "qSyncThreadStateSupported", "OK");
  HandlePacket(server, "QSyncThreadState:0047;", "OK");
  ASSERT_TRUE(async_result.get());
}

TEST_F(GDBRemoteCommunicationClientTest, GetModulesInfo) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  llvm::Triple triple("i386-pc-linux");

  FileSpec file_specs[] = {
      FileSpec("/foo/bar.so", false, FileSpec::ePathSyntaxPosix),
      FileSpec("/foo/baz.so", false, FileSpec::ePathSyntaxPosix)};
  std::future<llvm::Optional<std::vector<ModuleSpec>>> async_result =
      std::async(std::launch::async,
                 [&] { return client.GetModulesInfo(file_specs, triple); });
  HandlePacket(
      server, "jModulesInfo:["
              R"({"file":"/foo/bar.so","triple":"i386-pc-linux"},)"
              R"({"file":"/foo/baz.so","triple":"i386-pc-linux"}])",
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f","triple":"i386-pc-linux",)"
      R"("file_path":"/foo/bar.so","file_offset":0,"file_size":1234}]])");

  auto result = async_result.get();
  ASSERT_TRUE(result.hasValue());
  ASSERT_EQ(1u, result->size());
  EXPECT_EQ("/foo/bar.so", result.getValue()[0].GetFileSpec().GetPath());
  EXPECT_EQ(triple, result.getValue()[0].GetArchitecture().GetTriple());
  EXPECT_EQ(UUID("@ABCDEFGHIJKLMNO", 16), result.getValue()[0].GetUUID());
  EXPECT_EQ(0u, result.getValue()[0].GetObjectOffset());
  EXPECT_EQ(1234u, result.getValue()[0].GetObjectSize());
}

TEST_F(GDBRemoteCommunicationClientTest, GetModulesInfoInvalidResponse) {
  TestClient client;
  MockServer server;
  Connect(client, server);
  if (HasFailure())
    return;

  llvm::Triple triple("i386-pc-linux");
  FileSpec file_spec("/foo/bar.so", false, FileSpec::ePathSyntaxPosix);

  const char *invalid_responses[] = {
      "OK", "E47", "[]",
      // no UUID
      R"([{"triple":"i386-pc-linux",)"
      R"("file_path":"/foo/bar.so","file_offset":0,"file_size":1234}])",
      // no triple
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f",)"
      R"("file_path":"/foo/bar.so","file_offset":0,"file_size":1234}])",
      // no file_path
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f","triple":"i386-pc-linux",)"
      R"("file_offset":0,"file_size":1234}])",
      // no file_offset
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f","triple":"i386-pc-linux",)"
      R"("file_path":"/foo/bar.so","file_size":1234}])",
      // no file_size
      R"([{"uuid":"404142434445464748494a4b4c4d4e4f","triple":"i386-pc-linux",)"
      R"("file_path":"/foo/bar.so","file_offset":0}])",
  };

  for (const char *response : invalid_responses) {
    std::future<llvm::Optional<std::vector<ModuleSpec>>> async_result =
        std::async(std::launch::async,
                   [&] { return client.GetModulesInfo(file_spec, triple); });
    HandlePacket(
        server,
        R"(jModulesInfo:[{"file":"/foo/bar.so","triple":"i386-pc-linux"}])",
        response);

    ASSERT_FALSE(async_result.get().hasValue()) << "response was: " << response;
  }
}
<|endoftext|>


<commit_before>// Standard header files
#include<iostream>
using namespace std;

// SMP HEADER FILES ------
#include <smp/components/samplers/uniform.hpp>
#include <smp/components/distance_evaluators/kdtree.hpp>
#include <smp/components/extenders/double_integrator.hpp>
#include <smp/components/collision_checkers/mu_calculus.hpp>
#include <smp/components/model_checkers/mu_calculus.hpp>

#include <smp/planners/rrg.hpp>

#include <smp/planner_utils/trajectory.hpp>

// PARAMETERS TO THE PROBLEM *******************************************
// *

/* Change the number of dimensions from here.
   Scale it up to 6 - 10 dimensions to see the convergence of RRT*
   towards an optimal solution in very high dimensional configuration
   spaces without employing any heuristics. */
#define NUM_DIMENSIONS 2

/* Maximum length of an extension. This should scale up with sqrt(d)
   and with L, where d is the dimensionality of space and L is the
   side length of a box containing the obstacle free space. */
#define EXTENSION_LENGTH  5.0
// *
// *********************************************************************

// SMP TYPE DEFINITIONS -------
using namespace smp;

// State, input, vertex_data, and edge_data definitions
typedef state_double_integrator<NUM_DIMENSIONS> state_t;
typedef input_double_integrator<NUM_DIMENSIONS> input_t;
typedef model_checker_mu_calculus_vertex_data vertex_data_t;
typedef model_checker_mu_calculus_edge_data edge_data_t;

// Create the typeparams structure
typedef struct _typeparams {
  typedef state_t state;
  typedef input_t input;
  typedef vertex_data_t vertex_data;
  typedef edge_data_t edge_data;
} typeparams;

// Define the trajectory type
typedef trajectory<typeparams> trajectory_t;

// Define all planner component types
typedef sampler_uniform<typeparams,NUM_DIMENSIONS*2> sampler_t;
typedef distance_evaluator_kdtree<typeparams,NUM_DIMENSIONS*2> distance_evaluator_t;
typedef extender_double_integrator<typeparams,NUM_DIMENSIONS> extender_t;
typedef collision_checker_mu_calculus<typeparams,NUM_DIMENSIONS> collision_checker_t;
typedef model_checker_mu_calculus<typeparams> model_checker_t;

// Define all algorithm types
typedef rrg<typeparams>  rrg_t;

int
main ()
{
  // 1. CREATE PLANNING OBJECTS

  // 1.a Create the components
  sampler_t sampler;
  distance_evaluator_t distance_evaluator;
  extender_t extender;
  collision_checker_t collision_checker;
  model_checker_t model_checker;

  // 1.b Create the planner algorithm
  rrg_t planner (sampler, distance_evaluator, extender, collision_checker, model_checker);

  /* The phase parameter can be used to run the algorithm as an RRT,
     See the documentation of the RRG algorithm for more
     information. */
  planner.parameters.set_phase (2);

  /* Set this parameter should be set at least to the side length of
     the (bounded) state space. E.g., if the state space is a box with
     side length L, then this parameter should be set to at least L
     for rapid and efficient convergence in trajectory space. */
  planner.parameters.set_gamma (35.0);
  planner.parameters.set_dimension(NUM_DIMENSIONS*2);
  planner.parameters.set_max_radius (5.0);  // This parameter is expecially capped to a certain to limit
                                            //   the amount of data that is published through the libbot interface.

  // 2. INITALIZE PLANNING OBJECTS

  // 2.a Initialize the sampler
  region<4> sampler_support;
  for (int i = 0; i < 2; i++) {
    sampler_support.center[i] = 0.0;
    sampler_support.size[i] = 20.0;
  }
  for (int i = 2; i < 4; i++) {
    sampler_support.center[i] = 0.0;
    sampler_support.size[i] = 2.0;
  }
  sampler.set_support (sampler_support);

  // 2.b Initialize the distance evaluator
  //     Nothing to initialize. One could change the kdtree weights.

  // 2.c Initialize the extender

  // 2.d Initialize the collision checker
  /* NOTE that these are chosen to match the regions that are hard-coded in
     model_checker_mu_calculus::mc_update_insert_vertex(), as defined in
     model_checkers/mu_calculus.hpp */
  region<NUM_DIMENSIONS> R;
  R.center[0] = R.center[1] = -3.5;
  R.size[0] = R.size[1] = 1.0;
  collision_checker.add_region( R );

  R.center[0] = 5.5;
  R.center[1] = 1.5;
  R.size[0] = R.size[1] = 1.0;
  collision_checker.add_region( R );

  R.center[0] = R.center[1] = 2.05;
  R.size[0] = R.size[1] = 3.9;
  collision_checker.add_region( R );

  // 2.e Initialize the model checker
  /* NOTE that the formula is currently hard-coded in ParseTree::parseFormula()
     as the parse tree provided in parseFormulaLoop4(), both defined in the file
     inc_mu_mc/pt.cpp */

  // 2.f Initialize the planner
  state_t *state_initial = new state_t;
  for (int i = 0; i < 4; i++) {
    state_initial->state_vars[i] = 0.0;
  }
  planner.initialize (state_initial);

  // 3. RUN THE PLANNER
  for (int i = 0; i < 10000 && !planner.has_feasible(); i++)
    planner.iteration ();

  // 4. GET THE RESULTS
  planner.dump_json();

  return 0;

}
<commit_msg>EXAMPLE: Update rrg/double_integrator to use new labeler<commit_after>// Standard header files
#include<iostream>
using namespace std;

// SMP HEADER FILES ------
#include <smp/components/samplers/uniform.hpp>
#include <smp/components/distance_evaluators/kdtree.hpp>
#include <smp/components/extenders/double_integrator.hpp>
#include <smp/components/collision_checkers/mu_calculus.hpp>
#include <smp/components/model_checkers/mu_calculus.hpp>

#include <smp/planners/rrg.hpp>

#include <smp/planner_utils/trajectory.hpp>

// PARAMETERS TO THE PROBLEM *******************************************
// *

/* Change the number of dimensions from here.
   Scale it up to 6 - 10 dimensions to see the convergence of RRT*
   towards an optimal solution in very high dimensional configuration
   spaces without employing any heuristics. */
#define NUM_DIMENSIONS 2

/* Maximum length of an extension. This should scale up with sqrt(d)
   and with L, where d is the dimensionality of space and L is the
   side length of a box containing the obstacle free space. */
#define EXTENSION_LENGTH  5.0
// *
// *********************************************************************

// SMP TYPE DEFINITIONS -------
using namespace smp;

// State, input, vertex_data, and edge_data definitions
typedef state_double_integrator<NUM_DIMENSIONS> state_t;
typedef input_double_integrator<NUM_DIMENSIONS> input_t;
typedef region<NUM_DIMENSIONS> region_t;
typedef model_checker_mu_calculus_vertex_data vertex_data_t;
typedef model_checker_mu_calculus_edge_data edge_data_t;

// Create the typeparams structure
typedef struct _typeparams {
  typedef state_t state;
  typedef input_t input;
  typedef vertex_data_t vertex_data;
  typedef edge_data_t edge_data;
  typedef region_t region;
} typeparams;

// Define the trajectory type
typedef trajectory<typeparams> trajectory_t;

// Define all planner component types
typedef sampler_uniform<typeparams,NUM_DIMENSIONS*2> sampler_t;
typedef distance_evaluator_kdtree<typeparams,NUM_DIMENSIONS*2> distance_evaluator_t;
typedef extender_double_integrator<typeparams,NUM_DIMENSIONS> extender_t;
typedef collision_checker_mu_calculus<typeparams> collision_checker_t;
typedef model_checker_mu_calculus<typeparams> model_checker_t;

// Define all algorithm types
typedef rrg<typeparams>  rrg_t;

int
main ()
{
  // 1. CREATE PLANNING OBJECTS

  // 1.a Create the components
  sampler_t sampler;
  distance_evaluator_t distance_evaluator;
  extender_t extender;
  collision_checker_t collision_checker;
  model_checker_t model_checker;
  model_checker.add_labeler( &collision_checker );

  // 1.b Create the planner algorithm
  rrg_t planner (sampler, distance_evaluator, extender, collision_checker, model_checker);

  /* The phase parameter can be used to run the algorithm as an RRT,
     See the documentation of the RRG algorithm for more
     information. */
  planner.parameters.set_phase (2);

  /* Set this parameter should be set at least to the side length of
     the (bounded) state space. E.g., if the state space is a box with
     side length L, then this parameter should be set to at least L
     for rapid and efficient convergence in trajectory space. */
  planner.parameters.set_gamma (35.0);
  planner.parameters.set_dimension(NUM_DIMENSIONS*2);
  planner.parameters.set_max_radius (5.0);  // This parameter is expecially capped to a certain to limit
                                            //   the amount of data that is published through the libbot interface.

  // 2. INITALIZE PLANNING OBJECTS

  // 2.a Initialize the sampler
  region<NUM_DIMENSIONS*2> sampler_support;
  for (int i = 0; i < NUM_DIMENSIONS; i++) {
    sampler_support.center[i] = 0.0;
    sampler_support.size[i] = 20.0;
  }
  for (int i = NUM_DIMENSIONS; i < NUM_DIMENSIONS*2; i++) {
    sampler_support.center[i] = 0.0;
    sampler_support.size[i] = 2.0;
  }
  sampler.set_support (sampler_support);

  // 2.b Initialize the distance evaluator
  //     Nothing to initialize. One could change the kdtree weights.

  // 2.c Initialize the extender

  // 2.d Initialize the collision checker
  region_t R;
  R.center[0] = R.center[1] = -3.5;
  R.size[0] = R.size[1] = 1.0;
  if (NUM_DIMENSIONS >= 3) {
	  R.center[2] = 5.0;
	  R.size[2] = 2.0;
  }
  collision_checker.add_region( R );

  R.center[0] = 5.5;
  R.center[1] = 1.5;
  R.size[0] = R.size[1] = 1.0;
  if (NUM_DIMENSIONS >= 3) {
	  R.center[2] = 5.0;
	  R.size[2] = 2.0;
  }
  collision_checker.add_region( R );

  R.center[0] = R.center[1] = 2.05;
  R.size[0] = R.size[1] = 3.9;
  if (NUM_DIMENSIONS >= 3) {
	  R.center[2] = 5.0;
	  R.size[2] = 2.0;
  }
  collision_checker.add_region( R );

  // 2.e Initialize the model checker
  /* NOTE that the formula is currently hard-coded in ParseTree::parseFormula()
     as the parse tree provided in parseFormulaLoop4(), both defined in the file
     inc_mu_mc/pt.cpp */

  // 2.f Initialize the planner
  state_t *state_initial = new state_t;
  for (int i = 0; i < NUM_DIMENSIONS*2; i++) {
    state_initial->state_vars[i] = 0.0;
  }
  planner.initialize (state_initial);

  // 3. RUN THE PLANNER
  for (int i = 0; i < 10000 && !planner.has_feasible(); i++)
    planner.iteration ();

  // 4. GET THE RESULTS
  planner.dump_json();

  return 0;

}
<|endoftext|>


<commit_before>/*************************************************************************
 * Copyright (c) 2014 eProsima. All rights reserved.
 *
 * This copy of eProsima RTPS is licensed to you under the terms described in the
 * fastrtps_LIBRARY_LICENSE file included in this distribution.
 *
 *************************************************************************/

/**
 * @file main_ClientServerTest.cpp
 *
 */

#include <stdio.h>
#include <string>
#include <iostream>
#include <iomanip>
#include <bitset>
#include <cstdint>
#include <sstream>

#include "EprosimaServer.h"
#include "EprosimaClientTest.h"

#include "fastrtps/utils/RTPSLog.h"
#include "fastrtps/Domain.h"

using namespace eprosima;
using namespace fastrtps;
using std::cout;
using std::endl;

#if defined(__LITTLE_ENDIAN__)
const Endianness_t DEFAULT_ENDIAN = LITTLEEND;
#elif defined (__BIG_ENDIAN__)
const Endianness_t DEFAULT_ENDIAN = BIGEND;
#endif

#if defined(_WIN32)
#define COPYSTR strcpy_s
#else
#define COPYSTR strcpy
#endif

enum E_SIDE
{
	CLIENT,
	SERVER
};

int main(int argc, char** argv){
	Log::setVerbosity(VERB_WARNING);
	cout << "Starting "<< endl;
	E_SIDE side;
	int samples = 10000;
	if(argc > 1)
	{
		if(strcmp(argv[1],"client")==0)
			side = CLIENT;
		else if(strcmp(argv[1],"server")==0)
			side = SERVER;
		else
		{
			cout << "Argument 1 needs to be client OR server"<<endl;
			return 0;
		}
		if(argc > 3)
		{
			std::istringstream iss( argv[3] );
			if (!(iss >> samples))
			{
				cout << "Problem reading samples number,using default 10000 samples "<< endl;
				samples = 10000;
			}
		}
	}
	else
	{
		cout << "Client Server Test needs 1 arguments: (client/server)"<<endl;
		return 0;
	}

	if(side == SERVER)
	{
		EprosimaServer server;
		server.init();
		server.serve();
	}
	if(side == CLIENT)
	{
		EprosimaClientTest clienttest;
		double result = clienttest.run(samples);
		if(result > 0)
			cout << "Mean Time of " << samples << " samples: " << result << " us" << endl;
		else
			cout << "Some problem with the test "<<endl;
	}

	Domain::stopAll();
	cout << "EVERYTHING STOPPED FINE"<<endl;

	return 0;
}

<commit_msg>Changed test<commit_after>/*************************************************************************
 * Copyright (c) 2014 eProsima. All rights reserved.
 *
 * This copy of eProsima RTPS is licensed to you under the terms described in the
 * fastrtps_LIBRARY_LICENSE file included in this distribution.
 *
 *************************************************************************/

/**
 * @file main_ClientServerTest.cpp
 *
 */

#include <stdio.h>
#include <string>
#include <iostream>
#include <iomanip>
#include <bitset>
#include <cstdint>
#include <sstream>

#include "EprosimaServer.h"
#include "EprosimaClientTest.h"

#include "fastrtps/utils/RTPSLog.h"
#include "fastrtps/Domain.h"

using namespace eprosima;
using namespace fastrtps;
using std::cout;
using std::endl;

#if defined(__LITTLE_ENDIAN__)
const Endianness_t DEFAULT_ENDIAN = LITTLEEND;
#elif defined (__BIG_ENDIAN__)
const Endianness_t DEFAULT_ENDIAN = BIGEND;
#endif

#if defined(_WIN32)
#define COPYSTR strcpy_s
#else
#define COPYSTR strcpy
#endif

enum E_SIDE
{
	CLIENT,
	SERVER
};

int main(int argc, char** argv){
	Log::setVerbosity(VERB_ERROR);
	cout << "Starting "<< endl;
	E_SIDE side;
	int samples = 10000;
	if(argc > 1)
	{
		if(strcmp(argv[1],"client")==0)
			side = CLIENT;
		else if(strcmp(argv[1],"server")==0)
			side = SERVER;
		else
		{
			cout << "Argument 1 needs to be client OR server"<<endl;
			return 0;
		}
		if(argc > 3)
		{
			std::istringstream iss( argv[3] );
			if (!(iss >> samples))
			{
				cout << "Problem reading samples number,using default 10000 samples "<< endl;
				samples = 10000;
			}
		}
	}
	else
	{
		cout << "Client Server Test needs 1 arguments: (client/server)"<<endl;
		return 0;
	}

	if(side == SERVER)
	{
		EprosimaServer server;
		server.init();
		server.serve();
	}
	if(side == CLIENT)
	{
		EprosimaClientTest clienttest;
		double result = clienttest.run(samples);
		if(result > 0)
			cout << "Mean Time of " << samples << " samples: " << result << " us" << endl;
		else
			cout << "Some problem with the test "<<endl;
	}

	Domain::stopAll();
	cout << "EVERYTHING STOPPED FINE"<<endl;

	return 0;
}

<|endoftext|>


<commit_before>#include <mutex>
#include <vector>
#include <FL/Fl.H>
#include <FL/Fl_Window.H>
#include <FL/Fl_Image.H>
#include <FL/Fl_Box.H>
#include "data_type.hpp"

namespace gui
{
namespace impl
{
    class MoveBox : public Fl_Box
    {
    public:
        MoveBox(point_type const& point, int const x, int const y, int const w, int const h);
        virtual ~MoveBox() = default;

        point_type const& first_point() const;

    private:
        point_type const first_point_;
    };

    class MoveWindow : public Fl_Window
    {
    public:
        MoveWindow(split_image_type const& images, std::string const& window_name);
        virtual ~MoveWindow() = default;

        // 結果を返す関数であるが，期待通りの働きをしない
        std::vector<std::vector<point_type>> result() const;

        // boxes_配列に基づいて，各boxの位置を算出しなおして再描画する
        void boxes_redraw();

        // boxの並び替え操作に関する関数群(内部でredrawするかどうかはランタイム的に検討)
        void box_swap(point_type const& p1, point_type const& p2);
        void box_x_line_swap(int const y1, int const y2);
        void box_y_line_swap(int const x1, int const x2);

        // boxからのイベントを受け取る関数群
        void click_box(int const button, point_type const& event_box);
        void drag_box(point_type const& event_box);
        void release_box(int const button, point_type const& event_box);

        bool border_check(int const x, int const y);          // TrueならWindow内
        point_type window_to_point(int const x, int const y); // Window上の座標から，point_typeを算出
        int handle(int event) override;

    private:
        splitter const sp_;
        std::string const window_name_;

        int const image_width_;
        int const image_height_;
        int const split_x_;
        int const split_y_;

        int latest_button_;
        point_type move_begin_;
        point_type select_begin_;
        point_type select_end_;
        std::mutex handler_mutex_;

        std::vector<std::vector<unfold_image_type>> image_;
        std::vector<std::vector<Fl_RGB_Image*>> rgbs_;
        std::vector<std::vector<std::unique_ptr<MoveBox>>> boxes_;
    };

    MoveBox::MoveBox(point_type const& point, int const x, int const y, int const w, int const h)
        : Fl_Box(x, y, w, h), first_point_(point)
    {
    }

    point_type const& MoveBox::first_point() const
    {
        return first_point_;
    }

    MoveWindow::MoveWindow(split_image_type const& images, std::string const& window_name)
        : Fl_Window(/*temporary size*/10, 10), window_name_(window_name), sp_(), latest_button_(0),
          select_begin_({-1,-1}), select_end_({-1,-1}),
          image_width_ (images[0][0].cols),
          image_height_(images[0][0].rows),
          split_x_     (images[0]      .size()),
          split_y_     (images         .size())
    {
        int const window_width  = split_x_ * image_width_;
        int const window_height = split_y_ * image_height_;

        this->size(window_width, window_height);
        this->label(window_name_.c_str());

        // 引数の「2次元配置された2次元イメージ」を「2次元配置された1次元イメージ」に変える
        image_.clear();
        image_.reserve(split_y_);
        for(int i=0; i<split_y_; ++i)
        {
            std::vector<unfold_image_type> line;
            line.reserve(split_x_);
            for(int j=0; j<split_x_; ++j)
            {
                line.push_back(sp_.unfold_image(images[i][j]));
            }
            image_.push_back(std::move(line));
        }

        // 「1次元配置された1次元イメージ」をFLTK用のイメージにリンクさせる．(どちらも生きている必要がある)
        rgbs_.clear();
        rgbs_.reserve(split_y_);
        for(int i=0; i<split_y_; ++i)
        {
            std::vector<Fl_RGB_Image*> line;
            line.reserve(split_x_);
            for(int j=0; j<split_x_; ++j)
            {
                line.push_back(new Fl_RGB_Image(image_[i][j].data(), image_width_, image_height_));
            }
            rgbs_.push_back(std::move(line));
        }

        // FLTKイメージをMoveBoxに関連付ける
        boxes_.clear();
        boxes_.reserve(split_y_);
        for(int i=0; i<split_y_; ++i)
        {
            std::vector<std::unique_ptr<MoveBox>> line;
            line.reserve(split_x_);
            for(int j=0; j<split_x_; ++j)
            {
                auto box = std::make_unique<MoveBox>(point_type{j,i}, image_width_*j, image_height_*i, image_width_, image_height_);
                box->image(rgbs_[i][j]);
                line.push_back(std::move(box));
            }
            boxes_.push_back(std::move(line));
        }
    }

    std::vector<std::vector<point_type>> MoveWindow::result() const
    {
        std::vector<std::vector<point_type>> result(
            split_y_,
            std::vector<point_type>(split_x_, point_type{-1,-1})
            );

        for(int i=0; i<split_y_; ++i)
        {
            for(int j=0; j<split_x_; ++j)
            {
                auto const& target = boxes_[i][j]->first_point();
                result[target.y][target.x] = point_type{j, i};
            }
        }

        return result;
    }

    void MoveWindow::boxes_redraw()
    {
        for(int i=0; i<split_y_; ++i)
        {
            for(int j=0; j<split_x_; ++j)
            {
                auto& target_box = boxes_[i][j];
                target_box->resize(image_width_*j, image_height_*i, image_width_, image_height_);
                target_box->redraw();
            }
        }
        return;
    }

    void MoveWindow::box_swap(point_type const& p1, point_type const& p2)
    {
        std::swap(boxes_[p1.y][p1.x], boxes_[p2.y][p2.x]);
    }

    void MoveWindow::box_x_line_swap(int const y1, int const y2)
    {
        std::swap(boxes_[y1], boxes_[y2]);
    }

    void MoveWindow::box_y_line_swap(int const x1, int const x2)
    {
        for(int i=0; i<split_y_; ++i)
        {
            std::swap(boxes_[i][x1], boxes_[i][x2]);
        }
    }

    void MoveWindow::click_box(int const button, point_type const& event_box)
    {
        if(button == FL_LEFT_MOUSE)
        {
            // 通常クリックは交換開始
            move_begin_ = event_box;
        }
        else if(button == FL_MIDDLE_MOUSE)
        {
            // 中クリックは情報クリア
            select_begin_ = select_end_ = point_type{-1,-1};
        }
        else if(button == FL_RIGHT_MOUSE)
        {
            // 右クリックは同時移動ブロックの選択開始
            select_begin_ = event_box;
        }

        latest_button_ = button;
    }

    void MoveWindow::drag_box(point_type const& event_box)
    {
        if(latest_button_ == FL_LEFT_MOUSE)
        {
            // 交換
            if(select_begin_==point_type{-1,-1} || select_end_==point_type{-1,-1})
            {
                // 1つのみ交換
                box_swap(move_begin_, event_box);
                move_begin_ = event_box;
                boxes_redraw();
            }
            else
            {
                // 矩形交換
                if(event_box.manhattan(move_begin_) == 1)
                {
                    //　移動方向
                    point_type direction = event_box - move_begin_;

                    // 端チェック
                    if(
                        (select_begin_.x + direction.x < 0) || (select_end_.x + direction.x >= split_x_) ||
                        (select_begin_.y + direction.y < 0) || (select_end_.y + direction.y >= split_y_)
                        )
                    {
                        // 移動不可
                        return;
                    }

                    if(direction.x == 1)       // 右
                    {
                        for(int i=select_end_.x; i>=select_begin_.x; --i)
                        {
                            for(int j=select_begin_.y; j<=select_end_.y; ++j)
                            {
                                box_swap({i, j}, {i+1, j});
                            }
                        }

                        ++select_begin_.x;
                        ++select_end_.x;
                    }
                    else if(direction.x == -1) // 左
                    {
                        for(int i=select_begin_.x; i<=select_end_.x; ++i)
                        {
                            for(int j=select_begin_.y; j<=select_end_.y; ++j)
                            {
                                box_swap({i, j}, {i-1, j});
                            }
                        }

                        --select_begin_.x;
                        --select_end_.x;
                    }
                    else if(direction.y == 1)  // 下
                    {
                        for(int i=select_end_.y; i>=select_begin_.y; --i)
                        {
                            for(int j=select_begin_.x; j<=select_end_.x; ++j)
                            {
                                box_swap({j, i}, {j, i+1});
                            }
                        }

                        ++select_begin_.y;
                        ++select_end_.y;
                    }
                    else if(direction.y == -1) // 上
                    {
                        for(int i=select_begin_.y; i<=select_end_.y; ++i)
                        {
                            for(int j=select_begin_.x; j<=select_end_.x; ++j)
                            {
                                box_swap({j, i}, {j, i-1});
                            }
                        }

                        --select_begin_.y;
                        --select_end_.y;
                    }
                    boxes_redraw();
                    move_begin_ = event_box;
                }
            }
        }
    }

    void MoveWindow::release_box(int const button, point_type const& event_box)
    {
        latest_button_ = 0;
        if(button == FL_RIGHT_MOUSE)
        {
            // 右クリックは同時移動ブロックの選択終了
            select_end_ = event_box;

            // 正格化: select_{begin,end}_を，beginが左上になるようにする
            point_type new_begin, new_end;
            new_begin.x = std::min(select_begin_.x, select_end_.x);
            new_begin.y = std::min(select_begin_.y, select_end_.y);
            new_end.x   = std::max(select_begin_.x, select_end_.x);
            new_end.y   = std::max(select_begin_.y, select_end_.y);

            select_begin_ = new_begin;
            select_end_   = new_end;
        }
    }

    bool MoveWindow::border_check(int const x, int const y)
    {
        int const window_width  = image_width_  * split_x_;
        int const window_height = image_height_ * split_y_;

        return (x >= 0 && x < window_width && y >= 0 && y < window_height);
    }

    point_type MoveWindow::window_to_point(int const x, int const y)
    {
        return {x/image_width_, y/image_height_};
    }

    int MoveWindow::handle(int event)
    {
        std::lock_guard<std::mutex> lock(handler_mutex_);
        switch(event)
        {
            case FL_PUSH:
            {
                if(border_check(Fl::event_x(), Fl::event_y()))
                {
                    auto const button = Fl::event_button();
                    auto const point = window_to_point(Fl::event_x(), Fl::event_y());
                    click_box(button, point);
                    return 1;
                }
            }
            case FL_DRAG:
            {
                if(border_check(Fl::event_x(), Fl::event_y()))
                {
                    auto const point = window_to_point(Fl::event_x(), Fl::event_y());
                    drag_box(point);
                    return 1;
                }
            }
            case FL_RELEASE:
            {
                if(border_check(Fl::event_x(), Fl::event_y()))
                {
                    auto const button = Fl::event_button();
                    auto const point = window_to_point(Fl::event_x(), Fl::event_y());
                    release_box(button, point);
                    return 1;
                }
            }
        }

        return 0;
    }

} // namespace impl
} // namespace gui
<commit_msg>GUIを拡張可能な設計に切り替えるための修正． MoveBoxの移動でなく，point_typeのswapをすることによって，sort_algorithmの結果から並び替えが可能になる． また，何も置かないBox"point_type(-1,-1)"についての対応．<commit_after>#include <mutex>
#include <vector>
#include <FL/Fl.H>
#include <FL/Fl_Window.H>
#include <FL/Fl_Image.H>
#include <FL/Fl_Box.H>
#include "data_type.hpp"

namespace gui
{
namespace impl
{
    class MoveBox : public Fl_Box
    {
    public:
        MoveBox(point_type const& point, int const x, int const y, int const w, int const h);
        virtual ~MoveBox() = default;

        point_type const& first_point() const;

    private:
        point_type const first_point_;
    };

    class MoveWindow : public Fl_Window
    {
    public:
        MoveWindow(split_image_type const& images, std::string const& window_name);
        virtual ~MoveWindow() = default;

        void set_data(std::vector<std::vector<point_type>> const& possitions);

        // 結果を返す関数であるが，期待通りの働きをしない
        std::vector<std::vector<point_type>> result() const;

        // boxes_配列に基づいて，各boxの位置を算出しなおして再描画する
        void boxes_redraw();

        // boxの並び替え操作に関する関数群(内部でredrawするかどうかはランタイム的に検討)
        void box_swap(point_type const& p1, point_type const& p2);
        void box_x_line_swap(int const y1, int const y2);
        void box_y_line_swap(int const x1, int const x2);

        // boxからのイベントを受け取る関数群
        void click_box(int const button, point_type const& event_box);
        void drag_box(point_type const& event_box);
        void release_box(int const button, point_type const& event_box);

        bool border_check(int const x, int const y);          // TrueならWindow内
        point_type window_to_point(int const x, int const y); // Window上の座標から，point_typeを算出
        int handle(int event) override;

    private:
        splitter const sp_;
        std::string const window_name_;

        int const image_width_;
        int const image_height_;
        int const split_x_;
        int const split_y_;

        int latest_button_;
        point_type move_begin_;
        point_type select_begin_;
        point_type select_end_;
        std::mutex handler_mutex_;

        std::vector<std::vector<unfold_image_type>> image_;
        std::vector<std::vector<Fl_RGB_Image*>> rgbs_;
        std::vector<std::vector<std::unique_ptr<MoveBox>>> boxes_;

        std::vector<std::vector<point_type>> positions_;
    };

    MoveBox::MoveBox(point_type const& point, int const x, int const y, int const w, int const h)
        : Fl_Box(x, y, w, h), first_point_(point)
    {
    }

    point_type const& MoveBox::first_point() const
    {
        return first_point_;
    }

    MoveWindow::MoveWindow(split_image_type const& images, std::string const& window_name)
        : Fl_Window(/*temporary size*/10, 10), window_name_(window_name), sp_(), latest_button_(0),
          select_begin_({-1,-1}), select_end_({-1,-1}),
          image_width_ (images[0][0].cols),
          image_height_(images[0][0].rows),
          split_x_     (images[0]      .size()),
          split_y_     (images         .size())
    {
        int const window_width  = split_x_ * image_width_;
        int const window_height = split_y_ * image_height_;

        this->size(window_width, window_height);
        this->label(window_name_.c_str());

        // 引数の「2次元配置された2次元イメージ」を「2次元配置された1次元イメージ」に変える
        image_.clear();
        image_.reserve(split_y_);
        for(int i=0; i<split_y_; ++i)
        {
            std::vector<unfold_image_type> line;
            line.reserve(split_x_);
            for(int j=0; j<split_x_; ++j)
            {
                line.push_back(sp_.unfold_image(images[i][j]));
            }
            image_.push_back(std::move(line));
        }

        // 「1次元配置された1次元イメージ」をFLTK用のイメージにリンクさせる．(どちらも生きている必要がある)
        rgbs_.clear();
        rgbs_.reserve(split_y_);
        for(int i=0; i<split_y_; ++i)
        {
            std::vector<Fl_RGB_Image*> line;
            line.reserve(split_x_);
            for(int j=0; j<split_x_; ++j)
            {
                line.push_back(new Fl_RGB_Image(image_[i][j].data(), image_width_, image_height_));
            }
            rgbs_.push_back(std::move(line));
        }

        // FLTKイメージをMoveBoxに関連付ける
        boxes_.clear();
        boxes_.reserve(split_y_);
        for(int i=0; i<split_y_; ++i)
        {
            std::vector<std::unique_ptr<MoveBox>> line;
            line.reserve(split_x_);
            for(int j=0; j<split_x_; ++j)
            {
                auto box = std::make_unique<MoveBox>(point_type{j,i}, image_width_*j, image_height_*i, image_width_, image_height_);
                box->image(rgbs_[i][j]);
                line.push_back(std::move(box));
            }
            boxes_.push_back(std::move(line));
        }

        //
        positions_.clear();
        positions_.reserve(split_y_);
        for(int i=0; i<split_y_; ++i)
        {
            std::vector<point_type> line;
            line.reserve(split_x_);
            for(int j=0; j<split_x_; ++j)
            {
                line.push_back(point_type{j, i});
            }
            positions_.push_back(std::move(line));
        }
        boxes_redraw();
    }

    void MoveWindow::set_data(std::vector<std::vector<point_type>> const& positions)
    {
        positions_ = positions;
        boxes_redraw();
    }

    std::vector<std::vector<point_type>> MoveWindow::result() const
    {
        return positions_;
    }

    void MoveWindow::boxes_redraw()
    {
        int const window_width  = positions_.at(0).size() * image_width_;
        int const window_height = positions_.size() * image_height_;
        this->size(window_width, window_height);

        for(int i = 0; i < positions_.size(); ++i)
        {
            for(int j = 0; j < positions_.at(0).size(); ++j)
            {
                if(positions_[i][j] == point_type{-1, -1}) continue; // -1は特別マス

                auto& target_box = boxes_[positions_[i][j].y][positions_[i][j].x];
                target_box->resize(image_width_*j, image_height_*i, image_width_, image_height_);
                target_box->redraw();
            }
        }

        return;
    }

    void MoveWindow::box_swap(point_type const& p1, point_type const& p2)
    {
        std::swap(positions_[p1.y][p1.x], positions_[p2.y][p2.x]);
    }

    void MoveWindow::box_x_line_swap(int const y1, int const y2)
    {
        std::swap(positions_[y1], positions_[y2]);
    }

    void MoveWindow::box_y_line_swap(int const x1, int const x2)
    {
        for(int i=0; i<split_y_; ++i)
        {
            std::swap(positions_[i][x1], positions_[i][x2]);
        }
    }

    void MoveWindow::click_box(int const button, point_type const& event_box)
    {
        if(button == FL_LEFT_MOUSE)
        {
            // 通常クリックは交換開始
            move_begin_ = event_box;
        }
        else if(button == FL_MIDDLE_MOUSE)
        {
            // 中クリックは情報クリア
            select_begin_ = select_end_ = point_type{-1,-1};
        }
        else if(button == FL_RIGHT_MOUSE)
        {
            // 右クリックは同時移動ブロックの選択開始
            select_begin_ = event_box;
        }

        latest_button_ = button;
    }

    void MoveWindow::drag_box(point_type const& event_box)
    {
        if(latest_button_ == FL_LEFT_MOUSE)
        {
            // 交換
            if(select_begin_==point_type{-1,-1} || select_end_==point_type{-1,-1})
            {
                // 1つのみ交換
                box_swap(move_begin_, event_box);
                move_begin_ = event_box;
                boxes_redraw();
            }
            else
            {
                // 矩形交換
                if(event_box.manhattan(move_begin_) == 1)
                {
                    //　移動方向
                    point_type direction = event_box - move_begin_;

                    // 端チェック
                    if(
                        (select_begin_.x + direction.x < 0) || (select_end_.x + direction.x >= split_x_) ||
                        (select_begin_.y + direction.y < 0) || (select_end_.y + direction.y >= split_y_)
                        )
                    {
                        // 移動不可
                        return;
                    }

                    if(direction.x == 1)       // 右
                    {
                        for(int i=select_end_.x; i>=select_begin_.x; --i)
                        {
                            for(int j=select_begin_.y; j<=select_end_.y; ++j)
                            {
                                box_swap({i, j}, {i+1, j});
                            }
                        }

                        ++select_begin_.x;
                        ++select_end_.x;
                    }
                    else if(direction.x == -1) // 左
                    {
                        for(int i=select_begin_.x; i<=select_end_.x; ++i)
                        {
                            for(int j=select_begin_.y; j<=select_end_.y; ++j)
                            {
                                box_swap({i, j}, {i-1, j});
                            }
                        }

                        --select_begin_.x;
                        --select_end_.x;
                    }
                    else if(direction.y == 1)  // 下
                    {
                        for(int i=select_end_.y; i>=select_begin_.y; --i)
                        {
                            for(int j=select_begin_.x; j<=select_end_.x; ++j)
                            {
                                box_swap({j, i}, {j, i+1});
                            }
                        }

                        ++select_begin_.y;
                        ++select_end_.y;
                    }
                    else if(direction.y == -1) // 上
                    {
                        for(int i=select_begin_.y; i<=select_end_.y; ++i)
                        {
                            for(int j=select_begin_.x; j<=select_end_.x; ++j)
                            {
                                box_swap({j, i}, {j, i-1});
                            }
                        }

                        --select_begin_.y;
                        --select_end_.y;
                    }
                    boxes_redraw();
                    move_begin_ = event_box;
                }
            }
        }
    }

    void MoveWindow::release_box(int const button, point_type const& event_box)
    {
        latest_button_ = 0;
        if(button == FL_RIGHT_MOUSE)
        {
            // 右クリックは同時移動ブロックの選択終了
            select_end_ = event_box;

            // 正格化: select_{begin,end}_を，beginが左上になるようにする
            point_type new_begin, new_end;
            new_begin.x = std::min(select_begin_.x, select_end_.x);
            new_begin.y = std::min(select_begin_.y, select_end_.y);
            new_end.x   = std::max(select_begin_.x, select_end_.x);
            new_end.y   = std::max(select_begin_.y, select_end_.y);

            select_begin_ = new_begin;
            select_end_   = new_end;
        }
    }

    bool MoveWindow::border_check(int const x, int const y)
    {
        int const window_width  = image_width_  * split_x_;
        int const window_height = image_height_ * split_y_;

        return (x >= 0 && x < window_width && y >= 0 && y < window_height);
    }

    point_type MoveWindow::window_to_point(int const x, int const y)
    {
        return {x/image_width_, y/image_height_};
    }

    int MoveWindow::handle(int event)
    {
        std::lock_guard<std::mutex> lock(handler_mutex_);
        switch(event)
        {
            case FL_PUSH:
            {
                if(border_check(Fl::event_x(), Fl::event_y()))
                {
                    auto const button = Fl::event_button();
                    auto const point = window_to_point(Fl::event_x(), Fl::event_y());
                    click_box(button, point);
                    return 1;
                }
            }
            case FL_DRAG:
            {
                if(border_check(Fl::event_x(), Fl::event_y()))
                {
                    auto const point = window_to_point(Fl::event_x(), Fl::event_y());
                    drag_box(point);
                    return 1;
                }
            }
            case FL_RELEASE:
            {
                if(border_check(Fl::event_x(), Fl::event_y()))
                {
                    auto const button = Fl::event_button();
                    auto const point = window_to_point(Fl::event_x(), Fl::event_y());
                    release_box(button, point);
                    return 1;
                }
            }
        }

        return 0;
    }

} // namespace impl
} // namespace gui
<|endoftext|>