text:
//
//  Copyright (C) 2018 Susan H. Leung
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#include "MolStandardize.h"
#include "Metal.h"
#include "Normalize.h"
#include "Tautomer.h"
#include "Fragment.h"
#include <GraphMol/RDKitBase.h>
#include <iostream>
#include <GraphMol/ROMol.h>
#include <GraphMol/MolOps.h>
#include <GraphMol/MolStandardize/TransformCatalog/TransformCatalogParams.h>
#include "Charge.h"
#include <GraphMol/SmilesParse/SmilesWrite.h>
#include <GraphMol/SmilesParse/SmilesParse.h>

#include <RDGeneral/BoostStartInclude.h>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <RDGeneral/BoostEndInclude.h>

using namespace std;
namespace RDKit {
namespace MolStandardize {
const CleanupParameters defaultCleanupParameters;

#define PT_OPT_GET(opt) params.opt = pt.get(#opt, params.opt)
void updateCleanupParamsFromJSON(CleanupParameters &params,
                                 const std::string &json) {
  if (json.empty()) {
    return;
  }
  std::istringstream ss;
  ss.str(json);
  boost::property_tree::ptree pt;
  boost::property_tree::read_json(ss, pt);
  PT_OPT_GET(rdbase);
  PT_OPT_GET(normalizations);
  PT_OPT_GET(acidbaseFile);
  PT_OPT_GET(fragmentFile);
  PT_OPT_GET(tautomerTransforms);
  PT_OPT_GET(maxRestarts);
  PT_OPT_GET(preferOrganic);
  PT_OPT_GET(doCanonical);
  PT_OPT_GET(maxTautomers);
  PT_OPT_GET(maxTransforms);
  PT_OPT_GET(tautomerRemoveSp3Stereo);
  PT_OPT_GET(tautomerRemoveBondStereo);
  PT_OPT_GET(tautomerRemoveIsotopicHs);
  PT_OPT_GET(tautomerReassignStereo);
  {
    const auto norm_tfs = pt.get_child_optional("normalizationData");
    if (norm_tfs) {
      for (const auto &entry : *norm_tfs) {
        std::string nm = entry.second.get<std::string>("name", "");
        std::string smarts = entry.second.get<std::string>("smarts", "");
        if (nm.empty() || smarts.empty()) {
          BOOST_LOG(rdWarningLog)
              << " empty transformation name or SMARTS" << std::endl;
          continue;
        }
        params.normalizationData.push_back(std::make_pair(nm, smarts));
      }
    }
  }
  {
    const auto frag_tfs = pt.get_child_optional("fragmentData");
    if (frag_tfs) {
      for (const auto &entry : *frag_tfs) {
        std::string nm = entry.second.get<std::string>("name", "");
        std::string smarts = entry.second.get<std::string>("smarts", "");
        if (nm.empty() || smarts.empty()) {
          BOOST_LOG(rdWarningLog)
              << " empty transformation name or SMARTS" << std::endl;
          continue;
        }
        params.fragmentData.push_back(std::make_pair(nm, smarts));
      }
    }
  }
  {
    const auto ab_data = pt.get_child_optional("acidbaseData");
    if (ab_data) {
      for (const auto &entry : *ab_data) {
        std::string nm = entry.second.get<std::string>("name", "");
        std::string acid = entry.second.get<std::string>("acid", "");
        std::string base = entry.second.get<std::string>("base", "");
        if (nm.empty() || acid.empty() || base.empty()) {
          BOOST_LOG(rdWarningLog)
              << " empty component in acidbaseData" << std::endl;
          continue;
        }
        params.acidbaseData.push_back(std::make_tuple(nm, acid, base));
      }
    }
  }
  {
    const auto taut_data = pt.get_child_optional("tautomerTransformData");
    if (taut_data) {
      for (const auto &entry : *taut_data) {
        std::string nm = entry.second.get<std::string>("name", "");
        std::string smarts = entry.second.get<std::string>("smarts", "");
        std::string bonds = entry.second.get<std::string>("bonds", "");
        std::string charges = entry.second.get<std::string>("charges", "");
        if (nm.empty() || smarts.empty()) {
          BOOST_LOG(rdWarningLog)
              << " empty component in tautomerTransformData" << std::endl;
          continue;
        }
        params.tautomerTransformData.push_back(
            std::make_tuple(nm, smarts, bonds, charges));
      }
    }
  }
}

RWMol *cleanup(const RWMol *mol, const CleanupParameters &params) {
  RWMol m(*mol);
  MolOps::removeHs(m);

  MolStandardize::MetalDisconnector md;
  md.disconnect(m);
  RWMOL_SPTR normalized(MolStandardize::normalize(&m, params));
  RWMol *reionized = MolStandardize::reionize(normalized.get(), params);
  bool cleanIt = true;
  bool force = true;
  MolOps::assignStereochemistry(*reionized, cleanIt, force);

  // update properties of reionized using m.
  reionized->updateProps(m);

  return reionized;
}

RWMol *tautomerParent(const RWMol &mol, const CleanupParameters &params,
                      bool skip_standardize) {
  const RWMol *cleaned = nullptr;
  std::unique_ptr<RWMol> cleanedHolder;

  if (!skip_standardize) {
    cleanedHolder.reset(cleanup(mol, params));
    cleaned = cleanedHolder.get();
  } else {
    cleaned = &mol;
  }

  std::unique_ptr<RWMol> ct{canonicalTautomer(cleaned, params)};

  return cleanup(ct.get(), params);
}

// Return the fragment parent of a given molecule.
// The fragment parent is the largest organic covalent unit in the molecule.
//
RWMol *fragmentParent(const RWMol &mol, const CleanupParameters &params,
                      bool skip_standardize) {
  const RWMol *cleaned = nullptr;
  std::unique_ptr<RWMol> cleanedHolder;

  if (!skip_standardize) {
    cleanedHolder.reset(cleanup(mol, params));
    cleaned = cleanedHolder.get();
  } else {
    cleaned = &mol;
  }

  LargestFragmentChooser lfragchooser(params.preferOrganic);
  return static_cast<RWMol *>(lfragchooser.choose(*cleaned));
}

RWMol *stereoParent(const RWMol &mol, const CleanupParameters &params,
                    bool skip_standardize) {
  RWMol *res;
  if (!skip_standardize) {
    res = cleanup(mol, params);
  } else {
    res = new RWMol(mol);
  }

  MolOps::removeStereochemistry(*res);
  return res;
}

RWMol *isotopeParent(const RWMol &mol, const CleanupParameters &params,
                     bool skip_standardize) {
  RWMol *res;
  if (!skip_standardize) {
    res = cleanup(mol, params);
  } else {
    res = new RWMol(mol);
  }

  for (auto atom : res->atoms()) {
    atom->setIsotope(0);
  }
  return res;
}

RWMol *chargeParent(const RWMol &mol, const CleanupParameters &params,
                    bool skip_standardize) {
  // Return the charge parent of a given molecule.
  // The charge parent is the uncharged version of the fragment parent.

  RWMOL_SPTR fragparent(fragmentParent(mol, params, skip_standardize));

  // if fragment...
  ROMol nm(*fragparent);

  Uncharger uncharger(params.doCanonical);
  ROMOL_SPTR uncharged(uncharger.uncharge(nm));
  RWMol *omol = cleanup(static_cast<RWMol *>(uncharged.get()), params);
  return omol;
}

RWMol *superParent(const RWMol &mol, const CleanupParameters &params,
                   bool skip_standardize) {
  std::unique_ptr<RWMol> res;
  if (!skip_standardize) {
    res.reset(cleanup(mol, params));
  } else {
    res.reset(new RWMol(mol));
  }
  // we can skip fragmentParent since the chargeParent takes care of that
  res.reset(chargeParent(*res, params, true));
  res.reset(isotopeParent(*res, params, true));
  res.reset(stereoParent(*res, params, true));
  res.reset(tautomerParent(*res, params, true));
  return cleanup(*res, params);
}

RWMol *normalize(const RWMol *mol, const CleanupParameters &params) {
  PRECONDITION(mol, "bad molecule");
  std::unique_ptr<Normalizer> normalizer{normalizerFromParams(params)};
  return static_cast<RWMol *>(normalizer->normalize(*mol));
}

RWMol *reionize(const RWMol *mol, const CleanupParameters &params) {
  PRECONDITION(mol, "bad molecule");
  std::unique_ptr<Reionizer> reionizer{reionizerFromParams(params)};
  return static_cast<RWMol *>(reionizer->reionize(*mol));
}

RWMol *removeFragments(const RWMol *mol, const CleanupParameters &params) {
  PRECONDITION(mol, "bad molecule");
  std::unique_ptr<FragmentRemover> remover{fragmentRemoverFromParams(params)};
  return static_cast<RWMol *>(remover->remove(*mol));
}

RWMol *canonicalTautomer(const RWMol *mol, const CleanupParameters &params) {
  PRECONDITION(mol, "bad molecule");
  std::unique_ptr<TautomerEnumerator> te{tautomerEnumeratorFromParams(params)};
  return static_cast<RWMol *>(te->canonicalize(*mol));
}

std::string standardizeSmiles(const std::string &smiles) {
  RWMOL_SPTR mol(SmilesToMol(smiles, 0, false));
  if (!mol) {
    std::string message =
        "SMILES Parse Error: syntax error for input: " + smiles;
    throw ValueErrorException(message);
  }

  CleanupParameters params;
  RWMOL_SPTR cleaned(cleanup(*mol, params));
  return MolToSmiles(*cleaned);
}

std::vector<std::string> enumerateTautomerSmiles(
    const std::string &smiles, const CleanupParameters &params) {
  std::unique_ptr<RWMol> mol(SmilesToMol(smiles, 0, false));
  mol.reset(cleanup(mol.get(), params));
  MolOps::sanitizeMol(*mol);

  TautomerEnumerator te(params);

  auto res = te.enumerate(*mol);

  return res.smiles();
}

}  // end of namespace MolStandardize
}  // namespace RDKit

max_stars_repo_name: i-tub/rdkit

max_issues_repo_name: i-tub/rdkit

max_forks_repo_name: i-tub/rdkit


text:
//
// Created by kerin on 13/11/2018.
//
// Holds variational_parameters + other diagnostic values that we might want to track
// Also other auxilliary functions for writing interim output to file
// One instance per 'run'

#ifndef VBAYES_TRACKER_HPP
#define VBAYES_TRACKER_HPP

#include "parameters.hpp"
#include "genotype_matrix.hpp"
#include "hyps.hpp"
#include "file_utils.hpp"
#include "variational_parameters.hpp"
#include "tools/eigen3.3/Dense"

#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/device/file_descriptor.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/filesystem.hpp>

#include <chrono>
#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <string>
#include <limits>
#include <vector>

namespace boost_io = boost::iostreams;

class VbTracker {
public:
	long count;
	VariationalParametersLite vp;
	double logw;
	Hyps hyps;

	parameters p;

// For writing interim output
	boost::filesystem::path dir, subdir;
	boost_io::filtering_ostream outf_elbo, outf_alpha_diff, outf_inits, outf_iter, outf_alpha;
	boost_io::filtering_ostream outf_weights;
	std::string main_out_file;

	std::chrono::system_clock::time_point time_check;
	long count_to_convergence;

	VbTracker(const parameters& my_params) : p(my_params), vp(my_params), hyps(my_params) {
		main_out_file = p.out_file;
		count_to_convergence = 0;
	}

	VbTracker(const VbTracker& tracker) : p(tracker.p), vp(tracker.p), hyps(tracker.p) {
		main_out_file = p.out_file;
		count_to_convergence = 0;
	}

	~VbTracker(){
		boost_io::close(outf_elbo);
		boost_io::close(outf_alpha_diff);
		boost_io::close(outf_inits);
		boost_io::close(outf_iter);
		boost_io::close(outf_alpha);
		boost_io::close(outf_weights);
	};

	void init_interim_output(const long& ii,
	                         const int& round_index,
	                         const int& n_effects,
	                         const long& n_covar,
	                         const long& n_env,
	                         std::vector< std::string > env_names,
	                         const VariationalParameters& vp){
		time_check = std::chrono::system_clock::now();

		// Create directories
		std::string ss = "lemma_interim_files";
		boost::filesystem::path interim_ext(ss), path(main_out_file);
		dir = path.parent_path() / interim_ext;
		subdir = interim_ext;
		boost::filesystem::create_directories(dir);

		// Initialise fstreams
		fileUtils::fstream_init(outf_iter, p.out_file, subdir.string() + "/", "_iter_updates", false);

		// Diagnostics - add header
		outf_iter << "count sigma";
		for (int ee = 0; ee < n_effects; ee++) {
			outf_iter << " pve" << ee;
			if((ee == 0 && p.mode_mog_prior_beta) || (ee == 1 && p.mode_mog_prior_gam)) {
				outf_iter << " pve_large" << ee;
			}
			outf_iter << " sigma" << ee;
			if((ee == 0 && p.mode_mog_prior_beta) || (ee == 1 && p.mode_mog_prior_gam)) {
				outf_iter << " sigma_spike" << ee;
			}
			outf_iter << " lambda" << ee;
		}
		outf_iter << " s_x";
		if(n_env > 0) outf_iter << " s_z";
		outf_iter << " var_covar elbo";
		if (n_covar > 0) outf_iter << " max_covar_diff";
		outf_iter << " max_beta_diff";
		if (n_env > 0) outf_iter << " max_gam_diff";
		if (n_env > 1) outf_iter << " max_w_diff";
		outf_iter << " secs" << std::endl;

		// Weights - add header + initial values
		if(n_env > 0) {
			fileUtils::fstream_init(outf_weights, p.out_file, subdir.string() + "/", "_env_weights", false);
			outf_weights << "count ";
			for (int ll = 0; ll < n_env; ll++) {
				outf_weights << env_names[ll];
				if (ll + 1 < n_env) outf_weights << " ";
			}
			outf_weights << std::endl;
			outf_weights << "-1 ";
			for (int ll = 0; ll < n_env; ll++) {
				outf_weights << vp.muw(ll);
				if (ll + 1 < n_env) outf_weights << " ";
			}
			outf_weights << std::endl;
		}
	}

	void push_interim_hyps(const long& cnt,
	                       const Hyps& i_hyps,
	                       const double& c_logw,
	                       const double& covar_diff,
	                       const double& beta_diff,
	                       const double& gam_diff,
	                       const double& w_diff,
	                       const int& n_effects,
	                       const long& n_var,
	                       const long& n_covar,
	                       const long& n_env,
	                       const VariationalParameters& vp) {
		// Diagnostics + env-weights from latest vb iteration
		std::chrono::duration<double> lapsecs = std::chrono::system_clock::now() - time_check;

		outf_iter << cnt << " ";
		outf_iter << std::setprecision(6) << std::fixed;
		outf_iter << i_hyps.sigma << " ";

		for (int ee = 0; ee < n_effects; ee++) {
			// PVE
			outf_iter << std::setprecision(6) << std::fixed;
			outf_iter << i_hyps.pve(ee) << " ";
			if ((ee == 0 && p.mode_mog_prior_beta) || (ee == 1 && p.mode_mog_prior_gam)) {
				outf_iter << i_hyps.pve_large(ee) << " ";
			}

			// Relative variance
			outf_iter << std::setprecision(12) << std::fixed;
			outf_iter << i_hyps.slab_relative_var(ee) << " ";
			if ((ee == 0 && p.mode_mog_prior_beta) || (ee == 1 && p.mode_mog_prior_gam)) {
				outf_iter << i_hyps.spike_relative_var(ee) << " ";
			}

			// Lambda
			outf_iter << i_hyps.lambda(ee) << " ";
		}

		outf_iter << std::setprecision(6) << std::scientific;
		for (int ee = 0; ee < n_effects; ee++) {
			outf_iter << i_hyps.s_x(ee) << " ";
		}
		outf_iter << vp.muc.square().sum() << " ";
		outf_iter << std::setprecision(6) << std::fixed;
		outf_iter << c_logw << " ";
		outf_iter << std::setprecision(6) << std::scientific;
		if (n_covar > 0) outf_iter << covar_diff << " ";
		outf_iter << beta_diff << " ";
		if (n_env > 0) outf_iter << gam_diff << " ";
		if (n_env > 1) outf_iter << w_diff << " ";
		outf_iter << std::setprecision(6) << std::fixed;
		outf_iter << lapsecs.count() << std::endl;

		if(n_effects > 1) {
			outf_weights << cnt << " ";
			for (int ll = 0; ll < n_env; ll++) {
				outf_weights << vp.muw(ll);
				if (ll < n_env - 1) outf_weights << " ";
			}
			outf_weights << std::endl;
		}
	}

	void dump_state(const std::string& count,
	                const long& n_samples,
	                const long& n_covar,
	                const long& n_var,
	                const long& n_env,
	                const int& n_effects,
	                const VariationalParameters& vp,
	                const Hyps& hyps,
	                const EigenDataVector& Y,
	                const EigenDataArrayXX& C,
	                const GenotypeMatrix& X,
	                const std::vector< std::string >& covar_names,
	                const std::vector< std::string >& env_names,
	                const std::unordered_map<long, bool>& sample_is_invalid,
	                const std::map<long, int>& sample_location){
		std::string path;

		// Aggregate effects
		Eigen::VectorXd Ealpha = Eigen::VectorXd::Zero(n_samples);
		if(n_covar > 0) {
			Ealpha += (C.matrix() * vp.muc.matrix().cast<scalarData>()).cast<double>();
		}

		std::string header = "Y";
		long n_cols = 1;
		if (n_covar > 0) {
			header += " Ealpha"; n_cols += 1;
		}
		header += " Xbeta"; n_cols += 1;
		if (n_env > 0) {
			header += " eta Xgamma"; n_cols += 2;
		}

		Eigen::MatrixXd tmp(n_samples, n_cols);
		int cc = 0;
		tmp.col(cc) = Y; cc++;
		if (n_covar > 0) {
			tmp.col(cc) = Ealpha; cc++;
		}
		tmp.col(cc) = vp.ym; cc++;
		if (n_env > 0) {
			tmp.col(cc) = vp.eta; cc++;
			tmp.col(cc) = vp.yx; cc++;
		}
		assert(cc == n_cols);

		path = fileUtils::filepath_format(p.out_file, subdir.string() + "/", "_dump_it" + count + "_aggregate");
		fileUtils::dump_predicted_vec_to_file(tmp, path, header, sample_location);

		int world_rank;
		MPI_Comm_rank(MPI_COMM_WORLD, &world_rank);
		if(world_rank == 0) {
			path = fileUtils::filepath_format(p.out_file, subdir.string() + "/", "_dump_it" + count + "_latent_snps");
			vp.snps_to_file(path, X, n_env);

			if(n_covar > 0) {
				path = fileUtils::filepath_format(p.out_file, subdir.string() + "/", "_dump_it" + count + "_covars");
				vp.covar_to_file(path, covar_names);
			}

			if(n_env > 0) {
				path = fileUtils::filepath_format(p.out_file, subdir.string() + "/", "_dump_it" + count + "_env");
				vp.env_to_file(path, env_names);
			}

			path = fileUtils::filepath_format(p.out_file, subdir.string() + "/", "_dump_it" + count + "_hyps");
			hyps.to_file(path);
		}
	}

};

#endif

ext: hpp

max_stars_repo_name: mkerin/LEMMA

max_issues_repo_name: lfelipe-ferrao/LEMMA

max_forks_repo_name: lfelipe-ferrao/LEMMA


text:
/*
BSD 2-Clause License

Copyright (c) 2019, Simranjeet Singh
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <edrone_client/edrone_services.h>
#include <geometry_msgs/PoseArray.h>
#include <sys/time.h>
#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <pthread.h>
#include <unistd.h>
#include <eyantra_drone/Common.h>
#include <eyantra_drone/Protocol.h>
#include <edrone_client/edrone_msgs.h>

#define PORT 23

using namespace std;

bool isSocketCreate=false;

Communication com;
Protocol pro;

ros::ServiceClient serviceClient;
edrone_client::edrone_services service;

int userRC[8]={1500,1500,1500,1500,1000,1000,1000,1000};

void *createSocket(void *threadid){
  isSocketCreate=com.connectSock();
  pthread_exit(NULL);
};

void *writeFunction(void *threadid){
  std::vector<int> requests;
  requests.push_back(MSP_RC);
  requests.push_back(MSP_ATTITUDE);
  requests.push_back(MSP_RAW_IMU);
  requests.push_back(MSP_ALTITUDE);
  requests.push_back(MSP_ANALOG);

  while(1)
  {
    pro.sendRequestMSP_SET_RAW_RC(userRC);
    pro.sendRequestMSP_GET_DEBUG(requests);
    usleep(22000);
  }
  pthread_exit(NULL);
}

void *readFunction(void *threadid){
  do
  {
    com.readFrame();
    //usleep(5);
  }
  while(1);
  pthread_exit(NULL);
}

void *serviceFunction(void *threadid){
  while (1)
  {
      if (serviceClient.call(service))
      {

       service.request.accX=accX;
       service.request.accY=accY;
       service.request.accZ=accZ;
       service.request.gyroX=gyroX;
       service.request.gyroY=gyroY;
       service.request.gyroZ=gyroZ;
       service.request.magX=magX;
       service.request.magY=magY;
       service.request.magZ=magZ;
       service.request.roll=roll;
       service.request.pitch=pitch;
       service.request.yaw=yaw;
       service.request.alt=alt;
       service.request.battery=battery;
       service.request.rssi=rssi;
      }
  }
  pthread_exit(NULL);
}

void Callback(const edrone_client::edrone_msgs::ConstPtr& msg){
   userRC[0] = msg->rcRoll;
   userRC[1] = msg->rcPitch;
   userRC[2] = msg->rcThrottle;
   userRC[3] = msg->rcYaw;
   userRC[4] = msg->rcAUX1;
   userRC[5] = msg->rcAUX2;
   userRC[6] = msg->rcAUX3;
   userRC[7] = msg->rcAUX4;
 // cout << "roll = " << userRC[0] << "pitch = " <<userRC[1] << "Throttle = "<< userRC[2]  << endl;
}

int main(int argc, char **argv){
    pthread_t thread, readThread, writeThread, serviceThread;
    int rc;
    ros::init(argc, argv, "DroneNode");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("drone_command", 1000, Callback);
    rc = pthread_create(&thread, NULL, createSocket, 	(void *)1);
    if (rc)
    {
     cout << "Error:unable to create communication thread," << rc << endl;
     exit(-1);
    }
    pthread_join( thread, NULL);

    if(isSocketCreate)
    {
      //cout << "main() : creating write thread, " << i << endl;
      rc = pthread_create(&writeThread, NULL, writeFunction, 	(void *)2);
      if (rc)
      {
        cout << "Error:unable to create write thread," << rc << endl;
        exit(-1);
      }
      // cout << "main() : creating read thread, " << i << endl;
      rc = pthread_create(&readThread, NULL, readFunction, 	(void *)3);
      if (rc)
      {
        cout << "Error:unable to read create thread," << rc << endl;
        exit(-1);
      }

      serviceClient = n.serviceClient<edrone_client::edrone_services>("eDroneService",true);
      //cout << "main() : creating service thread, " << i << endl;
      rc = pthread_create(&serviceThread, NULL, serviceFunction, 	(void *)4);

      if (rc)
      {
        cout << "Error:unable to service create thread," << rc << endl;
        exit(-1);
      }
    }

    ros::spin();
    return 0;
}

ext: cpp


text:
#include <boost/assign/list_of.hpp>
#include <map>
#include "Command.hpp"

namespace gtp {

bool is_command(const std::string &str) {
	return string_to_command(str) != CMD_UNKNOWN;
}

Command string_to_command(const std::string &str) {
	std::map<std::string, Command> map = boost::assign::map_list_of
		("protocol_version",	CMD_PROTOCOL_VERSION)
		("name", 				CMD_NAME)
		("version",				CMD_VERSION)
		("known_command",		CMD_KNOWN_COMMAND)
		("list_commands",		CMD_LIST_COMMANDS)
		("quit",				CMD_QUIT)
		("boardsize",			CMD_BOARDSIZE)
		("clear_board",			CMD_CLEAR_BOARD)
		("komi",				CMD_KOMI)
		("play",				CMD_PLAY)
		("genmove",				CMD_GENMOVE);

	try {
		return map.at(str);
	} catch (std::out_of_range e) {
		return CMD_UNKNOWN;
	}
}

std::vector<std::string> all_commands() {
	std::vector<std::string> arr = {
		"unknown",
		"protocol_version",
		"name",
		"version",
		"known_command",
		"list_commands",
		"quit",
		"boardsize",
		"clear_board",
		"komi",
		"play",
		"genmove"
	};

	return arr;
}

std::string command_to_string(const Command &c) {
	return all_commands()[c];
}

}

max_stars_repo_name: imalerich/imgo

max_issues_repo_name: imalerich/imgo

max_forks_repo_name: imalerich/imgo


text:
#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <stdexcept>
#include <cmath>

TEST(MathFunctions, expm1) {
  using stan::math::expm1;
  using std::exp;
  EXPECT_FLOAT_EQ(exp(-14.2) - 1, expm1(-14.2));
  EXPECT_FLOAT_EQ(exp(0) - 1, expm1(0));
  EXPECT_FLOAT_EQ(exp(172.987) - 1, expm1(172.987));
  EXPECT_FLOAT_EQ(-1, expm1(-std::numeric_limits<double>::infinity()));
}

TEST(MathFunctions, expm1_inf_return) {
  EXPECT_EQ(std::numeric_limits<double>::infinity(),
            stan::math::expm1(std::numeric_limits<double>::infinity()));
}

TEST(MathFunctions, expm1_nan) {
  using stan::math::expm1;
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::expm1(std::numeric_limits<double>::quiet_NaN()));
}

max_stars_repo_name: jrmie/math

max_issues_repo_name: jrmie/math

max_forks_repo_name: jrmie/math


text:
/*
 * Copyright (C) 2020 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

#include <functional>
#include <mutex>
#include <carma_wm_ctrl/WMBroadcaster.h>
#include <carma_wm/Geometry.h>
#include <carma_wm/MapConformer.h>
#include <lanelet2_extension/utility/message_conversion.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_core/primitives/BoundingBox.h>
#include <type_traits>
#include <cav_msgs/CheckActiveGeofence.h>
#include <lanelet2_core/primitives/Polygon.h>
#include <proj.h>
#include <lanelet2_io/Projection.h>
#include <lanelet2_core/utility/Units.h>
#include <lanelet2_core/Forward.h>
#include <lanelet2_extension/utility/utilities.h>
#include <algorithm>
#include <limits>
#include <carma_wm/Geometry.h>
#include <math.h>
#include <boost/date_time/date_defs.hpp>

namespace carma_wm_ctrl
{
using std::placeholders::_1;

WMBroadcaster::WMBroadcaster(const PublishMapCallback& map_pub, const PublishMapUpdateCallback& map_update_pub, const PublishCtrlRequestCallback& control_msg_pub,
const PublishActiveGeofCallback& active_pub, std::unique_ptr<carma_utils::timers::TimerFactory> timer_factory)
  : map_pub_(map_pub), map_update_pub_(map_update_pub), control_msg_pub_(control_msg_pub), active_pub_(active_pub), scheduler_(std::move(timer_factory))
{
  scheduler_.onGeofenceActive(std::bind(&WMBroadcaster::addGeofence, this, _1));
  scheduler_.onGeofenceInactive(std::bind(&WMBroadcaster::removeGeofence, this, _1));
  std::bind(&WMBroadcaster::routeCallbackMessage, this, _1);
};

void WMBroadcaster::baseMapCallback(const autoware_lanelet2_msgs::MapBinConstPtr& map_msg)
{
  std::lock_guard<std::mutex> guard(map_mutex_);

  static bool firstCall = true;
  // This function should generally only ever be called one time so log a warning if it occurs multiple times
  if (firstCall)
  {
    firstCall = false;
    ROS_INFO("WMBroadcaster::baseMapCallback called for first time with new map message");
  }
  else
  {
    ROS_WARN("WMBroadcaster::baseMapCallback called multiple times in the same node");
  }

  lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap);
  lanelet::LaneletMapPtr new_map_to_change(new lanelet::LaneletMap);

  lanelet::utils::conversion::fromBinMsg(*map_msg, new_map);
  lanelet::utils::conversion::fromBinMsg(*map_msg, new_map_to_change);

  base_map_ = new_map;  // Store map
  current_map_ = new_map_to_change; // broadcaster makes changes to this

  lanelet::MapConformer::ensureCompliance(base_map_, config_limit);     // Update map to ensure it complies with expectations
  lanelet::MapConformer::ensureCompliance(current_map_, config_limit);

  // Publish map
  current_map_version_ += 1; // Increment the map version. It should always start from 1 for the first map
  autoware_lanelet2_msgs::MapBin compliant_map_msg;
  lanelet::utils::conversion::toBinMsg(base_map_, &compliant_map_msg);
  compliant_map_msg.map_version = current_map_version_;
  map_pub_(compliant_map_msg);
};

std::shared_ptr<Geofence> WMBroadcaster::geofenceFromMsg(const cav_msgs::TrafficControlMessageV01& msg_v01)
{
  auto gf_ptr = std::make_shared<Geofence>(Geofence());
  // Get ID
  std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin());

  // Get affected lanelet or areas by converting the georeference and querying the map using points in the geofence
  gf_ptr->affected_parts_ = getAffectedLaneletOrAreas(msg_v01);

  std::vector<lanelet::Lanelet> affected_llts;
  std::vector<lanelet::Area> affected_areas;

  // used for assigning them to the regem as parameters
  for (auto llt_or_area : gf_ptr->affected_parts_)
  {
    if (llt_or_area.isLanelet()) affected_llts.push_back(current_map_->laneletLayer.get(llt_or_area.lanelet()->id()));
    if (llt_or_area.isArea()) affected_areas.push_back(current_map_->areaLayer.get(llt_or_area.area()->id()));
  }

  // TODO: logic to determine what type of geofence goes here
  // currently only converting portion of control message that is relevant to:
  // - digital speed limit, passing control line, digital minimum gap, region access rule
  lanelet::Velocity sL;
  cav_msgs::TrafficControlDetail msg_detail = msg_v01.params.detail;

  if (msg_detail.choice == cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE)
  {
    //Acquire speed limit information from TafficControlDetail msg
    sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPH());

    if(config_limit > 0_mph && config_limit < 80_mph && config_limit < sL)//Accounting for the configured speed limit, input zero when not in use
        sL = config_limit;
    //Ensure Geofences do not provide invalid speed limit data (exceed predetermined maximum value)
    // @SONAR_STOP@
    if(sL > 80_mph )
    {
     ROS_WARN_STREAM("Digital maximum speed limit is invalid. Value capped at max speed limit."); //Output warning message
     sL = 80_mph; //Cap the speed limit to the predetermined maximum value

    }
    if(sL < 0_mph)
    {
           ROS_WARN_STREAM("Digital  speed limit is invalid. Value set to 0mph.");
      sL = 0_mph;
    }// @SONAR_START@
    gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
                                        sL, affected_llts, affected_areas, { lanelet::Participants::VehicleCar }));
  }

  if (msg_detail.choice == cav_msgs::TrafficControlDetail::MINSPEED_CHOICE)
  {
    //Acquire speed limit information from TafficControlDetail msg
     sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPH());
     if(config_limit > 0_mph && config_limit < 80_mph)//Accounting for the configured speed limit, input zero when not in use
        sL = config_limit;
    //Ensure Geofences do not provide invalid speed limit data
    // @SONAR_STOP@
    if(sL > 80_mph )
    {
     ROS_WARN_STREAM("Digital speed limit is invalid. Value capped at max speed limit.");
     sL = 80_mph;
    }
    if(sL < 0_mph)
    {
           ROS_WARN_STREAM("Digital  speed limit is invalid. Value set to 0mph.");
      sL = 0_mph;
    }// @SONAR_START@
    gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
                                        sL, affected_llts, affected_areas, { lanelet::Participants::VehicleCar }));
  }
  if (msg_detail.choice == cav_msgs::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == cav_msgs::TrafficControlDetail::LATAFFINITY_CHOICE)
  {
    addPassingControlLineFromMsg(gf_ptr, msg_v01, affected_llts);
  }

 if (msg_detail.choice == cav_msgs::TrafficControlDetail::CLOSED_CHOICE && msg_detail.closed==cav_msgs::TrafficControlDetail::CLOSED)
  {
    addRegionAccessRule(gf_ptr,msg_v01,affected_llts);
  }

  if (msg_detail.choice == cav_msgs::TrafficControlDetail::MINHDWY_CHOICE)
  {

    double min_gap = (double)msg_detail.minhdwy;

    if(min_gap < 0)
    {
      ROS_WARN_STREAM("Digital min gap is invalid. Value set to 0 meter.");
      min_gap = 0;
    }
    addRegionMinimumGap(gf_ptr,msg_v01, min_gap, affected_llts, affected_areas);
  }

  // Handle schedule provessing
  cav_msgs::TrafficControlSchedule msg_schedule = msg_v01.params.schedule;

  ros::Time end_time = msg_schedule.end;
  if (!msg_schedule.end_exists) {
    ROS_DEBUG_STREAM("No end time for geofence, using ros::TIME_MAX");
    end_time = ros::TIME_MAX; // If there is no end time use the max time
  }

  // If the days of the week are specified then convert them to the boost format.
  GeofenceSchedule::DayOfTheWeekSet week_day_set = { 0, 1, 2, 3, 4, 5, 6 }; // Default to all days  0==Sun to 6==Sat
  if (msg_schedule.dow_exists) {
   // sun (6), mon (5), tue (4), wed (3), thu (2), fri (1), sat (0)
   week_day_set.clear();
   for (size_t i = 0; i < msg_schedule.dow.dow.size(); i++) {
     if (msg_schedule.dow.dow[i] == 1) {
      switch(i) {
          case 6: // sun
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Sunday);
            break;
          case 5: // mon
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Monday);
            break;
          case 4: // tue
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Tuesday);
            break;
          case 3: // wed
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Wednesday);
            break;
          case 2: // thur
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Thursday);
            break;
          case 1: // fri
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Friday);
            break;
          case 0: // sat
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Saturday);
            break;
          default:
            throw std::invalid_argument("Unrecognized weekday value: " + std::to_string(i));
      }
     }
   }
  }

  if (msg_schedule.between_exists) {

    for (auto daily_schedule : msg_schedule.between)
    {
      if (msg_schedule.repeat_exists) {
        gf_ptr->schedules.push_back(GeofenceSchedule(msg_schedule.start,
                                    end_time,
                                    daily_schedule.begin,
                                    daily_schedule.duration,
                                    msg_schedule.repeat.offset,
                                    msg_schedule.repeat.span,
                                    msg_schedule.repeat.period,
                                    week_day_set));
      } else {
        gf_ptr->schedules.push_back(GeofenceSchedule(msg_schedule.start,
                                  end_time,
                                  daily_schedule.begin,
                                  daily_schedule.duration,
                                  ros::Duration(0.0), // No offset
                                  daily_schedule.duration - daily_schedule.begin,   // Compute schedule portion end time
                                  daily_schedule.duration - daily_schedule.begin,   // No repetition so same as portion end time
                                  week_day_set));
      }

    }
  }
  else {
    if (msg_schedule.repeat_exists) {
      gf_ptr->schedules.push_back(GeofenceSchedule(msg_schedule.start,
                                  end_time,
                                  ros::Duration(0.0),
                                  ros::Duration(86400.0), // 24 hr daily application
                                  msg_schedule.repeat.offset,
                                  msg_schedule.repeat.span,
                                  msg_schedule.repeat.period,
                                  week_day_set));
    } else {
      gf_ptr->schedules.push_back(GeofenceSchedule(msg_schedule.start,
                                  end_time,
                                  ros::Duration(0.0),
                                  ros::Duration(86400.0), // 24 hr daily application
                                  ros::Duration(0.0),     // No offset
                                  ros::Duration(86400.0), // Applied for full lenth of 24 hrs
                                  ros::Duration(86400.0), // No repetition
                                  week_day_set));
    }

  }

  return gf_ptr;
}

void WMBroadcaster::addPassingControlLineFromMsg(std::shared_ptr<Geofence> gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, const std::vector<lanelet::Lanelet>& affected_llts) const
{
  cav_msgs::TrafficControlDetail msg_detail;
  msg_detail = msg_v01.params.detail;
  // Get affected bounds
  lanelet::LineStrings3d pcl_bounds;
  if (msg_detail.lataffinity == cav_msgs::TrafficControlDetail::LEFT)
  {
    for (auto llt : affected_llts) pcl_bounds.push_back(llt.leftBound());
    gf_ptr->pcl_affects_left_ = true;
  }
  else //right
  {
    for (auto llt : affected_llts) pcl_bounds.push_back(llt.rightBound());
    gf_ptr->pcl_affects_right_ = true;
  }

  // Get specified participants
  ros::V_string left_participants;
  ros::V_string right_participants;
  ros::V_string participants=participantsChecker(msg_v01);

  // Create the pcl depending on the allowed passing control direction, left, right, or both
  if (msg_detail.latperm[0] == cav_msgs::TrafficControlDetail::PERMITTED ||
      msg_detail.latperm[0] == cav_msgs::TrafficControlDetail::PASSINGONLY)
  {
    left_participants = participants;
  }
  else if (msg_detail.latperm[0] == cav_msgs::TrafficControlDetail::EMERGENCYONLY)
  {
    left_participants.push_back(lanelet::Participants::VehicleEmergency);
  }
  if (msg_detail.latperm[1] == cav_msgs::TrafficControlDetail::PERMITTED ||
      msg_detail.latperm[1] == cav_msgs::TrafficControlDetail::PASSINGONLY)
  {
    right_participants = participants;
  }
  else if (msg_detail.latperm[1] == cav_msgs::TrafficControlDetail::EMERGENCYONLY)
  {
    right_participants.push_back(lanelet::Participants::VehicleEmergency);
  }

  gf_ptr->regulatory_element_ = std::make_shared<lanelet::PassingControlLine>(lanelet::PassingControlLine::buildData(
    lanelet::utils::getId(), pcl_bounds, left_participants, right_participants));
}

void WMBroadcaster::addRegionAccessRule(std::shared_ptr<Geofence> gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, const std::vector<lanelet::Lanelet>& affected_llts) const
{
  const std::string& reason = msg_v01.package.label;
  auto regulatory_element = std::make_shared<lanelet::RegionAccessRule>(lanelet::RegionAccessRule::buildData(lanelet::utils::getId(),affected_llts,{},invertParticipants(participantsChecker(msg_v01)), reason));

  if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck ))
  {
    gf_ptr->invalidate_route_=true;
  }
  else
  {
    gf_ptr->invalidate_route_=false;
  }
  gf_ptr->regulatory_element_ = regulatory_element;
}

void WMBroadcaster::addRegionMinimumGap(std::shared_ptr<Geofence> gf_ptr,  const cav_msgs::TrafficControlMessageV01& msg_v01, double min_gap, const std::vector<lanelet::Lanelet>& affected_llts, const std::vector<lanelet::Area>& affected_areas) const
{
  auto regulatory_element = std::make_shared<lanelet::DigitalMinimumGap>(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(),
                                        min_gap, affected_llts, affected_areas,participantsChecker(msg_v01) ));

  gf_ptr->regulatory_element_ = regulatory_element;
}

ros::V_string WMBroadcaster::participantsChecker(const cav_msgs::TrafficControlMessageV01& msg_v01) const
{
  ros::V_string participants;
  for (j2735_msgs::TrafficControlVehClass participant : msg_v01.params.vclasses)
  {
    // Currently j2735_msgs::TrafficControlVehClass::RAIL is not supported
    if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::ANY)
    {
      participants = {lanelet::Participants::Vehicle, lanelet::Participants::Pedestrian, lanelet::Participants::Bicycle};
      break;
    }
    else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::PEDESTRIAN)
    {
      participants.push_back(lanelet::Participants::Pedestrian);
    }
    else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::BICYCLE)
    {
      participants.push_back(lanelet::Participants::Bicycle);
    }
    else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::MICROMOBILE ||
              participant.vehicle_class == j2735_msgs::TrafficControlVehClass::MOTORCYCLE)
    {
      participants.push_back(lanelet::Participants::VehicleMotorcycle);
    }
    else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::BUS)
    {
      participants.push_back(lanelet::Participants::VehicleBus);
    }
    else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::LIGHT_TRUCK_VAN ||
            participant.vehicle_class == j2735_msgs::TrafficControlVehClass::PASSENGER_CAR)
    {
      participants.push_back(lanelet::Participants::VehicleCar);
    }
    else if (8<= participant.vehicle_class && participant.vehicle_class <= 16) // Truck enum definition range from 8-16 currently
    {
      participants.push_back(lanelet::Participants::VehicleTruck);
    }
  }

  return  participants;
}

ros::V_string WMBroadcaster::invertParticipants(const ros::V_string& input_participants) const
{
  ros::V_string participants;

  if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Pedestrian ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Pedestrian);
  if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Bicycle ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Bicycle);
  if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Vehicle ) == input_participants.end())
  {
    if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleMotorcycle);
    if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleBus);
    if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleCar);
    if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleTruck);
  }
  return  participants;
}

// currently only supports geofence message version 1: TrafficControlMessageV01
void WMBroadcaster::geofenceCallback(const cav_msgs::TrafficControlMessage& geofence_msg)
{

  std::lock_guard<std::mutex> guard(map_mutex_);
  // quickly check if the id has been added
  if (geofence_msg.choice != cav_msgs::TrafficControlMessage::TCMV01)
    return;

  boost::uuids::uuid id;
  std::copy(geofence_msg.tcmV01.id.id.begin(), geofence_msg.tcmV01.id.id.end(), id.begin());
  if (checked_geofence_ids_.find(boost::uuids::to_string(id)) != checked_geofence_ids_.end())
    return;

  // convert reqid to string check if it has been seen before
  boost::array<uint8_t, 16UL> req_id;
  for (auto i = 0; i < 8; i ++) req_id[i] = geofence_msg.tcmV01.reqid.id[i];
  boost::uuids::uuid uuid_id;
  std::copy(req_id.begin(),req_id.end(), uuid_id.begin());
  std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8);
  // drop if the req has never been sent
  if (generated_geofence_reqids_.find(reqid) == generated_geofence_reqids_.end() && reqid.compare("00000000") != 0)
  {
    ROS_WARN_STREAM("CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid);
    return;
  }

  checked_geofence_ids_.insert(boost::uuids::to_string(id));
  auto gf_ptr = geofenceFromMsg(geofence_msg.tcmV01);
  if (gf_ptr->affected_parts_.size() == 0)
  {
    ROS_WARN_STREAM("There is no applicable component in map for the new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
    return;
  }
  scheduler_.addGeofence(gf_ptr);  // Add the geofence to the scheduler
  ROS_INFO_STREAM("New geofence message received by WMBroadcaster with id: " << gf_ptr->id_);

};

void WMBroadcaster::geoReferenceCallback(const std_msgs::String& geo_ref)
{
  std::lock_guard<std::mutex> guard(map_mutex_);
  base_map_georef_ = geo_ref.data;
}

void WMBroadcaster::setMaxLaneWidth(double max_lane_width)
{
  max_lane_width_ = max_lane_width;
}

void WMBroadcaster::setConfigSpeedLimit(double cL)
{
  /*Logic to change config_lim to Velocity value config_limit*/
  config_limit = lanelet::Velocity(cL * lanelet::units::MPH());
}

// currently only supports geofence message version 1: TrafficControlMessageV01
lanelet::ConstLaneletOrAreas WMBroadcaster::getAffectedLaneletOrAreas(const cav_msgs::TrafficControlMessageV01& tcmV01)
{
  if (!current_map_)
  {
    throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
  }
  if (base_map_georef_ == "")
    throw lanelet::InvalidObjectStateError(std::string("Base lanelet map has empty proj string loaded as georeference. Therefore, WMBroadcaster failed to\n ") +
                                          std::string("get transformation between the geofence and the map"));

  PJ* geofence_in_map_proj = proj_create_crs_to_crs(PJ_DEFAULT_CTX, tcmV01.geometry.proj.c_str(), base_map_georef_.c_str(), nullptr);

  // convert all geofence points into our map's frame
  std::vector<lanelet::Point3d> gf_pts;
  for (auto pt : tcmV01.geometry.nodes)
  {
    ROS_DEBUG_STREAM("Before conversion: Point X "<< pt.x <<" Before conversion: Point Y "<< pt.y);

    PJ_COORD c {{pt.x, pt.y, 0, 0}}; // z is not currently used
    PJ_COORD c_out;
    c_out = proj_trans(geofence_in_map_proj, PJ_FWD, c);
    gf_pts.push_back(lanelet::Point3d{current_map_->pointLayer.uniqueId(), c_out.xyz.x, c_out.xyz.y});

    ROS_DEBUG_STREAM("After conversion: Point X "<< c_out.xyz.x <<" After conversion: Point Y "<< c_out.xyz.y);
  }

  // Logic to detect which part is affected

  std::unordered_set<lanelet::Lanelet> affected_lanelets;
  for (size_t idx = 0; idx < gf_pts.size(); idx ++)
  {
    std::unordered_set<lanelet::Lanelet> possible_lanelets;
    // get nearest few nearest llts within max_lane_width_
    // which actually house this geofence_point
    auto searchFunc = [&](const lanelet::BoundingBox2d& lltBox, const lanelet::Lanelet& llt)
    {
      bool should_stop_searching = boost::geometry::distance(gf_pts[idx].basicPoint2d(), llt.polygon2d()) > max_lane_width_;
      if (!should_stop_searching && boost::geometry::within(gf_pts[idx].basicPoint2d(), llt.polygon2d()))
      {
        possible_lanelets.insert(llt);
      }
      return should_stop_searching;
    };

    // this call updates possible_lanelets
    current_map_->laneletLayer.nearestUntil(gf_pts[idx], searchFunc);

    // among these llts, filter the ones that are on same direction as the geofence using routing
    if (idx + 1 == gf_pts.size()) // we only check this for the last gf_pt after saving everything
    {
      std::unordered_set<lanelet::Lanelet> filtered = filterSuccessorLanelets(possible_lanelets, affected_lanelets);
      affected_lanelets.insert(filtered.begin(), filtered.end());
      break;
    }

    // check if each lines connecting end points of the llt is crossing with the line connecting current and next gf_pts
    for (auto llt: possible_lanelets)
    {
      lanelet::BasicLineString2d gf_dir_line({gf_pts[idx].basicPoint2d(), gf_pts[idx+1].basicPoint2d()});
      lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().end() -1)->basicPoint2d(), (llt.rightBound2d().end() - 1)->basicPoint2d()});

      // record the llts that are on the same dir
      if (boost::geometry::intersects(llt_boundary, gf_dir_line))
      {
        affected_lanelets.insert(llt);
      }
      // check condition if two geofence points are in one lanelet then check matching direction and record it also
      else if (boost::geometry::within(gf_pts[idx+1].basicPoint2d(), llt.polygon2d()) &&
              affected_lanelets.find(llt) == affected_lanelets.end())
      {
        lanelet::BasicPoint2d median({((llt.leftBound2d().end() - 1)->basicPoint2d().x() + (llt.rightBound2d().end() - 1)->basicPoint2d().x())/2 ,
                                      ((llt.leftBound2d().end() - 1)->basicPoint2d().y() + (llt.rightBound2d().end() - 1)->basicPoint2d().y())/2});
        // turn into vectors
        Eigen::Vector2d vec_to_median(median);
        Eigen::Vector2d vec_to_gf_start(gf_pts[idx].basicPoint2d());
        Eigen::Vector2d vec_to_gf_end(gf_pts[idx + 1].basicPoint2d());

        // Get vector from start to external point
        Eigen::Vector2d start_to_median = vec_to_median - vec_to_gf_start;

        // Get vector from start to end point
        Eigen::Vector2d start_to_end = vec_to_gf_end - vec_to_gf_start;

        // Get angle between both vectors
        double interior_angle = carma_wm::geometry::getAngleBetweenVectors(start_to_median, start_to_end);
        // Save the lanelet if the direction of two points inside aligns with that of the lanelet
        if (interior_angle < M_PI_2 && interior_angle >= 0) affected_lanelets.insert(llt);
      }
    }
  }

  // Currently only returning lanelet, but this could be expanded to LanelerOrArea compound object
  // by implementing non-const version of that LaneletOrArea
  lanelet::ConstLaneletOrAreas affected_parts;
  affected_parts.insert(affected_parts.end(), affected_lanelets.begin(), affected_lanelets.end());
  return affected_parts;
}

// helper function that filters successor lanelets of root_lanelets from possible_lanelets
std::unordered_set<lanelet::Lanelet> WMBroadcaster::filterSuccessorLanelets(const std::unordered_set<lanelet::Lanelet>& possible_lanelets, const std::unordered_set<lanelet::Lanelet>& root_lanelets)
{
  std::unordered_set<lanelet::Lanelet> filtered_lanelets;
  // we utilize routes to filter llts that are overlapping but not connected
  lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
  lanelet::traffic_rules::CarmaUSTrafficRules::Location, lanelet::Participants::VehicleCar);
  lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);

  // as this is the last lanelet
  // we have to filter the llts that are only geometrically overlapping yet not connected to prev llts
  for (auto recorded_llt: root_lanelets)
  {
    for (auto following_llt: map_graph->following(recorded_llt, false))
    {
      auto mutable_llt = current_map_->laneletLayer.get(following_llt.id());
      auto it = possible_lanelets.find(mutable_llt);
      if (it != possible_lanelets.end())
      {
        filtered_lanelets.insert(mutable_llt);
      }
    }
  }
  return filtered_lanelets;
}

/*!
  * \brief This is a helper function that returns true if the provided regem is marked to be changed by the geofence as there are
  *  usually multiple passing control lines are in the lanelet.
  * \param geofence_msg The ROS msg that contains geofence information
  * \param el The LaneletOrArea that houses the regem
  * \param regem The regulatoryElement that needs to be checked
  * NOTE: Currently this function only works on lanelets. It returns true if the regem is not passingControlLine or if the pcl should be changed.
  */
bool WMBroadcaster::shouldChangeControlLine(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr<Geofence> gf_ptr) const
{
  // should change if if the regem is not a passing control line or area, which is not supported by this logic
  if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::PassingControlLine::RuleName) != 0 || !el.isLanelet())
  {
    return true;
  }

  lanelet::PassingControlLinePtr pcl =  std::dynamic_pointer_cast<lanelet::PassingControlLine>(current_map_->regulatoryElementLayer.get(regem->id()));
  // if this geofence's pcl doesn't match with the lanelets current bound side, return false as we shouldn't change
  bool should_change_pcl = false;
  for (auto control_line : pcl->controlLine())
  {
    if ((control_line.id() == el.lanelet()->leftBound2d().id() && gf_ptr->pcl_affects_left_) ||
        (control_line.id() == el.lanelet()->rightBound2d().id() && gf_ptr->pcl_affects_right_))
    {
      should_change_pcl = true;
      break;
    }
  }
  return should_change_pcl;
}

void WMBroadcaster::addRegulatoryComponent(std::shared_ptr<Geofence> gf_ptr) const
{

  // First loop is to save the relation between element and regulatory element
  // so that we can add back the old one after geofence deactivates
  for (auto el: gf_ptr->affected_parts_)
  {
    for (auto regem : el.regulatoryElements())
    {
      if (!shouldChangeControlLine(el, regem, gf_ptr)) continue;

      if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
      {
        lanelet::RegulatoryElementPtr nonconst_regem = current_map_->regulatoryElementLayer.get(regem->id());
        gf_ptr->prev_regems_.push_back(std::make_pair(el.id(), nonconst_regem));
        gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));

        current_map_->remove(current_map_->laneletLayer.get(el.lanelet()->id()), nonconst_regem);
      }
    }
  }

  // this loop is also kept separately because previously we assumed
  // there was existing regem, but this handles changes to all of the elements
  for (auto el: gf_ptr->affected_parts_)
  {
    // update it with new regem
    if (gf_ptr->regulatory_element_->id() != lanelet::InvalId)
    {
      current_map_->update(current_map_->laneletLayer.get(el.id()), gf_ptr->regulatory_element_);
      gf_ptr->update_list_.push_back(std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>(el.id(), gf_ptr->regulatory_element_));
    }
  }

}

void WMBroadcaster::addBackRegulatoryComponent(std::shared_ptr<Geofence> gf_ptr) const
{
  // First loop is to remove the relation between element and regulatory element that this geofence added initially

  for (auto el: gf_ptr->affected_parts_)
  {
    for (auto regem : el.regulatoryElements())
    {
      if (!shouldChangeControlLine(el, regem, gf_ptr)) continue;

      if (regem->attribute(lanelet::AttributeName::Subtype).value() ==  gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
      {
        auto nonconst_regem = current_map_->regulatoryElementLayer.get(regem->id());
        gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
        current_map_->remove(current_map_->laneletLayer.get(el.lanelet()->id()), nonconst_regem);
      }
    }
  }

  // As this gf received is the first gf that was sent in through addGeofence,
  // we have prev speed limit information inside it to put them back
  for (auto pair : gf_ptr->prev_regems_)
  {
    if (pair.second->attribute(lanelet::AttributeName::Subtype).value() ==  gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
    {
      current_map_->update(current_map_->laneletLayer.get(pair.first), pair.second);
      gf_ptr->update_list_.push_back(pair);
    }
  }
}

void WMBroadcaster::addGeofence(std::shared_ptr<Geofence> gf_ptr)
{

  std::lock_guard<std::mutex> guard(map_mutex_);
  ROS_INFO_STREAM("Adding active geofence to the map with geofence id: " << gf_ptr->id_);

  // Process the geofence object to populate update remove lists
  addGeofenceHelper(gf_ptr);

  for (auto pair : gf_ptr->update_list_) active_geofence_llt_ids_.insert(pair.first);

  // Publish
  autoware_lanelet2_msgs::MapBin gf_msg;
  auto send_data = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_));
  carma_wm::toBinMsg(send_data, &gf_msg);
  gf_msg.invalidates_route=gf_ptr->invalidate_route_;
  gf_msg.map_version = current_map_version_;
  map_update_pub_(gf_msg);
};

void WMBroadcaster::removeGeofence(std::shared_ptr<Geofence> gf_ptr)
{
  std::lock_guard<std::mutex> guard(map_mutex_);
  ROS_INFO_STREAM("Removing inactive geofence from the map with geofence id: " << gf_ptr->id_);

  // Process the geofence object to populate update remove lists
  removeGeofenceHelper(gf_ptr);

  for (auto pair : gf_ptr->remove_list_) active_geofence_llt_ids_.erase(pair.first);

  // publish
  autoware_lanelet2_msgs::MapBin gf_msg_revert;
  auto send_data = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_));

  carma_wm::toBinMsg(send_data, &gf_msg_revert);
  gf_msg_revert.map_version = current_map_version_;
  map_update_pub_(gf_msg_revert);

};

void  WMBroadcaster::routeCallbackMessage(const cav_msgs::Route& route_msg)
{

 cav_msgs::TrafficControlRequest cR;
 cR =  controlRequestFromRoute(route_msg);
 control_msg_pub_(cR);

}

cav_msgs::TrafficControlRequest WMBroadcaster::controlRequestFromRoute(const cav_msgs::Route& route_msg, std::shared_ptr<j2735_msgs::Id64b> req_id_for_testing)
{
  lanelet::ConstLanelets path;

  if (!current_map_)
  {
   // Return / log warning etc.
    ROS_INFO_STREAM("Value 'current_map_' does not exist.");
    throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));

  }

  for(auto id : route_msg.route_path_lanelet_ids)
  {
    auto laneLayer = current_map_->laneletLayer.get(id);
    path.push_back(laneLayer);
  }

  // update local copy
  route_path_ = path;

  if(path.size() == 0) throw lanelet::InvalidObjectStateError(std::string("No lanelets available in path."));

   /*logic to determine route bounds*/
  std::vector<lanelet::ConstLanelet> llt;
  std::vector<lanelet::BoundingBox2d> pathBox;
  double minX = std::numeric_limits<double>::max();
  double minY = std::numeric_limits<double>::max();
  double maxX = std::numeric_limits<double>::lowest();
  double maxY = std::numeric_limits<double>::lowest();

  while (path.size() != 0) //Continue until there are no more lanelet elements in path
  {
      llt.push_back(path.back()); //Add a lanelet to the vector

      pathBox.push_back(lanelet::geometry::boundingBox2d(llt.back())); //Create a bounding box of the added lanelet and add it to the vector

      if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x() < minX)
        minX = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x(); //minimum x-value

      if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y() < minY)
        minY = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y(); //minimum y-value

     if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x() > maxX)
        maxX = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x(); //maximum x-value

     if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y() > maxY)
        maxY = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y(); //maximum y-value

      path.pop_back(); //remove the added lanelet from path an reduce pack.size() by 1
  } //end of while loop

  std::string target_frame = base_map_georef_;
  if (target_frame.empty())
  {
   // Return / log warning etc.
    ROS_INFO_STREAM("Value 'target_frame' is empty.");
    throw lanelet::InvalidObjectStateError(std::string("Base georeference map may not be loaded to the WMBroadcaster"));

  }

  lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str());
  lanelet::BasicPoint3d localPoint;

  localPoint.x()= minX;
  localPoint.y()= minY;

  lanelet::GPSPoint gpsRoute = local_projector.reverse(localPoint); //If the appropriate library is included, the reverse() function can be used to convert from local xyz to lat/lon

  cav_msgs::TrafficControlRequest cR; /*Fill the latitude value in message cB with the value of lat */
  cav_msgs::TrafficControlBounds cB; /*Fill the longitude value in message cB with the value of lon*/

  cB.reflat = gpsRoute.lat;
  cB.reflon = gpsRoute.lon;

  cav_msgs::OffsetPoint offsetX;
  offsetX.deltax = maxX - minX;
  cav_msgs::OffsetPoint offsetY;
  offsetY.deltay = maxY - minY;
  cB.offsets[0].deltax = minX;
  cB.offsets[0].deltay = maxY;
  cB.offsets[1].deltax = maxX;
  cB.offsets[1].deltay = minY;
  cB.offsets[2].deltax = maxX;
  cB.offsets[2].deltay = maxY;
  cB.oldest =ros::Time::now();

  cR.choice = cav_msgs::TrafficControlRequest::TCRV01;

  // create 16 byte uuid
  boost::uuids::uuid uuid_id = boost::uuids::random_generator()();
  // take half as string
  std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8);
  std::string req_id_test = "12345678"; // TODO this is an extremely risky way of performing a unit test. This method needs to be refactored so that unit tests cannot side affect actual implementations
  generated_geofence_reqids_.insert(req_id_test);
  generated_geofence_reqids_.insert(reqid);

  // copy to reqid array
  boost::array<uint8_t, 16UL> req_id;
  std::copy(uuid_id.begin(),uuid_id.end(), req_id.begin());
  for (auto i = 0; i < 8; i ++)
  {
    cR.tcrV01.reqid.id[i] = req_id[i];
    if (req_id_for_testing) req_id_for_testing->id[i] = req_id[i];
  }

  cR.tcrV01.bounds.push_back(cB);

  return cR;

}

double WMBroadcaster::distToNearestActiveGeofence(const lanelet::BasicPoint2d& curr_pos)
{
  std::lock_guard<std::mutex> guard(map_mutex_);

  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
    throw lanelet::InvalidObjectStateError(std::string("Lanelet map (current_map_) is not loaded to the WMBroadcaster"));
  }

  // filter only the lanelets in the route
  std::vector<lanelet::Id> active_geofence_on_route;
  for (auto llt : route_path_)
  {
    if (active_geofence_llt_ids_.find(llt.id()) != active_geofence_llt_ids_.end())
      active_geofence_on_route.push_back(llt.id());
  }

  // Get the lanelet of this point
  auto curr_lanelet = current_map_->laneletLayer.nearest(curr_pos, 1)[0]; //guaranteed to at least return 1 lanelet

  // Check if this point at least is actually within this lanelets
  if (!boost::geometry::within(curr_pos, curr_lanelet.polygon2d().basicPolygon()))
    throw std::invalid_argument("Given point is not within any lanelet");

  // get route distance (downtrack + cross_track) distances to every lanelets by their ids
  std::vector<double> route_distances;
  // and take abs of cross_track to add them to get route distance
  for (auto id: active_geofence_on_route)
  {
    carma_wm::TrackPos tp = carma_wm::geometry::trackPos(current_map_->laneletLayer.get(id), curr_pos);
    // downtrack needs to be negative for lanelet to be in front of the point,
    // also we don't account for the lanelet that the vehicle is on
    if (tp.downtrack < 0 && id != curr_lanelet.id())
    {
      double dist = fabs(tp.downtrack) + fabs(tp.crosstrack);
      route_distances.push_back(dist);
    }
  }
  std::sort(route_distances.begin(), route_distances.end());

  if (route_distances.size() != 0 ) return route_distances[0];
  else return 0.0;

}
// helper function that detects the type of geofence and delegates
void WMBroadcaster::addGeofenceHelper(std::shared_ptr<Geofence> gf_ptr) const
{
  // resetting the information inside geofence
  gf_ptr->remove_list_ = {};
  gf_ptr->update_list_ = {};

  // TODO: Logic to determine what type of geofence goes here in the future
  // currently only speedchange is available, so it is assumed that
  addRegulatoryComponent(gf_ptr);
}

// helper function that detects the type of geofence and delegates
void WMBroadcaster::removeGeofenceHelper(std::shared_ptr<Geofence> gf_ptr) const
{
  // again, TODO: Logic to determine what type of geofence goes here in the future
  // reset the info inside geofence
  gf_ptr->remove_list_ = {};
  gf_ptr->update_list_ = {};
  addBackRegulatoryComponent(gf_ptr);
  // as all changes are reverted back, we no longer need prev_regems
  gf_ptr->prev_regems_ = {};
}

void WMBroadcaster::currentLocationCallback(const geometry_msgs::PoseStamped& current_pos)
{
  if (current_map_ && current_map_->laneletLayer.size() != 0) {
    cav_msgs::CheckActiveGeofence check = checkActiveGeofenceLogic(current_pos);
    active_pub_(check);//Publish
  } else {
    ROS_DEBUG_STREAM("Could not check active geofence logic because map was not loaded");
  }

}

cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geometry_msgs::PoseStamped& current_pos)
{

  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
    throw lanelet::InvalidObjectStateError(std::string("Lanelet map 'current_map_' is not loaded to the WMBroadcaster"));
  }

//Store current position values to be compared to geofence boundary values
  double current_pos_x = current_pos.pose.position.x;
  double current_pos_y = current_pos.pose.position.y;

lanelet::BasicPoint2d curr_pos;
  curr_pos.x() = current_pos_x;
  curr_pos.y() = current_pos_y;

  auto current_llt = current_map_->laneletLayer.nearest(curr_pos, 1)[0];
  cav_msgs::CheckActiveGeofence outgoing_geof; //message to publish
  double next_distance = 0 ; //Distance to next geofence

  std::vector<double> route_distances;

  if (active_geofence_llt_ids_.size() <= 0 )
  {
    ROS_INFO_STREAM("No active geofence llt ids are loaded to the WMBroadcaster");
    return outgoing_geof;
  }

    /* determine whether or not the vehicle's current position is within an active geofence */
     if (boost::geometry::within(curr_pos, current_llt.polygon2d().basicPolygon()))
      {
        next_distance = distToNearestActiveGeofence(curr_pos);
        for(auto id : active_geofence_llt_ids_)
        {
          if (id == current_llt.id())
          {
            outgoing_geof.is_on_active_geofence = true;
            for (auto regem: current_llt.regulatoryElements())
              {
                  if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0)
                  {
                    lanelet::DigitalSpeedLimitPtr speed =  std::dynamic_pointer_cast<lanelet::DigitalSpeedLimit>
                    (current_map_->regulatoryElementLayer.get(regem->id()));
                    outgoing_geof.value = speed->speed_limit_.value();
                    outgoing_geof.advisory_speed = speed->speed_limit_.value();
                    outgoing_geof.type = cav_msgs::CheckActiveGeofence::SPEED_LIMIT;
                 }

                 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalMinimumGap::RuleName) == 0)
                 {
                    lanelet::DigitalMinimumGapPtr min_gap =  std::dynamic_pointer_cast<lanelet::DigitalMinimumGap>
                    (current_map_->regulatoryElementLayer.get(regem->id()));
                    outgoing_geof.minimum_gap = min_gap->getMinimumGap();
                 }

                 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
                 {
                    lanelet::RegionAccessRulePtr accessRuleReg =  std::dynamic_pointer_cast<lanelet::RegionAccessRule>
                    (current_map_->regulatoryElementLayer.get(regem->id()));
                    outgoing_geof.reason = accessRuleReg->getReason();
                    outgoing_geof.type = cav_msgs::CheckActiveGeofence::LANE_CLOSED;
                 }

              }//End for loop

          }
        }
      }

      outgoing_geof.distance_to_next_geofence = next_distance;

  //end for loop
    return outgoing_geof;

}

void WMBroadcaster::newMapSubscriber(const ros::SingleSubscriberPublisher& single_sub_pub) const {
  if (!current_map_) {
    return;
  }
  autoware_lanelet2_msgs::MapBin map_msg;
  lanelet::utils::conversion::toBinMsg(current_map_, &map_msg);
  map_msg.map_version = current_map_version_;
  single_sub_pub.publish(map_msg); // Publish the most updated version of the map to the new subscriber so any future updates are synchronized
}

}  // namespace carma_wm_ctrl


text:
// =============================================================================
// Copyright 2017 National Technology & Engineering Solutions of Sandia, LLC
// (NTESS). Under the terms of Contract DE-NA0003525 with NTESS, the U.S.
// Government retains certain rights in this software.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
// =============================================================================
// Maintained by Sandia National Laboratories <Netmeld@sandia.gov>
// =============================================================================

#ifndef PROGRAM_OPTIONS_HPP
#define PROGRAM_OPTIONS_HPP

#include <boost/program_options.hpp>

#include <netmeld/core/utils/FileManager.hpp>
#include <netmeld/core/utils/LoggerSingleton.hpp>

namespace po = boost::program_options;

#define NULL_SEMANTIC (const po::value_semantic*)NULL

namespace netmeld::core::utils {

  typedef std::tuple<const char*, const po::value_semantic*, const char*>
    OptionsValue;

  typedef std::map<std::string, OptionsValue>
    OptionsMap;

  class ProgramOptions {
    // =========================================================================
    // Variables
    // =========================================================================
    private:
      FileManager& nmfm {FileManager::getInstance()};

      po::options_description             cmdLineOptions;
      po::options_description             confFileOptions;
      po::positional_options_description  positionalOptionsLocation;
      std::map<std::string, OptionsMap>   optionsMaps;
      po::variables_map                   varMap;
      std::vector<std::string>            unrecognizedArgs;

      std::string                         commandLine;
      std::string                         confFile;

      const std::string REQUIRED  {"Required"};
      const std::string OPTIONAL  {"Optional"};
      const std::string ADVANCED  {"Advanced"};
      const std::string CONFIG    {"Configuration"};

    public:
      static const int NONE    {0};
      static const int HELP    {1};
      static const int VERSION {2};

    // =========================================================================
    // Constructors
    // =========================================================================
    public:
      ProgramOptions();

    // =========================================================================
    // Methods
    // =========================================================================
    private:
      po::options_description getMappedOptions(const std::string&,
                                               const OptionsMap&) const;

    public:
      void createCustomOptionsMap(const std::string&);

      void addOption(const std::string&, const std::string&,
                     const OptionsValue&);
      void addRequiredOption(const std::string&, const OptionsValue&);
      void addOptionalOption(const std::string&, const OptionsValue&);
      void addAdvancedOption(const std::string&, const OptionsValue&);
      void addConfFileOption(const std::string&, const OptionsValue&);
      void addPositionalOption(const std::string&, int loc);

      void removeOption(const std::string&, const std::string&);
      void removeRequiredOption(const std::string&);
      void removeOptionalOption(const std::string&);
      void removeAdvancedOption(const std::string&);

      void setConfFile(const std::string&);

      bool exists(const std::string&) const;
      std::string getCommandLine() const;
      std::string getValue(const std::string&) const;
      std::vector<std::string> getValues(const std::string&) const;
      std::vector<std::string> getUnrecognized() const;

      int validateOptions(int argc, char** argv);

      friend std::ostream& operator<<(std::ostream&, const ProgramOptions&);

      template<typename T>
      T getValueAs(const std::string& key) const
      {
        try {
          return varMap.at(key).as<T>();
        } catch (std::exception& e) {
          LOG_ERROR << "ProgramOptions::getValueAs<T>(): key lookup issue for "
                    << key
                    << std::endl;
          std::exit(Exit::FAILURE);
        }
      }
  };
}
#endif // PROGRAM_OPTIONS_HPP

max_stars_repo_name: cmwill/netmeld

max_issues_repo_name: cmwill/netmeld

max_forks_repo_name: cmwill/netmeld


text:
//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SAFE_MAX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SAFE_MAX_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing safe_max capabilities

    Returns a safe_max relative to the input,  i.e. a
    value which will not overflow when multiplied by the input.

    @par Semantic:

    For every parameter of type @c T

    @code
    auto r = safe_max(x);
    @endcode

    is similar to:

    @code
    auto r = x ? Sqrtvalmax<T>()/abs(x) : Inf<T>();
    @endcode

    @see Sqrtvalmax, safe_min

  **/
  Value safe_max(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/safe_max.hpp>
#include <boost/simd/function/simd/safe_max.hpp>

#endif

max_stars_repo_name: xmar/pythran

max_issues_repo_name: xmar/pythran

max_forks_repo_name: xmar/pythran


text:
#include "joiners/example_joiner.h"
#include "test_common.h"
#include <boost/test/unit_test.hpp>
namespace v2 = reinforcement_learning::messages::flatbuff::v2;

const int DEFAULT_REWARD = -1000;

std::vector<float> get_float_rewards(
  std::string int_file_name,
  std::string obs_file_name,
  v2::RewardFunctionType reward_function_type,
  v2::LearningModeType learning_mode=v2::LearningModeType_Online,
  v2::ProblemType problem_type=v2::ProblemType_CB,
  float default_reward=DEFAULT_REWARD
) {
  std::string command;
  switch (problem_type) {
    case v2::ProblemType_CB:
      command = "--quiet --binary_parser --cb_explore_adf";
      break;
    case v2::ProblemType_CCB:
      command = "--quiet --binary_parser --ccb_explore_adf";
      break;
  }

  auto vw = VW::initialize(command, nullptr, false, nullptr, nullptr);
  v_array<example *> examples = v_init<example*>();
  example_joiner joiner(vw);

  joiner.set_problem_type_config(problem_type);
  joiner.set_learning_mode_config(learning_mode);
  joiner.set_reward_function(reward_function_type);
  joiner.set_default_reward(default_reward);

  std::string input_files = get_test_files_location();
  auto interaction_buffer = read_file(input_files + "/reward_functions/" + int_file_name);
  auto observation_buffer = read_file(input_files + "/reward_functions/" + obs_file_name);

  // need to keep the fb buffer around in order to process the event
  std::vector<flatbuffers::DetachedBuffer> int_detached_buffers;
  auto joined_int_events = wrap_into_joined_events(interaction_buffer, int_detached_buffers);

  for (auto &je : joined_int_events) {
    joiner.process_event(*je);
    examples.push_back(&VW::get_unused_example(vw));
  }

  // need to keep the fb buffer around in order to process the event
  std::vector<flatbuffers::DetachedBuffer> obs_detached_buffers;
  auto joined_obs_events = wrap_into_joined_events(observation_buffer, obs_detached_buffers);

  for (auto &je : joined_obs_events) {
    joiner.process_event(*je);
  }

  joiner.process_joined(examples);

  std::vector<float> rewards;

  switch (problem_type) {
    case v2::ProblemType_CB:
    {
      for (auto *example : examples) {
        if (!CB::ec_is_example_header(*example) && example->l.cb.costs.size() > 0) {
          rewards.push_back(-1.0 * example->l.cb.costs[0].cost);
        }
      }
    }
      break;
    case v2::ProblemType_CCB:
      {
        // learn/predict isn't called in the unit test but cleanup examples expects
        // shared pred to be set
        examples[0]->pred.decision_scores = {v_init<ACTION_SCORE::action_score>()};
        examples[0]->pred.decision_scores[0].push_back({0, 0.f});

        for (auto *example : examples) {
          if (example->l.conditional_contextual_bandit.type ==
            CCB::example_type::slot) {
            rewards.push_back(-1.0 * example->l.conditional_contextual_bandit.outcome->cost);
          }
        }
      }
      break;
  }

  clear_examples(examples, vw);
  VW::finish(*vw);
  return rewards;
}

BOOST_AUTO_TEST_SUITE(reward_functions_with_cb_format_and_float_reward)

// 3 rewards in f-reward_3obs_v2.fb are: 5, 4, 3 and timestamps are in
// descending order added in test_common.cc
BOOST_AUTO_TEST_CASE(earliest) {
  auto rewards = get_float_rewards(
    "cb/cb_v2.fb",
    "cb/f-reward_3obs_v2.fb",
    v2::RewardFunctionType_Earliest
  );

  BOOST_CHECK_EQUAL(rewards.size(), 1);
  BOOST_CHECK_EQUAL(rewards.front(), 3);
}

BOOST_AUTO_TEST_CASE(average) {
  auto rewards = get_float_rewards(
    "cb/cb_v2.fb",
    "cb/f-reward_3obs_v2.fb",
    v2::RewardFunctionType_Average
  );

  BOOST_CHECK_EQUAL(rewards.size(), 1);
  BOOST_CHECK_EQUAL(rewards.front(), (3.0 + 4 + 5) / 3);
}

BOOST_AUTO_TEST_CASE(min) {
  auto rewards = get_float_rewards(
    "cb/cb_v2.fb",
    "cb/f-reward_3obs_v2.fb",
    v2::RewardFunctionType_Min
  );

  BOOST_CHECK_EQUAL(rewards.size(), 1);
  BOOST_CHECK_EQUAL(rewards.front(), 3);
}

BOOST_AUTO_TEST_CASE(max) {
  auto rewards = get_float_rewards(
    "cb/cb_v2.fb",
    "cb/f-reward_3obs_v2.fb",
    v2::RewardFunctionType_Max
  );

  BOOST_CHECK_EQUAL(rewards.size(), 1);
  BOOST_CHECK_EQUAL(rewards.front(), 5);
}

BOOST_AUTO_TEST_CASE(median) {
  auto rewards = get_float_rewards(
    "cb/cb_v2.fb",
    "cb/f-reward_3obs_v2.fb",
    v2::RewardFunctionType_Median
  );

  BOOST_CHECK_EQUAL(rewards.size(), 1);
  BOOST_CHECK_EQUAL(rewards.front(), 4);
}

BOOST_AUTO_TEST_CASE(sum) {
  auto rewards = get_float_rewards(
    "cb/cb_v2.fb",
    "cb/f-reward_3obs_v2.fb",
    v2::RewardFunctionType_Sum
  );

  BOOST_CHECK_EQUAL(rewards.size(), 1);
  BOOST_CHECK_EQUAL(rewards.front(), 3 + 4 + 5);
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(reward_functions_with_cb_format_and_appentice_mode)
BOOST_AUTO_TEST_CASE(apprentice_with_first_action_matching_baseline_action_returns_real_reward) {
  // 3 rewards in f-reward_3obs_v2.fb are: 5, 4, 3
  auto rewards = get_float_rewards(
    "cb/cb_apprentice_match_baseline_v2.fb",
    "cb/f-reward_3obs_v2.fb",
    v2::RewardFunctionType_Sum,
    v2::LearningModeType_Apprentice
  );

  BOOST_CHECK_EQUAL(rewards.size(), 1);
  BOOST_CHECK_EQUAL(rewards.front(), 3 + 4 + 5);
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(reward_functions_with_ccb_format_with_slot_index)
// fi-reward-v2.fb contains outcome events for two slots:
// for slot 0, rewards are: 2, 5, 2, 4
// for slot 1, rewards are: 2, 2, 5, 1
// test_common.cc added outcome event timestamps in descending order
BOOST_AUTO_TEST_CASE(earliest)
{
  auto rewards = get_float_rewards(
    "ccb/ccb_v2.fb",
    "ccb/fi-reward_v2.fb",
    v2::RewardFunctionType_Earliest,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), 4);
  BOOST_CHECK_EQUAL(rewards.at(1), 1);
}

BOOST_AUTO_TEST_CASE(average)
{
  auto rewards = get_float_rewards(
    "ccb/ccb_v2.fb",
    "ccb/fi-reward_v2.fb",
    v2::RewardFunctionType_Average,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), (2.0 + 5.0 + 2.0 + 4.0) / 4);
  BOOST_CHECK_EQUAL(rewards.at(1), (2.0 + 2.0 + 5.0 + 1.0) / 4);
}

BOOST_AUTO_TEST_CASE(min)
{
  auto rewards = get_float_rewards(
    "ccb/ccb_v2.fb",
    "ccb/fi-reward_v2.fb",
    v2::RewardFunctionType_Min,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), 2.0);
  BOOST_CHECK_EQUAL(rewards.at(1), 1.0);
}

BOOST_AUTO_TEST_CASE(max)
{
  auto rewards = get_float_rewards(
    "ccb/ccb_v2.fb",
    "ccb/fi-reward_v2.fb",
    v2::RewardFunctionType_Max,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), 5.0);
  BOOST_CHECK_EQUAL(rewards.at(1), 5.0);
}

BOOST_AUTO_TEST_CASE(median)
{
  auto rewards = get_float_rewards(
    "ccb/ccb_v2.fb",
    "ccb/fi-reward_v2.fb",
    v2::RewardFunctionType_Median,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), (2.0 + 4.0) / 2);
  BOOST_CHECK_EQUAL(rewards.at(1), (2.0 + 2.0) / 2);
}

BOOST_AUTO_TEST_CASE(sum)
{
  auto rewards = get_float_rewards(
    "ccb/ccb_v2.fb",
    "ccb/fi-reward_v2.fb",
    v2::RewardFunctionType_Sum,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), 2 + 5 + 2 + 4);
  BOOST_CHECK_EQUAL(rewards.at(1), 2 + 2 + 5 + 1);
}

BOOST_AUTO_TEST_CASE(set_default_reward_to_slot_with_no_matching_outcomes)
{
  // This test case is for outcome event slot index out of bound
  auto rewards = get_float_rewards(
    "ccb/ccb_v2.fb",
    "ccb/fi-out-of-bound-reward_v2.fb",
    v2::RewardFunctionType_Earliest,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), DEFAULT_REWARD);
  BOOST_CHECK_EQUAL(rewards.at(1), DEFAULT_REWARD);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(reward_functions_with_ccb_format_with_slot_id)
// ccb-with-slot-id contains 1 interaction with 2 slots with _id equals to
// slot_0 and slot_1
// fs-reward-v2.fb contains outcome events with slot id for two slots:
// for slot_0, rewards are: 2, 5, 2, 4
// for slot_1, rewards are: 2, 2, 5, 1
// test_common.cc added outcome event timestamps in descending order

BOOST_AUTO_TEST_CASE(median)
{
  auto rewards = get_float_rewards(
    "ccb/ccb-with-slot-id_v2.fb",
    "ccb/fs-reward_v2.fb",
    v2::RewardFunctionType_Median,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), (2.0 + 4.0) / 2);
  BOOST_CHECK_EQUAL(rewards.at(1), (2.0 + 2.0) / 2);
}

BOOST_AUTO_TEST_CASE(set_default_reward_if_slot_has_no_outcome_events)
{
  // This test case is for outcome slot id does not match interaction
  // slot id
  auto rewards = get_float_rewards(
    "ccb/ccb_v2.fb",
    "ccb/fs-reward_v2.fb",
    v2::RewardFunctionType_Earliest,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), DEFAULT_REWARD);
  BOOST_CHECK_EQUAL(rewards.at(1), DEFAULT_REWARD);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(reward_functions_with_ccb_format_with_mixed_slot_index_and_slot_id)
// ccb-with-slot-id contains 1 interaction with 2 slots with _id equals to
// slot_0 and slot_1
// fmix-reward-v2.fb contains outcome events with mixed slot index and slot id for two slots:
// with first slot: (0, 2.0), ("slot_0", 2.0), (0, 5.0), ("slot_0", 2.0)
// with second slot: (1, 2.0), ("slot_1", 5.0), (1, 4.0), ("slot_1", 1.0)
// test_common.cc added outcome event timestamps in descending order

BOOST_AUTO_TEST_CASE(median)
{
  auto rewards = get_float_rewards(
    "ccb/ccb-with-slot-id_v2.fb",
    "ccb/fmix-reward_v2.fb",
    v2::RewardFunctionType_Median,
    v2::LearningModeType_Online,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), (2.0 + 2.0) / 2);
  BOOST_CHECK_EQUAL(rewards.at(1), (2.0 + 4.0) / 2);
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(ccb_reward_with_apprentice_mode)
// actions for slot 0: [0, 1], actions for slot 1: [1]
// fi-reward-v2.fb contains outcome events for two slots:
// for slot 0, rewards are: 2, 5, 2, 4
// for slot 1, rewards are: 2, 2, 5, 1
BOOST_AUTO_TEST_CASE(first_action_matching_baseline_action_returns_real_reward)
{ // baseline actions: [0, 1] (set in example_gen.cc)
  auto rewards = get_float_rewards(
    "ccb/ccb-apprentice-baseline-match_v2.fb",
    "ccb/fi-reward_v2.fb",
    v2::RewardFunctionType_Sum,
    v2::LearningModeType_Apprentice,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), 2 + 5 + 2 + 4);
  BOOST_CHECK_EQUAL(rewards.at(1), 2 + 2 + 5 + 1);
}

BOOST_AUTO_TEST_CASE(first_action_not_matching_baseline_action_returns_default_reward)
{ // baseline actions: [1, 0] (set in example_gen.cc)
  auto rewards = get_float_rewards(
    "ccb/ccb-apprentice-baseline-not-match_v2.fb",
    "ccb/fi-reward_v2.fb",
    v2::RewardFunctionType_Sum,
    v2::LearningModeType_Apprentice,
    v2::ProblemType_CCB
  );

  BOOST_CHECK_EQUAL(rewards.size(), 2);
  BOOST_CHECK_EQUAL(rewards.at(0), DEFAULT_REWARD);
  BOOST_CHECK_EQUAL(rewards.at(1), DEFAULT_REWARD);
}

BOOST_AUTO_TEST_SUITE_END()

ext: cc


text:
#pragma once

#include <QCompleter>
#include <QKeyEvent>
#include <QTextEdit>
#include <boost/signals2.hpp>

class ResizingTextEdit : public QTextEdit
{
public:
    ResizingTextEdit();

    QSize sizeHint() const override;

    bool hasHeightForWidth() const override;

    boost::signals2::signal<void(QKeyEvent *)> keyPressed;

    void setCompleter(QCompleter *c);
    QCompleter *getCompleter() const;

protected:
    virtual int heightForWidth(int) const override;
    virtual void keyPressEvent(QKeyEvent *event) override;

private:
    QCompleter *completer = nullptr;

    // hadSpace is set to true in case the "textUnderCursor" word was after a space
    QString textUnderCursor(bool *hadSpace = nullptr) const;

    bool nextCompletion = false;

private slots:
    void insertCompletion(const QString &completion);
};