<commit_before>#include <stdio.h>
#include <stdlib.h>

#include "Buffer.h"

#include "RamCloudBackend.h"
#include "Postgres.h"

const char* ramcloudConnectionString() {
  char* conn = (char*) malloc(128 * sizeof(char));
  snprintf(conn, 127, "fast+udp:host=%s,port=%d",
           RAMCLOUD_HOST.c_str(), RAMCLOUD_PORT);
  printf("connecting to %s\n", conn);
  return conn;
}

//
// RamCloudFactDB
//

// Construct the requisite tables for storing facts
// in RamCloud, if they don't already exist.
RamCloudFactDB::RamCloudFactDB()
    : ramcloud(ramcloudConnectionString()),
      factTableName("Facts")
    {

  factTableId = ramcloud.createTable(factTableName);

  // Create a Postgres connection
  char factQuery[127];
  snprintf(factQuery, 127, "SELECT left_arg, rel, right_arg, weight FROM %s LIMIT 10000000;", PG_TABLE_FACT.c_str());
  PGIterator iter = PGIterator(factQuery);
  uint64_t totalRead = 0;

  while (iter.hasNext()) {
    RAMCloud::MultiWriteObject requestObjects[100];
    RAMCloud::MultiWriteObject* requests[100];
    float confidences[100];
    uint32_t numRequests = 0;
    for (uint16_t i = 0; i < 100; ++i) {
      // If no more to write, break;
      if (!iter.hasNext()) { break; }

      // Get data from Postgres
      PGRow row = iter.next();
      word fact[256];
      uint8_t factLength = 0;
      for (int k = 0; k < 3; ++k) {  // for each argument...
        if (row[k] == NULL) { continue; }
        // ...read it as a string
        uint32_t stringLength = strlen(row[k]);
        char stringWithoutBrackets[stringLength];
        memcpy( stringWithoutBrackets, &row[k][1], stringLength - 2 );
        // ...tokenize it
        const char* token = strtok(stringWithoutBrackets, ",");
        while (token != NULL) {
          char* end;
          fact[factLength] = strtol(token, &end, 10);
          if (end == token || *end != '\0') {
            printf("Could not convert string to integer: %s\n", token);
            std::exit(1);
          }
          factLength += 1;  // append the word to the buffer
          token = strtok(NULL, ",");
        }
        // sanity check to prevent too long of a fact
        if (factLength >= 256) { break; }
      }
      confidences[i] = atof(row[3]);

      // Create request object
      requestObjects[i] =
        RAMCloud::MultiWriteObject(factTableId, fact, factLength,
          &confidences[i], sizeof(float));
      requests[i] = &requestObjects[i];
      numRequests += 1;
      totalRead += 1;
    }
    // Write request
    ramcloud.multiWrite(requests, numRequests);
    if (totalRead % 1000000 == 0) {
      printf("Read %luM facts\n", totalRead / 1000000);
    }
  }

}

const bool RamCloudFactDB::contains(const word* key, const uint8_t wordLength) {
  RAMCloud::Buffer buffer;
  ramcloud.read(factTableId, key, wordLength, &buffer);
  if (buffer.getTotalLength() > 0)
    return true;
  else
    return false;
}

// Clean up
RamCloudFactDB::~RamCloudFactDB() {
//  dropTable(factTableName);  // TODO(gabor) drop me somewhere!
}

FactDB* ReadRamCloudFactDB() {
  return new RamCloudFactDB();
}
<commit_msg>Null terminated my string (oops!)<commit_after>#include <stdio.h>
#include <stdlib.h>

#include "Buffer.h"

#include "RamCloudBackend.h"
#include "Postgres.h"

const char* ramcloudConnectionString() {
  char* conn = (char*) malloc(128 * sizeof(char));
  snprintf(conn, 127, "fast+udp:host=%s,port=%d",
           RAMCLOUD_HOST.c_str(), RAMCLOUD_PORT);
  printf("connecting to %s\n", conn);
  return conn;
}

//
// RamCloudFactDB
//

// Construct the requisite tables for storing facts
// in RamCloud, if they don't already exist.
RamCloudFactDB::RamCloudFactDB()
    : ramcloud(ramcloudConnectionString()),
      factTableName("Facts")
    {

  factTableId = ramcloud.createTable(factTableName);

  // Create a Postgres connection
  char factQuery[127];
  snprintf(factQuery, 127, "SELECT left_arg, rel, right_arg, weight FROM %s LIMIT 10000000;", PG_TABLE_FACT.c_str());
  PGIterator iter = PGIterator(factQuery);
  uint64_t totalRead = 0;

  while (iter.hasNext()) {
    RAMCloud::MultiWriteObject requestObjects[100];
    RAMCloud::MultiWriteObject* requests[100];
    float confidences[100];
    uint32_t numRequests = 0;
    for (uint16_t i = 0; i < 100; ++i) {
      // If no more to write, break;
      if (!iter.hasNext()) { break; }

      // Get data from Postgres
      PGRow row = iter.next();
      word fact[256];
      uint8_t factLength = 0;
      for (int k = 0; k < 3; ++k) {  // for each argument...
        if (row[k] == NULL) { continue; }
        // ...read it as a string
        uint32_t stringLength = strlen(row[k]);
        char stringWithoutBrackets[stringLength + 1];
        printf("input: %s\n", row[k]);
        memcpy( stringWithoutBrackets, &row[k][1], stringLength - 2 );
        stringWithoutBrackets[stringLength] = '\0';
        printf("string without brackets: %s\n", stringWithoutBrackets);

        // ...tokenize it
        const char* token = strtok(stringWithoutBrackets, ",");
        while (token != NULL) {
          char* end;
          fact[factLength] = strtol(token, &end, 10);
          if (end == token || *end != '\0') {
            printf("Could not convert string to integer: %s\n", token);
            std::exit(1);
          }
          factLength += 1;  // append the word to the buffer
          token = strtok(NULL, ",");
        }
        // sanity check to prevent too long of a fact
        if (factLength >= 256) { break; }
      }
      confidences[i] = atof(row[3]);

      // Create request object
      requestObjects[i] =
        RAMCloud::MultiWriteObject(factTableId, fact, factLength,
          &confidences[i], sizeof(float));
      requests[i] = &requestObjects[i];
      numRequests += 1;
      totalRead += 1;
    }
    // Write request
    ramcloud.multiWrite(requests, numRequests);
    if (totalRead % 1000000 == 0) {
      printf("Read %luM facts\n", totalRead / 1000000);
    }
  }

}

const bool RamCloudFactDB::contains(const word* key, const uint8_t wordLength) {
  RAMCloud::Buffer buffer;
  ramcloud.read(factTableId, key, wordLength, &buffer);
  if (buffer.getTotalLength() > 0)
    return true;
  else
    return false;
}

// Clean up
RamCloudFactDB::~RamCloudFactDB() {
//  dropTable(factTableName);  // TODO(gabor) drop me somewhere!
}

FactDB* ReadRamCloudFactDB() {
  return new RamCloudFactDB();
}
<|endoftext|>


<commit_before>#include "generator/generator.h"

// from STL
#include <cmath>
#include <iostream>

// from ROOT
#include "TRandom.h"

// from project
#include "configuration/configuration.h"

namespace cptoymc {
namespace generator {

bool GenerateExpo(TRandom& rndm, double par_expo, double& obs, double min, double max) {
  if (par_expo != 0.) {
    obs = (-1./par_expo)*log(exp(-1.*max*par_expo)+rndm.Uniform()*(exp(-1.*min*par_expo)-exp(-1.*max*par_expo)));
  } else {
    obs = rndm.Uniform(min,max);
  }
  return true;
}

bool GenerateMassBreitWigner(TRandom& rndm, double par_mean, double par_gamma, double& obs_mass_true) {
  obs_mass_true = rndm.BreitWigner(par_mean, par_gamma);
  return true;
}

bool GenerateLognormal(TRandom& rndm, double m, double k, double min, double max,
                       double& obs_sigma_t) {
  unsigned int num_samples(0);

  if (min == max) {
    obs_sigma_t = min;
    return true;
  } else {
    obs_sigma_t = std::exp(std::log(m) + std::log(k)*rndm.Gaus(0,1));
    ++num_samples;

    while (obs_sigma_t > max || obs_sigma_t < min) {
      obs_sigma_t = std::exp(std::log(m) + std::log(k)*rndm.Gaus(0,1));
      ++num_samples;

      if (num_samples % 10000000 == 0) {
        std::cout << "WARNING in GenerateLognormal(rndm, m=" << m << ", k=" << k << ", min=" << min << ", max=" << max << "): Generated " << num_samples << " sample values without one candidate passing. You probably want to check your parameters." << std::endl;
      }
    }

    return true;
  }
}

bool GenerateCPV_P2PV(TRandom& rndm, double par_prod_asym, double par_det_asym,
                      double par_tau, double par_dGamma, double par_dm,
                      double par_Sf, double par_Cf, double par_Df,
                      double par_Sfbar, double par_Cfbar, double par_Dfbar,
                      double& obs_time_true, int& obs_tag_true, int& finalstate) {
  // helper quantities
  double prob_B = (1. - par_prod_asym)/2.;
  double gamma_min = 1./par_tau - std::abs(par_dGamma)/2.;

  // local values of tag and time
  int    val_d = 0;
  int    val_final = 1;
  double val_t = 0.;

  // now hit and miss, stolen from BDecay!!!
  double val_envelope = 0.;
  double val_pdf      = 0.;
  double val_bar_pdf  = 0.;
  while(true) {
    // get initial flavour while taking production asymmetry into account
    val_d = (rndm.Uniform() < prob_B) ? +1 : -1;

    // get time
    val_t = -log(rndm.Uniform())/gamma_min;

    // get pdf value and envelope value
    // the bar CPV params are on -2.0 on default; this makes noe sense for both finalstates, thus generate only for one finalstate
    if(par_Sfbar == -2 || par_Cfbar == -2 || par_Dfbar == -2){
      val_pdf      = BCPV_PDF(val_t, val_d, par_tau, par_dGamma, par_dm, par_Sf, par_Cf, par_Df);
      val_envelope = BCPV_PDF_Envelope(val_t, gamma_min, par_Sf, par_Cf, par_Df);

      if (val_envelope < val_pdf) {
        std::cout << "WARNING: Envelope smaller than PDF!" << std::endl;
      }

      if(val_envelope*rndm.Uniform() > val_pdf) continue;
      else break;
    }
    else{
      val_pdf = BCPV_PDF(val_t, val_d, par_tau, par_dGamma, par_dm, par_Sf, par_Cf, par_Df);
      val_bar_pdf = BCPV_bar_PDF(val_t, val_d, par_tau, par_dGamma, par_dm, par_Sfbar, par_Cfbar, par_Dfbar);

      double prob_finalstate = val_pdf/(val_pdf + val_bar_pdf);
      double prob_detect = (1 - par_det_asym)/2;

      // std::cout << "val_pdf = " << val_pdf << std::endl;
      // std::cout << "val_bar_pdf = " << val_bar_pdf << std::endl;
      // std::cout << "prob_detect = " << prob_detect << std::endl;
      // std::cout << "C = " << par_Cf << std::endl;
      // std::cout << "Cbar = " << par_Cfbar << std::endl;
      // std::cout << "combined = " << val_pdf/(val_pdf + val_bar_pdf) << std::endl;
      // std::cout << "combined bar = " << val_bar_pdf/(val_pdf + val_bar_pdf) << std::endl;
      // std::cout << "prob_finalstate = " << prob_finalstate << "\n" << std::endl;

      val_final = (rndm.Uniform() < prob_detect) ? +1 : -1;

      if(val_final  == 1) {
        val_envelope = BCPV_PDF_Envelope(val_t, gamma_min, par_Sf, par_Cf, par_Df);
        if (val_envelope < val_pdf) std::cout << "WARNING: Envelope smaller than PDF!" << std::endl;
        if(val_envelope*rndm.Uniform() > val_pdf) continue;
        else break;
      }
      else {
        val_envelope = BCPV_PDF_Envelope(val_t, gamma_min, par_Sfbar, par_Cfbar, par_Dfbar);
        if (val_envelope < val_bar_pdf) std::cout << "WARNING: Envelope smaller than PDF!" << std::endl;
        if(val_envelope*rndm.Uniform() > val_bar_pdf) continue;
        else break;
      }
    }
  }

  obs_tag_true  = val_d;
  obs_time_true = val_t;
  finalstate    = val_final;

  return true;
}

double BCPV_PDF(double t, double d, double tau, double dGamma, double dm,
                double Sf, double Cf, double Df) {
  return exp(-t/tau)*(cosh(dGamma*t/2.)+Df*sinh(dGamma*t/2.)+d*Cf*cos(dm*t)-d*Sf*sin(dm*t));
}

double BCPV_bar_PDF(double t, double d, double tau, double dGamma, double dm,
                    double Sfbar, double Cfbar, double Dfbar) {
  return exp(-t/tau)*(cosh(dGamma*t/2.)+Dfbar*sinh(dGamma*t/2.)-d*Cfbar*cos(dm*t)+d*Sfbar*sin(dm*t));
}

double BCPV_PDF_Envelope(double t, double gamma_min, double S, double C, double D) {
  return exp(-t*gamma_min)*(1.+std::abs(D)+sqrt(S*S+C*C));
}

bool GenerateResolSingleGauss(TRandom& rndm, double par_bias, double par_sigma, double obs_true, double& obs_meas) {
  obs_meas = obs_true;
  obs_meas += rndm.Gaus(par_bias, par_sigma);

  return true;
}

bool GenerateResolSingleGaussPerEvent(TRandom& rndm, double par_bias, double par_scale, double obs_per_event_error, double obs_true, double& obs_meas) {
  obs_meas = obs_true;
  obs_meas += rndm.Gaus(par_bias, par_scale*obs_per_event_error);

  return true;
}

bool GenerateEtaFlat(TRandom& rndm, double& obs_eta) {
  return GenerateEtaFlat(rndm,0.0,0.5,obs_eta);
}

bool GenerateEtaFlat(TRandom& rndm, double obs_eta_min, double obs_eta_max, double& obs_eta) {
  obs_eta = rndm.Uniform(obs_eta_min,obs_eta_max);
  return true;
}

bool GenerateEtaGauss(TRandom& rndm, double m, double s, double obs_eta_min, double obs_eta_max, double& obs_eta) {
  unsigned int num_samples(0);

  // s is set to -1.0 on default; a negative Gaussian width does not make sense, thus generate a uniform distribution
  if (s < 0.0) {
    obs_eta = rndm.Uniform(obs_eta_min,obs_eta_max);
    return true;
  } else {
    obs_eta = rndm.Gaus(m,s);
    ++num_samples;

    while (obs_eta > obs_eta_max || obs_eta < obs_eta_min) {
      obs_eta = rndm.Gaus(m,s);
      ++num_samples;

      if (num_samples % 10000000 == 0) {
        std::cout << "WARNING in cptoymc::generator::GenerateEtaGauss(rndm, m=" << m << ", s=" << s << ", obs_eta_min=" << obs_eta_min << ", obs_eta_max=" << obs_eta_max << "): Generated " << num_samples << " sample values without one candidate passing. You probably want to check your parameters." << std::endl;
      }
    }

    return true;
  }
}

bool GenerateTag(TRandom& rndm, double par_omega, double par_domega,
                 const int par_tag_true_B, const int par_tag_true_Bb,
                 const int par_tag_B,      const int par_tag_Bb,
                 int obs_tag_true, int& obs_tag_meas) {
  //if (par_omega > 0.5) par_omega = 0.5;
  if (par_omega < 0.0) par_omega = 0.;

  int correct_tag = 0;

  // always assume that omega_B  = omega + dOmega/2 for true B mesons
  // and that           omega_Bb = omega - dOmega/2 for true Bb mesons
  if (obs_tag_true == par_tag_true_B) {
    par_omega += par_domega/2.; // B meson case
    correct_tag = par_tag_B;
  }
  else if (obs_tag_true == par_tag_true_Bb) {
    par_omega -= par_domega/2.; // Bb meson case
    correct_tag = par_tag_Bb;
  } else {
    std::cout << "Cannot interpret true tag of " << obs_tag_true << ". Failed." << std::endl;
    return false;
  }

  if (rndm.Uniform() < par_omega) {
    obs_tag_meas = -1*correct_tag;
  } else {
    obs_tag_meas = correct_tag;
  }
  return true;
}

bool GenerateTag(TRandom& rndm, double par_omega, double par_domega, int obs_tag_true, int& obs_tag_meas) {
  return GenerateTag(rndm, par_omega, par_domega, +1, -1, +1, -1, obs_tag_true, obs_tag_meas);
}

bool GenerateTag(TRandom& rndm,
                 std::function<double(double)>& func_omega,
                 std::function<double(double)>& func_domega,
                 const int par_tag_true_B, const int par_tag_true_Bb,
                 const int par_tag_B,      const int par_tag_Bb,
                 int obs_tag_true, double obs_eta, int& obs_tag_meas) {
  return GenerateTag(rndm, func_omega(obs_eta), func_domega(obs_eta),
                     par_tag_true_B, par_tag_true_Bb, par_tag_B, par_tag_Bb,
                     obs_tag_true, obs_tag_meas);
}

bool GenerateRandomTag(TRandom& rndm, int& obs_tag_meas) {
  obs_tag_meas = (rndm.Uniform() < 0.5) ? +1 : -1;
  return true;
}

int yieldToGenerate(TRandom& rndm, double yield_exp) {
  return rndm.Poisson(yield_exp);
}

} // namespace cptoymc
} // namespace generator

<commit_msg>tidied up generator.cpp<commit_after>#include "generator/generator.h"

// from STL
#include <cmath>
#include <iostream>

// from ROOT
#include "TRandom.h"

// from project
#include "configuration/configuration.h"

namespace cptoymc {
namespace generator {

bool GenerateExpo(TRandom& rndm, double par_expo, double& obs, double min, double max) {
  if (par_expo != 0.) {
    obs = (-1./par_expo)*log(exp(-1.*max*par_expo)+rndm.Uniform()*(exp(-1.*min*par_expo)-exp(-1.*max*par_expo)));
  } else {
    obs = rndm.Uniform(min,max);
  }
  return true;
}

bool GenerateMassBreitWigner(TRandom& rndm, double par_mean, double par_gamma, double& obs_mass_true) {
  obs_mass_true = rndm.BreitWigner(par_mean, par_gamma);
  return true;
}

bool GenerateLognormal(TRandom& rndm, double m, double k, double min, double max,
                       double& obs_sigma_t) {
  unsigned int num_samples(0);

  if (min == max) {
    obs_sigma_t = min;
    return true;
  } else {
    obs_sigma_t = std::exp(std::log(m) + std::log(k)*rndm.Gaus(0,1));
    ++num_samples;

    while (obs_sigma_t > max || obs_sigma_t < min) {
      obs_sigma_t = std::exp(std::log(m) + std::log(k)*rndm.Gaus(0,1));
      ++num_samples;

      if (num_samples % 10000000 == 0) {
        std::cout << "WARNING in GenerateLognormal(rndm, m=" << m << ", k=" << k << ", min=" << min << ", max=" << max << "): Generated " << num_samples << " sample values without one candidate passing. You probably want to check your parameters." << std::endl;
      }
    }

    return true;
  }
}

bool GenerateCPV_P2PV(TRandom& rndm, double par_prod_asym, double par_det_asym,
                      double par_tau, double par_dGamma, double par_dm,
                      double par_Sf, double par_Cf, double par_Df,
                      double par_Sfbar, double par_Cfbar, double par_Dfbar,
                      double& obs_time_true, int& obs_tag_true, int& finalstate) {
  // helper quantities
  double prob_B = (1. - par_prod_asym)/2.;
  double gamma_min = 1./par_tau - std::abs(par_dGamma)/2.;

  // local values of tag and time
  int    val_d = 0;
  int    val_final = 1;
  double val_t = 0.;

  // now hit and miss, stolen from BDecay!!!
  double val_envelope = 0.;
  double val_pdf      = 0.;
  double val_bar_pdf  = 0.;
  while(true) {
    // get initial flavour while taking production asymmetry into account
    val_d = (rndm.Uniform() < prob_B) ? +1 : -1;

    // get time
    val_t = -log(rndm.Uniform())/gamma_min;

    // get pdf value and envelope value
    // the bar CPV params are on -2.0 on default; this makes noe sense for both finalstates, thus generate only for one finalstate
    if(par_Sfbar == -2 || par_Cfbar == -2 || par_Dfbar == -2){
      val_pdf      = BCPV_PDF(val_t, val_d, par_tau, par_dGamma, par_dm, par_Sf, par_Cf, par_Df);
      val_envelope = BCPV_PDF_Envelope(val_t, gamma_min, par_Sf, par_Cf, par_Df);

      if (val_envelope < val_pdf) {
        std::cout << "WARNING: Envelope smaller than PDF!" << std::endl;
      }

      if(val_envelope*rndm.Uniform() > val_pdf) continue;
      else break;
    }
    else{
      double prob_detect = (1 - par_det_asym)/2;

      val_final = (rndm.Uniform() < prob_detect) ? +1 : -1;

      if(val_final  == 1) {
        val_pdf = BCPV_PDF(val_t, val_d, par_tau, par_dGamma, par_dm, par_Sf, par_Cf, par_Df);
        val_envelope = BCPV_PDF_Envelope(val_t, gamma_min, par_Sf, par_Cf, par_Df);
        if (val_envelope < val_pdf) std::cout << "WARNING: Envelope smaller than PDF!" << std::endl;
        if(val_envelope*rndm.Uniform() > val_pdf) continue;
        else break;
      }
      else {
        val_bar_pdf = BCPV_bar_PDF(val_t, val_d, par_tau, par_dGamma, par_dm, par_Sfbar, par_Cfbar, par_Dfbar);
        val_envelope = BCPV_PDF_Envelope(val_t, gamma_min, par_Sfbar, par_Cfbar, par_Dfbar);
        if (val_envelope < val_bar_pdf) std::cout << "WARNING: Envelope smaller than PDF!" << std::endl;
        if(val_envelope*rndm.Uniform() > val_bar_pdf) continue;
        else break;
      }
    }
  }

  obs_tag_true  = val_d;
  obs_time_true = val_t;
  finalstate    = val_final;

  return true;
}

double BCPV_PDF(double t, double d, double tau, double dGamma, double dm,
                double Sf, double Cf, double Df) {
  return exp(-t/tau)*(cosh(dGamma*t/2.)+Df*sinh(dGamma*t/2.)+d*Cf*cos(dm*t)-d*Sf*sin(dm*t));
}

double BCPV_bar_PDF(double t, double d, double tau, double dGamma, double dm,
                    double Sfbar, double Cfbar, double Dfbar) {
  return exp(-t/tau)*(cosh(dGamma*t/2.)+Dfbar*sinh(dGamma*t/2.)-d*Cfbar*cos(dm*t)+d*Sfbar*sin(dm*t));
}

double BCPV_PDF_Envelope(double t, double gamma_min, double S, double C, double D) {
  return exp(-t*gamma_min)*(1.+std::abs(D)+sqrt(S*S+C*C));
}

bool GenerateResolSingleGauss(TRandom& rndm, double par_bias, double par_sigma, double obs_true, double& obs_meas) {
  obs_meas = obs_true;
  obs_meas += rndm.Gaus(par_bias, par_sigma);

  return true;
}

bool GenerateResolSingleGaussPerEvent(TRandom& rndm, double par_bias, double par_scale, double obs_per_event_error, double obs_true, double& obs_meas) {
  obs_meas = obs_true;
  obs_meas += rndm.Gaus(par_bias, par_scale*obs_per_event_error);

  return true;
}

bool GenerateEtaFlat(TRandom& rndm, double& obs_eta) {
  return GenerateEtaFlat(rndm,0.0,0.5,obs_eta);
}

bool GenerateEtaFlat(TRandom& rndm, double obs_eta_min, double obs_eta_max, double& obs_eta) {
  obs_eta = rndm.Uniform(obs_eta_min,obs_eta_max);
  return true;
}

bool GenerateEtaGauss(TRandom& rndm, double m, double s, double obs_eta_min, double obs_eta_max, double& obs_eta) {
  unsigned int num_samples(0);

  // s is set to -1.0 on default; a negative Gaussian width does not make sense, thus generate a uniform distribution
  if (s < 0.0) {
    obs_eta = rndm.Uniform(obs_eta_min,obs_eta_max);
    return true;
  } else {
    obs_eta = rndm.Gaus(m,s);
    ++num_samples;

    while (obs_eta > obs_eta_max || obs_eta < obs_eta_min) {
      obs_eta = rndm.Gaus(m,s);
      ++num_samples;

      if (num_samples % 10000000 == 0) {
        std::cout << "WARNING in cptoymc::generator::GenerateEtaGauss(rndm, m=" << m << ", s=" << s << ", obs_eta_min=" << obs_eta_min << ", obs_eta_max=" << obs_eta_max << "): Generated " << num_samples << " sample values without one candidate passing. You probably want to check your parameters." << std::endl;
      }
    }

    return true;
  }
}

bool GenerateTag(TRandom& rndm, double par_omega, double par_domega,
                 const int par_tag_true_B, const int par_tag_true_Bb,
                 const int par_tag_B,      const int par_tag_Bb,
                 int obs_tag_true, int& obs_tag_meas) {
  //if (par_omega > 0.5) par_omega = 0.5;
  if (par_omega < 0.0) par_omega = 0.;

  int correct_tag = 0;

  // always assume that omega_B  = omega + dOmega/2 for true B mesons
  // and that           omega_Bb = omega - dOmega/2 for true Bb mesons
  if (obs_tag_true == par_tag_true_B) {
    par_omega += par_domega/2.; // B meson case
    correct_tag = par_tag_B;
  }
  else if (obs_tag_true == par_tag_true_Bb) {
    par_omega -= par_domega/2.; // Bb meson case
    correct_tag = par_tag_Bb;
  } else {
    std::cout << "Cannot interpret true tag of " << obs_tag_true << ". Failed." << std::endl;
    return false;
  }

  if (rndm.Uniform() < par_omega) {
    obs_tag_meas = -1*correct_tag;
  } else {
    obs_tag_meas = correct_tag;
  }
  return true;
}

bool GenerateTag(TRandom& rndm, double par_omega, double par_domega, int obs_tag_true, int& obs_tag_meas) {
  return GenerateTag(rndm, par_omega, par_domega, +1, -1, +1, -1, obs_tag_true, obs_tag_meas);
}

bool GenerateTag(TRandom& rndm,
                 std::function<double(double)>& func_omega,
                 std::function<double(double)>& func_domega,
                 const int par_tag_true_B, const int par_tag_true_Bb,
                 const int par_tag_B,      const int par_tag_Bb,
                 int obs_tag_true, double obs_eta, int& obs_tag_meas) {
  return GenerateTag(rndm, func_omega(obs_eta), func_domega(obs_eta),
                     par_tag_true_B, par_tag_true_Bb, par_tag_B, par_tag_Bb,
                     obs_tag_true, obs_tag_meas);
}

bool GenerateRandomTag(TRandom& rndm, int& obs_tag_meas) {
  obs_tag_meas = (rndm.Uniform() < 0.5) ? +1 : -1;
  return true;
}

int yieldToGenerate(TRandom& rndm, double yield_exp) {
  return rndm.Poisson(yield_exp);
}

} // namespace cptoymc
} // namespace generator

<|endoftext|>


<commit_before><commit_msg>`AnyValue`: refactoring: `std::snprintf` -> `std::stringstream`.<commit_after><|endoftext|>


<commit_before>#ifndef TOPAZ_GL_IMPORTED_SHADERS_HPP
#define TOPAZ_GL_IMPORTED_SHADERS_HPP
#include <string_view>
#include

#define STRINGIFY(X) STRINGIFY2(X)
#define STRINGIFY2(X) #X
#define CONCAT(X, Y) X##Y
#define JOIN(X, Y, Z, W) X##Y##Z##W

/**
 * @ingroup tz_gl2
 * @fn ImportedShaderHeader(shader_name, shader_type)
 * @hideinitializer
 * Retrieves a file path which is intended to be #included in a application's main source file. Once included, the imported shader's source code can be retrieved as a constexpr string_view via @ref ImportedShaderSource
 */
#if TZ_VULKAN
#define ImportedShaderHeader(shader_name, shader_type) STRINGIFY2(shader_name.shader_type.tzsl.spv.hpp)
#elif TZ_OGL
#define ImportedShaderHeader(shader_name, shader_type) STRINGIFY2(shader_name.shader_type.tzsl.glsl.hpp)
#endif

/**
 * @ingroup tz_gl2
 * @fn ImportedShaderSource(shader_name, shader_type)
 * @hideinitializer
 * Retrieves a token representing a `std::string_view` which contains the shader's compiled results. On Vulkan, this will be SPIRV, and GLSL source code for OpenGL. You can only retrieve an imported shader's source if the imported header has been included via @ref ImportedShaderHeader
 */
#if TZ_VULKAN
#define ImportedShaderSource(shader_name, shader_type) []()->std::string_view{std::span<const std::byte> shader_bin = std::as_bytes(std::span<const std::int8_t>(JOIN(shader_name, _, shader_type, _tzsl_spv))); return std::string_view{reinterpret_cast<const char*>(shader_bin.data()), shader_bin.size_bytes()};}()
#elif TZ_OGL
#define ImportedShaderSource(shader_name, shader_type) []()->std::string_view{std::span<const std::byte> shader_bin = std::as_bytes(std::span<const std::int8_t>(JOIN(shader_name, _, shader_type, _tzsl_glsl))); return std::string_view{reinterpret_cast<const char*>(shader_bin.data()), shader_bin.size_bytes()};}()
#endif

#endif // TOPAZ_GL_IMPORTED_SHADERS_HPP
<commit_msg>* Cleaned up imported_shaders.hpp after recent removal of tzslc header exports<commit_after>#ifndef TOPAZ_GL_IMPORTED_SHADERS_HPP
#define TOPAZ_GL_IMPORTED_SHADERS_HPP
#include <string_view>
#include

#define TZ_MACROHACKERY_STRINGIFY(X) TZ_MACROHACKERY_STRINGIFY2(X)
#define TZ_MACROHACKERY_STRINGIFY2(X) #X
#define TZ_MACROHACKERY_CONCAT(X, Y) X##Y
#define TZ_MACROHACKERY_JOIN(X, Y, Z, W) X##Y##Z##W
#if TZ_VULKAN
#define TZ_MACROHACKERY_JOIN_SHADER(X, Y, Z) TZ_MACROHACKERY_JOIN(X, Y, Z, _tzsl_spv)
#elif TZ_OGL
#define TZ_MACROHACKERY_JOIN_SHADER(X, Y, Z) TZ_MACROHACKERY_JOIN(X, Y, Z, _tzsl_glsl)
#endif
/**
 * @ingroup tz_gl2
 * @fn ImportedShaderHeader(shader_name, shader_type)
 * @hideinitializer
 * Retrieves a file path which is intended to be #included in a application's main source file. Once included, the imported shader's source code can be retrieved as a constexpr string_view via @ref ImportedShaderSource
 */
#if TZ_VULKAN
#define ImportedShaderHeader(shader_name, shader_type) TZ_MACROHACKERY_STRINGIFY2(shader_name.shader_type.tzsl.spv.hpp)
#elif TZ_OGL
#define ImportedShaderHeader(shader_name, shader_type) TZ_MACROHACKERY_STRINGIFY2(shader_name.shader_type.tzsl.glsl.hpp)
#endif

/**
 * @ingroup tz_gl2
 * @fn ImportedShaderSource(shader_name, shader_type)
 * @hideinitializer
 * Retrieves a token representing a `std::string_view` which contains the shader's compiled results. On Vulkan, this will be SPIRV, and GLSL source code for OpenGL. You can only retrieve an imported shader's source if the imported header has been included via @ref ImportedShaderHeader
 */

#define ImportedShaderSource(shader_name, shader_type) []()->std::string_view{std::span<const std::byte> shader_bin = std::as_bytes(std::span<const std::int8_t>(TZ_MACROHACKERY_JOIN_SHADER(shader_name, _, shader_type))); return std::string_view{reinterpret_cast<const char*>(shader_bin.data()), shader_bin.size_bytes()};}()

#endif // TOPAZ_GL_IMPORTED_SHADERS_HPP
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseStamped.h>
#include <boost/thread/mutex.hpp>
#include <global_planner/GarbageCan.h>
#include "std_msgs/Int32.h"
#include "std_msgs/String.h"

// The global planner uses the actionlib to do the following:
// Start in safe mode
//
// In safe state
//  cancel all goals and no april tag callbacks
//  if start command:
//   Transition to search state
//  if goto state command:
//   Transition to given state
//
// In search state
//  travel to poses A,B,etc... on known map
//  If a trashcan is found at any time:
//   Transition to approach_trash state
//  else if hit final search state:
//   Transition to end state
//
// In approach_trash state
//  travel to infront of trashcan
//  if reach trash pose:
//   transition to dump trash state
//
// In dump trash state
//  travel to dump trash position
//  if reach dump trash position:
//   transition to end state
//
// In end state
//  travel to final pose
//  if reaach final pose:
//   transition to safe state

// Convenience Typedef
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

boost::shared_ptr<MoveBaseClient> action_client_ptr;

// April tag subscriber
ros::Subscriber sub;
ros::Publisher cmd_pub;
ros::Publisher sound_pub;

// State Machine for Fall Demo #2
enum State {
  SAFE = 0,
  SEARCH_A = 1,
  SEARCH_B = 2,
  SEARCH_C = 3,
  SEARCH_D = 4,
  SEARCH_E = 5,
  APPROACH_TRASH = 6,
  DUMP_TRASH = 7,
  END = 8,
  PAUSE = 99,
  RESUME = 100,
  START = 999,
};
State currState = START;
State state_before_pause = SAFE;
ros::Time timeofpause;

// Command value read from topic, or on transition to SAFE
int command_value = -1;

bool given_start_state = false;

// Search poses
struct search_pose
{
  // x, y, rot
  double x, y, rz, rw;
};
search_pose search_poses[] = { {1, -0.8, 0, 1},
                               {1.45, 1.0, 1, 0},
                               {-2, .85, -.73, .68},
                               {-1.8, -0.8, 0, 1},
                               {0, -1, 0.67, 0.72}
                             };
// End position
search_pose end_pose = {0, 0, 0, 1};

void send_sound_num(std::string str, int num_times)
{
  std_msgs::String s;
  s.data = str;
  sound_pub.publish(s);
  ros::spinOnce();
}

void send_sound(std::string str)
{
  send_sound_num(str, 1);
}

// Sets given goal to given x,y and rotation quat rz,rw
void setGoalPoseRaw(double x, double y, double rz, double rw,
                 move_base_msgs::MoveBaseGoal &goal) {
  goal.target_pose.pose.position.x = x;
  goal.target_pose.pose.position.y = y;
  goal.target_pose.pose.orientation.z = rz;
  goal.target_pose.pose.orientation.w = rw;
}
void setGoalPose(const search_pose &s,
                 move_base_msgs::MoveBaseGoal &goal) {
  setGoalPoseRaw(s.x, s.y, s.rz, s.rw, goal);
}

// Given a trashcan as a PoseStamped messge, returns the goal pose in front of it
void getGoalPoseFromTrashcan(const geometry_msgs::PoseStamped& msg,
                             move_base_msgs::MoveBaseGoal &goal) {
  double theta = 2 * acos(msg.pose.orientation.w);
  setGoalPoseRaw(msg.pose.position.x + 0.5*cos(theta),
                 msg.pose.position.y + 0.5*sin(theta),
                 msg.pose.orientation.z,
                 msg.pose.orientation.w,
                 goal);
  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();
}

bool found_trashcan = false;

// Callback for new april tags
void trashcanTagSearcherCallback(const global_planner::GarbageCan::ConstPtr& msg) {
  // ROS_INFO("April Tag Callback!");

  geometry_msgs::PoseStamped pose = msg->pose;
  int can_num = msg->can_num;

   ROS_INFO("Can number: %d", msg->can_num);

  // If april tag is id 6 (trashcan)
  if (can_num == 6) {
    // ROS_INFO("Header Match!");
    found_trashcan = true;

    move_base_msgs::MoveBaseGoal goal;

    // Set goal position to in front of april tag
    getGoalPoseFromTrashcan(pose, goal);

    // http://mirror.umd.edu/roswiki/doc/diamondback/api/actionlib/html/classactionlib_1_1simple__action__client_1_1SimpleActionClient.html#a6bdebdd9f43a470ecd361d2c8b743188
    // If a previous goal is already active when this is called.
    // We simply forget about that goal and start tracking the new goal.
    // No cancel requests are made.
    ROS_INFO("TRASH GOAL SENT!");
    action_client_ptr->sendGoal(goal);
  }
}

void transition(State state, ros::NodeHandle &n) {
  //Only transition if it's new
  if (currState != state) {
    State lastState = currState; //used when pausing
    currState = state;

    ROS_INFO("-- STATE TRANSITION %d --", currState);

    int chosen_search_pose = -1;
    switch(currState) {
      case SAFE:
      action_client_ptr->cancelAllGoals(); // Cancel any current move goals
      sub.shutdown();
      command_value = 0;
      break;

      case SEARCH_A:
      chosen_search_pose = 0;
      ROS_INFO("SENDING POSE SEARCH_A to /move_base/goal");
      break;

      case SEARCH_B:
      chosen_search_pose = 1;
      ROS_INFO("SENDING POSE SEARCH_B to /move_base/goal");
      break;

      case SEARCH_C:
      chosen_search_pose = 2;
      ROS_INFO("SENDING POSE SEARCH_C to /move_base/goal");
      break;

      case SEARCH_D:
      chosen_search_pose = 3;
      ROS_INFO("SENDING POSE SEARCH_D to /move_base/goal");
      break;

      case SEARCH_E:
      chosen_search_pose = 4;
      ROS_INFO("SENDING POSE SEARCH_E to /move_base/goal");
      break;

      case APPROACH_TRASH:
      send_sound("approaching_trash");
      sub.shutdown();
      break;

      case DUMP_TRASH:
      sub.shutdown();
      {
        // Set goal to search pose
        move_base_msgs::MoveBaseGoal goal;
        setGoalPose(end_pose, goal);
        goal.target_pose.header.frame_id = "map";
        goal.target_pose.header.stamp = ros::Time::now();

        // Send goal to action client
        action_client_ptr->sendGoal(goal);
        ROS_INFO("END GOAL SENT");
      }

      break;

      case END:
      sub.shutdown();
      break;

      case PAUSE:
      ROS_INFO("PAUSE received... cancel all goals");
      state_before_pause = lastState;
      timeofpause = ros::Time::now();
      action_client_ptr->cancelAllGoals(); // Cancel any current move goals
      sub.shutdown();
      break;

      case RESUME:
      ROS_ERROR("Interesting... we are trying to transition to 'resume'. weird");
      break;
    }

    std_msgs::Int32 cmd_msg;
    cmd_msg.data = currState;
    cmd_pub.publish(cmd_msg);
    ROS_INFO_STREAM("Sent command state: "<<cmd_msg.data);

    // If searching
    if (chosen_search_pose >= 0) {
      // Subscribe to trashcan searcher
      sub = n.subscribe("garbageCan", 10, trashcanTagSearcherCallback);

      // Set goal to search pose
      move_base_msgs::MoveBaseGoal goal;
      setGoalPose(search_poses[chosen_search_pose], goal);
      goal.target_pose.header.frame_id = "map";
      goal.target_pose.header.stamp = ros::Time::now();

      // Send goal to action client
      action_client_ptr->sendGoal(goal);
      ROS_INFO("GOAL SENT");
    }
  }
}

void commandCallback(const std_msgs::Int32::ConstPtr& msg) {
  if (command_value != msg->data)
  {
    command_value = msg->data;
    ROS_INFO("Received command %d", command_value);
  }
}

int main(int argc, char** argv){
  // ROS Node Initialization
  ros::init(argc, argv, "global_planner");
  ros::NodeHandle n;

  // Subscribe to command node
  ros::Subscriber cmd_sub = n.subscribe("cmd_state", 10, commandCallback);
  // Publisher to send current state to the rest of the world when transition happens
  cmd_pub = n.advertise<std_msgs::Int32>("cmd_state", 10);

  // Publisher to send current state to the rest of the world when transition happens
  sound_pub = n.advertise<std_msgs::String>("play_sound", 10);
  ros::spinOnce();

  ROS_INFO_STREAM("Sending test sound");
  sleep(1);
  send_sound("beep.wav");

  action_client_ptr.reset( new MoveBaseClient("move_base", true) );

  // Create Rate Object for sleeping
  ros::Rate r(100);

  // Wait for the action server to come up
  while(ros::ok() && !action_client_ptr->waitForServer(ros::Duration(1.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  ROS_INFO("Starting global planner");

  while(ros::ok()){
    if (command_value == 0) {
      given_start_state = false;
      transition(SAFE, n);
    }
    if (found_trashcan) {
      found_trashcan = false;
      transition(APPROACH_TRASH, n);
    }

    // State machine command override from safe to chosen state
    switch(currState) {
      case START:
        if (command_value > 0 && command_value < 6)
        {
          given_start_state = true;
          for (int i=0; i<5; i++)
          {
            send_sound("beep.wav");
            sleep(1.5);
          }
          State s = static_cast<State>(command_value);
          transition(s, n);
        }
        break;
      case SAFE:
        switch (command_value) {
          State s;
          case SEARCH_A:
          case SEARCH_B:
          case SEARCH_C:
          case SEARCH_D:
          case SEARCH_E:
          s = static_cast<State>(command_value);
          transition(s, n);
          break;
          case APPROACH_TRASH:
          transition(APPROACH_TRASH, n);
          break;
          case DUMP_TRASH:
          transition(DUMP_TRASH, n);
          break;
          case 8:
          transition(END, n);
          break;
          case 99:
          transition(PAUSE, n);
          break;
          case 100:
          transition(RESUME, n);
        }
        break;

      case SEARCH_A:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 2) {
          send_sound("beep.wav");
          transition(SEARCH_B, n);
        }
        break;

      case SEARCH_B:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 3) {

          send_sound("beep.wav");
          transition(SEARCH_C, n);
        }
        break;

      case SEARCH_C:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 4) {
          send_sound("beep.wav");
          transition(SEARCH_D, n);
        }
        break;

      case SEARCH_D:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 5) {
          send_sound("beep.wav");
          transition(SEARCH_E, n);
        }
        break;

      case SEARCH_E:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 6) {
          send_sound("beep.wav");
          transition(DUMP_TRASH, n);
        }
        break;

      case APPROACH_TRASH:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 7) {
          send_sound("seen_goal.wav");
          transition(DUMP_TRASH, n);
        }
        break;

      // Dump trash means go to the end search_pos 6
      case DUMP_TRASH:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 8) {
          send_sound("beep.wav");
          transition(END, n);
        }
        break;

      case END:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        send_sound("seen_goal.wav");
        ros::spinOnce();
        sleep(1);
        exit(1);
        break;

      case PAUSE:
        if (command_value == RESUME)
        {
          transition(state_before_pause, n);
        }
        if (ros::Time::now() - timeofpause > ros::Duration(10))
        {
          ROS_ERROR("We've been paused for too long. Let's resume anyways!");
          transition(state_before_pause,n);
        }
    }
    if (given_start_state && action_client_ptr->getState() == actionlib::SimpleClientGoalState::ABORTED) {
      transition(SAFE, n);
      send_sound("crash.wav");
    }

    // Update callbacks after the fact, for next loop iteration.
    ros::spinOnce();

    r.sleep();
  }
}
<commit_msg>P3-DX Progress review 8 code<commit_after>#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseStamped.h>
#include <boost/thread/mutex.hpp>
#include <global_planner/GarbageCan.h>
#include <global_planner/SoundMsg.h>
#include "std_msgs/Int32.h"
#include "std_msgs/String.h"

// The global planner uses the actionlib to do the following:
// Start in safe mode
//
// In safe state
//  cancel all goals and no april tag callbacks
//  if start command:
//   Transition to search state
//  if goto state command:
//   Transition to given state
//
// In search state
//  travel to poses A,B,etc... on known map
//  If a trashcan is found at any time:
//   Transition to approach_trash state
//  else if hit final search state:
//   Transition to end state
//
// In approach_trash state
//  travel to infront of trashcan
//  if reach trash pose:
//   transition to dump trash state
//
// In dump trash state
//  travel to dump trash position
//  if reach dump trash position:
//   transition to end state
//
// In end state
//  travel to final pose
//  if reaach final pose:
//   transition to safe state

// Convenience Typedef
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

boost::shared_ptr<MoveBaseClient> action_client_ptr;

// April tag subscriber
ros::Subscriber sub;
ros::Publisher cmd_pub;
ros::Publisher sound_pub;

// State Machine for Fall Demo #2
enum State {
  SAFE = 0,
  SEARCH_A = 1,
  SEARCH_B = 2,
  SEARCH_C = 3,
  SEARCH_D = 4,
  SEARCH_E = 5,
  APPROACH_TRASH = 6,
  DUMP_TRASH = 7,
  END = 8,
  PAUSE = 99,
  RESUME = 100,
  START = 999,
};
State currState = START;
State state_before_pause = SAFE;
ros::Time timeofpause;

// Command value read from topic, or on transition to SAFE
int command_value = -1;

bool given_start_state = false;

// Search poses
struct search_pose
{
  // x, y, rot
  double x, y, rz, rw;
};
search_pose search_poses[] = { {1, -0.8, 0, 1},
                               {1.45, 1.0, 1, 0},
                               {-2, .85, -.73, .68},
                               {-1.8, -0.8, 0, 1},
                               {0, -1, 0.67, 0.72}
                             };
// End position
search_pose end_pose = {0, 0, 0, 1};

void send_sound_num(std::string str, int num_times)
{
  global_planner::SoundMsg s;
  s.filename = str;
  s.num_times = num_times;
  s.text_output = std::string();
  sound_pub.publish(s);
  ros::spinOnce();
}

void send_sound(std::string str)
{
  send_sound_num(str, 1);
}

// Sets given goal to given x,y and rotation quat rz,rw
void setGoalPoseRaw(double x, double y, double rz, double rw,
                 move_base_msgs::MoveBaseGoal &goal) {
  goal.target_pose.pose.position.x = x;
  goal.target_pose.pose.position.y = y;
  goal.target_pose.pose.orientation.z = rz;
  goal.target_pose.pose.orientation.w = rw;
}
void setGoalPose(const search_pose &s,
                 move_base_msgs::MoveBaseGoal &goal) {
  setGoalPoseRaw(s.x, s.y, s.rz, s.rw, goal);
}

// Given a trashcan as a PoseStamped messge, returns the goal pose in front of it
void getGoalPoseFromTrashcan(const geometry_msgs::PoseStamped& msg,
                             move_base_msgs::MoveBaseGoal &goal) {
  double theta = 2 * acos(msg.pose.orientation.w);
  setGoalPoseRaw(msg.pose.position.x - 0.5*cos(theta),
                 msg.pose.position.y - 0.5*sin(theta),
                 msg.pose.orientation.z,
                 msg.pose.orientation.w,
                 goal);
  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();
}

bool found_trashcan = false;

// Callback for new april tags
void trashcanTagSearcherCallback(const global_planner::GarbageCan::ConstPtr& msg) {
  // ROS_INFO("April Tag Callback!");

  geometry_msgs::PoseStamped pose = msg->pose;
  int can_num = msg->can_num;

   ROS_INFO("Can number: %d", msg->can_num);

  // If april tag is id 6 (trashcan)
  if (can_num == 6) {
    // ROS_INFO("Header Match!");
    found_trashcan = true;

    move_base_msgs::MoveBaseGoal goal;

    // Set goal position to in front of april tag
    getGoalPoseFromTrashcan(pose, goal);

    // http://mirror.umd.edu/roswiki/doc/diamondback/api/actionlib/html/classactionlib_1_1simple__action__client_1_1SimpleActionClient.html#a6bdebdd9f43a470ecd361d2c8b743188
    // If a previous goal is already active when this is called.
    // We simply forget about that goal and start tracking the new goal.
    // No cancel requests are made.
    ROS_INFO("TRASH GOAL SENT!");
    action_client_ptr->sendGoal(goal);
  }
}

void transition(State state, ros::NodeHandle &n) {
  //Only transition if it's new
  if (currState != state) {
    State lastState = currState; //used when pausing
    currState = state;

    ROS_INFO("-- STATE TRANSITION %d --", currState);

    int chosen_search_pose = -1;
    switch(currState) {
      case SAFE:
      action_client_ptr->cancelAllGoals(); // Cancel any current move goals
      sub.shutdown();
      command_value = 0;
      break;

      case SEARCH_A:
      chosen_search_pose = 0;
      ROS_INFO("SENDING POSE SEARCH_A to /move_base/goal");
      break;

      case SEARCH_B:
      chosen_search_pose = 1;
      ROS_INFO("SENDING POSE SEARCH_B to /move_base/goal");
      break;

      case SEARCH_C:
      chosen_search_pose = 2;
      ROS_INFO("SENDING POSE SEARCH_C to /move_base/goal");
      break;

      case SEARCH_D:
      chosen_search_pose = 3;
      ROS_INFO("SENDING POSE SEARCH_D to /move_base/goal");
      break;

      case SEARCH_E:
      chosen_search_pose = 4;
      ROS_INFO("SENDING POSE SEARCH_E to /move_base/goal");
      break;

      case APPROACH_TRASH:
      send_sound("approaching_trash");
      sub.shutdown();
      break;

      case DUMP_TRASH:
      sub.shutdown();
      {
        // Set goal to search pose
        move_base_msgs::MoveBaseGoal goal;
        setGoalPose(end_pose, goal);
        goal.target_pose.header.frame_id = "map";
        goal.target_pose.header.stamp = ros::Time::now();

        // Send goal to action client
        action_client_ptr->sendGoal(goal);
        ROS_INFO("END GOAL SENT");
      }

      break;

      case END:
      sub.shutdown();
      break;

      case PAUSE:
      ROS_INFO("PAUSE received... cancel all goals");
      state_before_pause = lastState;
      timeofpause = ros::Time::now();
      action_client_ptr->cancelAllGoals(); // Cancel any current move goals
      sub.shutdown();
      break;

      case RESUME:
      ROS_ERROR("Interesting... we are trying to transition to 'resume'. weird");
      break;
    }

    std_msgs::Int32 cmd_msg;
    cmd_msg.data = currState;
    cmd_pub.publish(cmd_msg);
    ROS_INFO_STREAM("Sent command state: "<<cmd_msg.data);

    // If searching
    if (chosen_search_pose >= 0) {
      // Subscribe to trashcan searcher
      sub = n.subscribe("garbageCan", 10, trashcanTagSearcherCallback);

      // Set goal to search pose
      move_base_msgs::MoveBaseGoal goal;
      setGoalPose(search_poses[chosen_search_pose], goal);
      goal.target_pose.header.frame_id = "map";
      goal.target_pose.header.stamp = ros::Time::now();

      // Send goal to action client
      action_client_ptr->sendGoal(goal);
      ROS_INFO("GOAL SENT");
    }
  }
}

void commandCallback(const std_msgs::Int32::ConstPtr& msg) {
  if (command_value != msg->data)
  {
    command_value = msg->data;
    ROS_INFO("Received command %d", command_value);
  }
}

int main(int argc, char** argv){
  // ROS Node Initialization
  ros::init(argc, argv, "global_planner");
  ros::NodeHandle n;

  // Subscribe to command node
  ros::Subscriber cmd_sub = n.subscribe("cmd_state", 10, commandCallback);
  // Publisher to send current state to the rest of the world when transition happens
  cmd_pub = n.advertise<std_msgs::Int32>("cmd_state", 10);

  // Publisher to send current state to the rest of the world when transition happens
  sound_pub = n.advertise<global_planner::SoundMsg>("play_sound", 10);
  ros::spinOnce();

  ROS_INFO_STREAM("Sending test sound");
  sleep(1);
  send_sound("beep.wav");

  action_client_ptr.reset( new MoveBaseClient("move_base", true) );

  // Create Rate Object for sleeping
  ros::Rate r(100);

  // Wait for the action server to come up
  while(ros::ok() && !action_client_ptr->waitForServer(ros::Duration(1.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  ROS_INFO("Starting global planner");

  while(ros::ok()){
    if (command_value == 0) {
      given_start_state = false;
      transition(SAFE, n);
    }
    if (found_trashcan) {
      found_trashcan = false;
      transition(APPROACH_TRASH, n);
    }

    // State machine command override from safe to chosen state
    switch(currState) {
      case START:
        if (command_value > 0 && command_value < 6)
        {
          given_start_state = true;
          for (int i=0; i<5; i++)
          {
            send_sound("beep.wav");
            sleep(1.5);
          }
          State s = static_cast<State>(command_value);
          transition(s, n);
        }
        break;
      case SAFE:
        switch (command_value) {
          State s;
          case SEARCH_A:
          case SEARCH_B:
          case SEARCH_C:
          case SEARCH_D:
          case SEARCH_E:
          s = static_cast<State>(command_value);
          transition(s, n);
          break;
          case APPROACH_TRASH:
          transition(APPROACH_TRASH, n);
          break;
          case DUMP_TRASH:
          transition(DUMP_TRASH, n);
          break;
          case 8:
          transition(END, n);
          break;
          case 99:
          transition(PAUSE, n);
          break;
          case 100:
          transition(RESUME, n);
        }
        break;

      case SEARCH_A:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 2) {
          send_sound("beep.wav");
          transition(SEARCH_B, n);
        }
        break;

      case SEARCH_B:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 3) {

          send_sound("beep.wav");
          transition(SEARCH_C, n);
        }
        break;

      case SEARCH_C:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 4) {
          send_sound("beep.wav");
          transition(SEARCH_D, n);
        }
        break;

      case SEARCH_D:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 5) {
          send_sound("beep.wav");
          transition(SEARCH_E, n);
        }
        break;

      case SEARCH_E:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 6) {
          send_sound("beep.wav");
          transition(DUMP_TRASH, n);
        }
        break;

      case APPROACH_TRASH:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 7) {
          send_sound("seen_goal.wav");
          transition(DUMP_TRASH, n);
        }
        break;

      // Dump trash means go to the end search_pos 6
      case DUMP_TRASH:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        if (command_value == PAUSE)
          transition(PAUSE, n);
        if (action_client_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED ||
            command_value == 8) {
          send_sound("beep.wav");
          transition(END, n);
        }
        break;

      case END:
        ROS_INFO_THROTTLE(2,"command_value = %d, Status : %s", command_value, action_client_ptr->getState().toString().c_str());
        send_sound("seen_goal.wav");
        ros::spinOnce();
        sleep(1);
        exit(1);
        break;

      case PAUSE:
        if (command_value == RESUME)
        {
          transition(state_before_pause, n);
        }
        if (ros::Time::now() - timeofpause > ros::Duration(10))
        {
          ROS_ERROR("We've been paused for too long. Let's resume anyways!");
          transition(state_before_pause,n);
        }
    }
    if (given_start_state && action_client_ptr->getState() == actionlib::SimpleClientGoalState::ABORTED) {
      transition(SAFE, n);
      send_sound("crash.wav");
    }

    // Update callbacks after the fact, for next loop iteration.
    ros::spinOnce();

    r.sleep();
  }
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2013-2017 Daniel Nicoletti <dantti12@gmail.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
#include "dispatcher_p.h"

#include "common.h"
#include "application.h"
#include "engine.h"
#include "context.h"
#include "controller.h"
#include "controller_p.h"
#include "action.h"
#include "request_p.h"
#include "dispatchtypepath.h"
#include "dispatchtypechained.h"
#include "utils.h"

#include <QUrl>
#include <QMetaMethod>

using namespace Cutelyst;

Dispatcher::Dispatcher(QObject *parent) : QObject(parent)
  , d_ptr(new DispatcherPrivate(this))
{
    new DispatchTypePath(parent);
    new DispatchTypeChained(parent);
}

Dispatcher::~Dispatcher()
{
    delete d_ptr;
}

void Dispatcher::setupActions(const QVector<Controller*> &controllers, const QVector<Cutelyst::DispatchType *> &dispatchers, bool printActions)
{
    Q_D(Dispatcher);

    d->dispatchers = dispatchers;

    ActionList registeredActions;
    for (Controller *controller : controllers) {
        bool instanceUsed = false;
        const auto actions = controller->actions();
        for (Action *action : actions) {
            bool registered = false;
            if (!d->actions.contains(action->reverse())) {
                if (!action->attributes().contains(QLatin1String("Private"))) {
                    // Register the action with each dispatcher
                    for (DispatchType *dispatch : dispatchers) {
                        if (dispatch->registerAction(action)) {
                            registered = true;
                        }
                    }
                } else {
                    // We register private actions
                    registered = true;
                }
            }

            // The Begin, Auto, End actions are not
            // registered by Dispatchers but we need them
            // as private actions anyway
            if (registered) {
                d->actions.insert(action->ns() + QLatin1Char('/') + action->name(), action);
                d->actionContainer[action->ns()] << action;
                registeredActions.append(action);
                instanceUsed = true;
            } else {
                qCDebug(CUTELYST_DISPATCHER) << "The action" << action->name() << "of"
                                             << action->controller()->objectName()
                                             << "controller was not registered in any dispatcher."
                                                " If you still want to access it internally (via actionFor())"
                                                " you may make it's method private.";
            }
        }

        if (instanceUsed) {
            d->controllers.insert(controller->objectName(), controller);
        }
    }

    if (printActions) {
        d->printActions();
    }

    // Cache root actions, BEFORE the controllers set them
    d->rootActions = d->actionContainer.value(QLatin1String(""));

    for (Controller *controller : controllers) {
        controller->d_ptr->setupFinished();
    }

    // Unregister any dispatcher that is not in use
    int i = 0;
    while (i < d->dispatchers.size()) {
        DispatchType *type = d->dispatchers.at(i);
        if (!type->inUse()) {
            d->dispatchers.removeAt(i);
            continue;
        }
        ++i;
    }

    if (printActions) {
        // List all public actions
        for (DispatchType *dispatch : dispatchers) {
            qCDebug(CUTELYST_DISPATCHER) << dispatch->list().constData();
        }
    }
}

bool Dispatcher::dispatch(Context *c)
{
    Action *action = c->action();
    if (action) {
        return action->controller()->_DISPATCH(c);
    } else {
        const QString path = c->req()->path();
        if (path.isEmpty()) {
            c->error(c->translate("Cutelyst::Dispatcher", "No default action defined"));
        } else {
            c->error(c->translate("Cutelyst::Dispatcher", "Unknown resource '%1'.").arg(path));
        }
    }
    return false;
}

bool Dispatcher::forward(Context *c, Component *component)
{
    Q_ASSERT(component);
    // If the component was an Action
    // the dispatch() would call c->execute
    return c->execute(component);
}

bool Dispatcher::forward(Context *c, const QString &opname)
{
    Q_D(const Dispatcher);

    Action *action = d->command2Action(c, opname, c->request()->args());
    if (action) {
        return action->dispatch(c);
    }

    qCCritical(CUTELYST_DISPATCHER) << "Action not found" << opname << c->request()->args();
    return false;
}

void Dispatcher::prepareAction(Context *c)
{
    Q_D(Dispatcher);

    Request *request = c->request();
    d->prepareAction(c, request->path());

    static const auto &log = CUTELYST_DISPATCHER();
    if (log.isDebugEnabled()) {
        if (!request->match().isEmpty()) {
            qCDebug(log) << "Path is" << request->match();
        }

        if (!request->args().isEmpty()) {
            qCDebug(log) << "Arguments are" << request->args().join(QLatin1Char('/'));
        }
    }
}

void DispatcherPrivate::prepareAction(Context *c, const QString &requestPath) const
{
    QString path = normalizePath(requestPath);
    QStringList args;

    //  "foo/bar"
    //  "foo/" skip
    //  "foo"
    //  ""
    Q_FOREVER {
        // Check out the dispatch types to see if any
        // will handle the path at this level
        for (DispatchType *type : dispatchers) {
            if (type->match(c, path, args) == DispatchType::ExactMatch) {
                return;
            }
        }

        // leave the loop if we are at the root "/"
        if (path.isEmpty()) {
            break;
        }

        int pos = path.lastIndexOf(QLatin1Char('/'));

        const QString arg = path.mid(pos + 1);
        args.prepend(arg);

        path.resize(pos);
    }
}

Action *Dispatcher::getAction(const QString &name, const QString &nameSpace) const
{
    Q_D(const Dispatcher);

    if (name.isEmpty()) {
        return nullptr;
    }

    if (nameSpace.isEmpty()) {
        return d->actions.value(QLatin1Char('/') + name);
    }

    const QString ns = DispatcherPrivate::cleanNamespace(nameSpace);
    return getActionByPath(ns + QLatin1Char('/') + name);
}

Action *Dispatcher::getActionByPath(const QString &path) const
{
    Q_D(const Dispatcher);

    QString _path = path;
    int slashes = _path.count(QLatin1Char('/'));
    if (slashes == 0) {
        _path.prepend(QLatin1Char('/'));
    } else if (_path.startsWith(QLatin1Char('/')) && slashes != 1) {
        _path.remove(0, 1);
    }
    return d->actions.value(_path);
}

ActionList Dispatcher::getActions(const QString &name, const QString &nameSpace) const
{
    Q_D(const Dispatcher);

    ActionList ret;

    if (name.isEmpty()) {
        return ret;
    }

    const QString ns = DispatcherPrivate::cleanNamespace(nameSpace);
    const ActionList containers = d->getContainers(ns);

    for (Action *action : containers) {
        if (action->name() == name) {
            ret.prepend(action);
        }
    }
    return ret;
}

QMap<QString, Controller *> Dispatcher::controllers() const
{
    Q_D(const Dispatcher);
    return d->controllers;
}

QString Dispatcher::uriForAction(Action *action, const QStringList &captures) const
{
    Q_D(const Dispatcher);
    QString ret;
    for (DispatchType *dispatch : d->dispatchers) {
        ret = dispatch->uriForAction(action, captures);
        if (!ret.isNull()) {
            if (ret.isEmpty()) {
                ret = QStringLiteral("/");
            }
            break;
        }
    }
    return ret;
}

Action *Dispatcher::expandAction(const Context *c, Action *action) const
{
    Q_D(const Dispatcher);
    for (DispatchType *dispatch : d->dispatchers) {
        Action *expandedAction = dispatch->expandAction(c, action);
        if (expandedAction) {
            return expandedAction;
        }
    }
    return action;
}

QVector<DispatchType *> Dispatcher::dispatchers() const
{
    Q_D(const Dispatcher);
    return d->dispatchers;
}

QString DispatcherPrivate::cleanNamespace(const QString &ns)
{
    QString ret = ns;
    bool lastWasSlash = true; // remove initial slash
    int nsSize = ns.size();
    for (int i = 0; i < nsSize; ++i) {
        // Mark if the last char was a slash
        // so that two or more consecutive slashes
        // could be converted to just one
        // "a///b" -> "a/b"
        if (ret.at(i) == QLatin1Char('/')) {
            if (lastWasSlash) {
                ret.remove(i, 1);
                --nsSize;
            } else {
                lastWasSlash = true;
            }
        } else {
            lastWasSlash = false;
        }
    }
    return ret;
}

QString DispatcherPrivate::normalizePath(const QString &path)
{
    QString ret = path;
    bool lastSlash = true;
    int i = 0;
    while (i < ret.size()) {
        if (ret.at(i) == QLatin1Char('/')) {
            if (lastSlash) {
                ret.remove(i, 1);
                continue;
            }
            lastSlash = true;
        } else {
            lastSlash = false;
        }
        ++i;
    }

    if (ret.endsWith(QLatin1Char('/'))) {
        ret.resize(ret.size() - 1);
    }
    return ret;
}

void DispatcherPrivate::printActions() const
{
    QVector<QStringList> table;

    QStringList keys = actions.keys();
    keys.sort(Qt::CaseInsensitive);
    for (const QString &key : keys) {
        Action *action = actions.value(key);
        QString path = key;
        if (!path.startsWith(QLatin1Char('/'))) {
            path.prepend(QLatin1Char('/'));
        }

        QStringList row;
        row.append(path);
        row.append(action->className());
        row.append(action->name());
        table.append(row);
    }

    qCDebug(CUTELYST_DISPATCHER) <<  Utils::buildTable(table, {
                                                           QLatin1String("Private"),
                                                           QLatin1String("Class"),
                                                           QLatin1String("Method")
                                                       },
                                                       QLatin1String("Loaded Private actions:")).constData();
}

ActionList DispatcherPrivate::getContainers(const QString &ns) const
{
    ActionList ret;

    if (ns != QLatin1String("/")) {
        int pos = ns.size();
//        qDebug() << pos << ns.mid(0, pos);
        while (pos > 0) {
//            qDebug() << pos << ns.mid(0, pos);
            ret.append(actionContainer.value(ns.mid(0, pos)));
            pos = ns.lastIndexOf(QLatin1Char('/'), pos - 1);
        }
    }
//    qDebug() << actionContainer.size() << rootActions;
    ret.append(rootActions);

    return ret;
}

Action *DispatcherPrivate::command2Action(Context *c, const QString &command, const QStringList &args) const
{
    auto it = actions.constFind(command);
    if (it != actions.constEnd()) {
        return it.value();
    }

    return invokeAsPath(c, command, args);
}

Action *DispatcherPrivate::invokeAsPath(Context *c, const QString &relativePath, const QStringList &args) const
{
    Q_Q(const Dispatcher);

    Action *ret;
    QString path = DispatcherPrivate::actionRel2Abs(c, relativePath);

    int pos = path.lastIndexOf(QLatin1Char('/'));
    int lastPos = path.size();
    do {
        if (pos == -1) {
            ret = q->getAction(path, QString());
            if (ret) {
                return ret;
            }
        } else {
            const QString name = path.mid(pos + 1, lastPos);
            path = path.mid(0, pos);
            ret = q->getAction(name, path);
            if (ret) {
                return ret;
            }
        }

        lastPos = pos;
        pos = path.indexOf(QLatin1Char('/'), pos - 1);
    } while (pos != -1);

    return nullptr;
}

QString DispatcherPrivate::actionRel2Abs(Context *c, const QString &path)
{
    QString ret;
    if (path.startsWith(QLatin1Char('/'))) {
        ret = path.mid(1);
        return ret;
    }

    const QString ns = qobject_cast<Action *>(c->stack().constLast())->ns();
    if (ns.isEmpty()) {
        ret = path;
    } else {
        ret = ns + QLatin1Char('/') + path;
    }
    return ret;
}

#include "moc_dispatcher.cpp"
<commit_msg>Do not prepend on vectors, it's slow<commit_after>/*
 * Copyright (C) 2013-2017 Daniel Nicoletti <dantti12@gmail.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
#include "dispatcher_p.h"

#include "common.h"
#include "application.h"
#include "engine.h"
#include "context.h"
#include "controller.h"
#include "controller_p.h"
#include "action.h"
#include "request_p.h"
#include "dispatchtypepath.h"
#include "dispatchtypechained.h"
#include "utils.h"

#include <QUrl>
#include <QMetaMethod>

using namespace Cutelyst;

Dispatcher::Dispatcher(QObject *parent) : QObject(parent)
  , d_ptr(new DispatcherPrivate(this))
{
    new DispatchTypePath(parent);
    new DispatchTypeChained(parent);
}

Dispatcher::~Dispatcher()
{
    delete d_ptr;
}

void Dispatcher::setupActions(const QVector<Controller*> &controllers, const QVector<Cutelyst::DispatchType *> &dispatchers, bool printActions)
{
    Q_D(Dispatcher);

    d->dispatchers = dispatchers;

    ActionList registeredActions;
    for (Controller *controller : controllers) {
        bool instanceUsed = false;
        const auto actions = controller->actions();
        for (Action *action : actions) {
            bool registered = false;
            if (!d->actions.contains(action->reverse())) {
                if (!action->attributes().contains(QLatin1String("Private"))) {
                    // Register the action with each dispatcher
                    for (DispatchType *dispatch : dispatchers) {
                        if (dispatch->registerAction(action)) {
                            registered = true;
                        }
                    }
                } else {
                    // We register private actions
                    registered = true;
                }
            }

            // The Begin, Auto, End actions are not
            // registered by Dispatchers but we need them
            // as private actions anyway
            if (registered) {
                d->actions.insert(action->ns() + QLatin1Char('/') + action->name(), action);
                d->actionContainer[action->ns()] << action;
                registeredActions.append(action);
                instanceUsed = true;
            } else {
                qCDebug(CUTELYST_DISPATCHER) << "The action" << action->name() << "of"
                                             << action->controller()->objectName()
                                             << "controller was not registered in any dispatcher."
                                                " If you still want to access it internally (via actionFor())"
                                                " you may make it's method private.";
            }
        }

        if (instanceUsed) {
            d->controllers.insert(controller->objectName(), controller);
        }
    }

    if (printActions) {
        d->printActions();
    }

    // Cache root actions, BEFORE the controllers set them
    d->rootActions = d->actionContainer.value(QLatin1String(""));

    for (Controller *controller : controllers) {
        controller->d_ptr->setupFinished();
    }

    // Unregister any dispatcher that is not in use
    int i = 0;
    while (i < d->dispatchers.size()) {
        DispatchType *type = d->dispatchers.at(i);
        if (!type->inUse()) {
            d->dispatchers.removeAt(i);
            continue;
        }
        ++i;
    }

    if (printActions) {
        // List all public actions
        for (DispatchType *dispatch : dispatchers) {
            qCDebug(CUTELYST_DISPATCHER) << dispatch->list().constData();
        }
    }
}

bool Dispatcher::dispatch(Context *c)
{
    Action *action = c->action();
    if (action) {
        return action->controller()->_DISPATCH(c);
    } else {
        const QString path = c->req()->path();
        if (path.isEmpty()) {
            c->error(c->translate("Cutelyst::Dispatcher", "No default action defined"));
        } else {
            c->error(c->translate("Cutelyst::Dispatcher", "Unknown resource '%1'.").arg(path));
        }
    }
    return false;
}

bool Dispatcher::forward(Context *c, Component *component)
{
    Q_ASSERT(component);
    // If the component was an Action
    // the dispatch() would call c->execute
    return c->execute(component);
}

bool Dispatcher::forward(Context *c, const QString &opname)
{
    Q_D(const Dispatcher);

    Action *action = d->command2Action(c, opname, c->request()->args());
    if (action) {
        return action->dispatch(c);
    }

    qCCritical(CUTELYST_DISPATCHER) << "Action not found" << opname << c->request()->args();
    return false;
}

void Dispatcher::prepareAction(Context *c)
{
    Q_D(Dispatcher);

    Request *request = c->request();
    d->prepareAction(c, request->path());

    static const auto &log = CUTELYST_DISPATCHER();
    if (log.isDebugEnabled()) {
        if (!request->match().isEmpty()) {
            qCDebug(log) << "Path is" << request->match();
        }

        if (!request->args().isEmpty()) {
            qCDebug(log) << "Arguments are" << request->args().join(QLatin1Char('/'));
        }
    }
}

void DispatcherPrivate::prepareAction(Context *c, const QString &requestPath) const
{
    QString path = normalizePath(requestPath);
    QStringList args;

    //  "foo/bar"
    //  "foo/" skip
    //  "foo"
    //  ""
    Q_FOREVER {
        // Check out the dispatch types to see if any
        // will handle the path at this level
        for (DispatchType *type : dispatchers) {
            if (type->match(c, path, args) == DispatchType::ExactMatch) {
                return;
            }
        }

        // leave the loop if we are at the root "/"
        if (path.isEmpty()) {
            break;
        }

        int pos = path.lastIndexOf(QLatin1Char('/'));

        const QString arg = path.mid(pos + 1);
        args.prepend(arg);

        path.resize(pos);
    }
}

Action *Dispatcher::getAction(const QString &name, const QString &nameSpace) const
{
    Q_D(const Dispatcher);

    if (name.isEmpty()) {
        return nullptr;
    }

    if (nameSpace.isEmpty()) {
        return d->actions.value(QLatin1Char('/') + name);
    }

    const QString ns = DispatcherPrivate::cleanNamespace(nameSpace);
    return getActionByPath(ns + QLatin1Char('/') + name);
}

Action *Dispatcher::getActionByPath(const QString &path) const
{
    Q_D(const Dispatcher);

    QString _path = path;
    int slashes = _path.count(QLatin1Char('/'));
    if (slashes == 0) {
        _path.prepend(QLatin1Char('/'));
    } else if (_path.startsWith(QLatin1Char('/')) && slashes != 1) {
        _path.remove(0, 1);
    }
    return d->actions.value(_path);
}

ActionList Dispatcher::getActions(const QString &name, const QString &nameSpace) const
{
    Q_D(const Dispatcher);

    ActionList ret;

    if (name.isEmpty()) {
        return ret;
    }

    const QString ns = DispatcherPrivate::cleanNamespace(nameSpace);
    const ActionList containers = d->getContainers(ns);
    auto rIt = containers.rbegin();
    while (rIt != containers.rend()) {
        if ((*rIt)->name() == name) {
            ret.append(*rIt);
        }
        ++rIt;
    }
    return ret;
}

QMap<QString, Controller *> Dispatcher::controllers() const
{
    Q_D(const Dispatcher);
    return d->controllers;
}

QString Dispatcher::uriForAction(Action *action, const QStringList &captures) const
{
    Q_D(const Dispatcher);
    QString ret;
    for (DispatchType *dispatch : d->dispatchers) {
        ret = dispatch->uriForAction(action, captures);
        if (!ret.isNull()) {
            if (ret.isEmpty()) {
                ret = QStringLiteral("/");
            }
            break;
        }
    }
    return ret;
}

Action *Dispatcher::expandAction(const Context *c, Action *action) const
{
    Q_D(const Dispatcher);
    for (DispatchType *dispatch : d->dispatchers) {
        Action *expandedAction = dispatch->expandAction(c, action);
        if (expandedAction) {
            return expandedAction;
        }
    }
    return action;
}

QVector<DispatchType *> Dispatcher::dispatchers() const
{
    Q_D(const Dispatcher);
    return d->dispatchers;
}

QString DispatcherPrivate::cleanNamespace(const QString &ns)
{
    QString ret = ns;
    bool lastWasSlash = true; // remove initial slash
    int nsSize = ns.size();
    for (int i = 0; i < nsSize; ++i) {
        // Mark if the last char was a slash
        // so that two or more consecutive slashes
        // could be converted to just one
        // "a///b" -> "a/b"
        if (ret.at(i) == QLatin1Char('/')) {
            if (lastWasSlash) {
                ret.remove(i, 1);
                --nsSize;
            } else {
                lastWasSlash = true;
            }
        } else {
            lastWasSlash = false;
        }
    }
    return ret;
}

QString DispatcherPrivate::normalizePath(const QString &path)
{
    QString ret = path;
    bool lastSlash = true;
    int i = 0;
    while (i < ret.size()) {
        if (ret.at(i) == QLatin1Char('/')) {
            if (lastSlash) {
                ret.remove(i, 1);
                continue;
            }
            lastSlash = true;
        } else {
            lastSlash = false;
        }
        ++i;
    }

    if (ret.endsWith(QLatin1Char('/'))) {
        ret.resize(ret.size() - 1);
    }
    return ret;
}

void DispatcherPrivate::printActions() const
{
    QVector<QStringList> table;

    QStringList keys = actions.keys();
    keys.sort(Qt::CaseInsensitive);
    for (const QString &key : keys) {
        Action *action = actions.value(key);
        QString path = key;
        if (!path.startsWith(QLatin1Char('/'))) {
            path.prepend(QLatin1Char('/'));
        }

        QStringList row;
        row.append(path);
        row.append(action->className());
        row.append(action->name());
        table.append(row);
    }

    qCDebug(CUTELYST_DISPATCHER) <<  Utils::buildTable(table, {
                                                           QLatin1String("Private"),
                                                           QLatin1String("Class"),
                                                           QLatin1String("Method")
                                                       },
                                                       QLatin1String("Loaded Private actions:")).constData();
}

ActionList DispatcherPrivate::getContainers(const QString &ns) const
{
    ActionList ret;

    if (ns != QLatin1String("/")) {
        int pos = ns.size();
//        qDebug() << pos << ns.mid(0, pos);
        while (pos > 0) {
//            qDebug() << pos << ns.mid(0, pos);
            ret.append(actionContainer.value(ns.mid(0, pos)));
            pos = ns.lastIndexOf(QLatin1Char('/'), pos - 1);
        }
    }
//    qDebug() << actionContainer.size() << rootActions;
    ret.append(rootActions);

    return ret;
}

Action *DispatcherPrivate::command2Action(Context *c, const QString &command, const QStringList &args) const
{
    auto it = actions.constFind(command);
    if (it != actions.constEnd()) {
        return it.value();
    }

    return invokeAsPath(c, command, args);
}

Action *DispatcherPrivate::invokeAsPath(Context *c, const QString &relativePath, const QStringList &args) const
{
    Q_Q(const Dispatcher);

    Action *ret;
    QString path = DispatcherPrivate::actionRel2Abs(c, relativePath);

    int pos = path.lastIndexOf(QLatin1Char('/'));
    int lastPos = path.size();
    do {
        if (pos == -1) {
            ret = q->getAction(path, QString());
            if (ret) {
                return ret;
            }
        } else {
            const QString name = path.mid(pos + 1, lastPos);
            path = path.mid(0, pos);
            ret = q->getAction(name, path);
            if (ret) {
                return ret;
            }
        }

        lastPos = pos;
        pos = path.indexOf(QLatin1Char('/'), pos - 1);
    } while (pos != -1);

    return nullptr;
}

QString DispatcherPrivate::actionRel2Abs(Context *c, const QString &path)
{
    QString ret;
    if (path.startsWith(QLatin1Char('/'))) {
        ret = path.mid(1);
        return ret;
    }

    const QString ns = qobject_cast<Action *>(c->stack().constLast())->ns();
    if (ns.isEmpty()) {
        ret = path;
    } else {
        ret = ns + QLatin1Char('/') + path;
    }
    return ret;
}

#include "moc_dispatcher.cpp"
<|endoftext|>