<commit_before>//
//  Exponent.cpp
//  Calculator
//
//  Created by Gavin Scheele on 3/27/14.
//  Copyright (c) 2014 Gavin Scheele. All rights reserved.
//

#include "Exponential.h"

Exponential::Exponential(Expression* base, Rational* exponent){
    this->type = "exponential";
    this->base = base;
    this->exponent = exponent;
    this->exde = new Integer(exponent->getDenominator());
    if (exde->getValue() != 1) {
    	//if the denominator of the exponent is not 1, make the base a root of the denominator, then setting the denominator equal to 1
    	Integer* baseAsInteger = (Integer *) base;
        base = new nthRoot(exde->getValue(), baseAsInteger->getValue(), 1);
        Integer* one = new Integer(1);
        exponent->setDenominator(one);
    }
    this->exnu = new Integer(exponent->getNumerator());
    if (canExponentiate()) {
    	exponentiate();
    }
}
Exponential::~Exponential(){

}

bool Exponential::canExponentiate() {
    if(base->type == "euler"){
        return false;

    }else if(base->type == "exponential"){
	Exponential* ex = (Exponential *) base;
	this->exponent->multiply(ex->getExponent());
	Integer* numSum = new Integer (1);
	ex->getExponent()->setNumerator(numSum);
        return false;
        // false is returned because the base itself would have already been exponentiated if it were possible

    }else if(base->type == "integer"){
        return true;

    }else if(base->type == "logarithm"){
        return false;

    }else if(base->type == "nthRoot"){
	nthRoot* nr = (nthRoot *) base;
	Rational* r = new Rational(this->exponent->getNumerator(), nr->getRoot()*this->exponent->getDenominator());
	//makes a new exponent, multiplying the denominator by the root, allowing the root to be simplified to one
	this->exponent = r;
	nr->setRoot(1);
	return false;

    }else if(base->type == "pi"){
        return false;

    }else if(base->type == "rational"){
        Rational* r = (Rational *) base;
        if (r->geteNumerator()->type == "integer" && r->geteDenominator()->type == "integer") {
          Exponential* nu = new Exponential(r->geteNumerator(), this->exponent);
          r->setNumerator(nu);
          Exponential* de = new Exponential(r->geteDenominator(), this->exponent);
          r->setDenominator(de);
        }

    }else{
        cout << "type not recognized" << endl;
    }
    return false;
}

void Exponential::exponentiate(){
	Integer* one = new Integer(1);
        Rational* oneRat = new Rational(1, 1);
	if (this->base->type == "rational") {
		Rational* ratBase = (Rational *) this->base;
		Exponential* numAsExponential = new Exponential (ratBase->geteNumerator(), this->exponent);  //no matching constructor for exponential
		Exponential* denAsExponential = new Exponential (ratBase->geteDenominator(), this->exponent);    //same error
		this->exponent = oneRat;
	}
	else {

    if (this->exponent->getNumerator()==0) {

       this->exponent=oneRat;
       this->base=one;

    }
    bool toFlip = false;
    if (exnu->getValue()<0) {
	    exnu->setValue(exnu->getValue()*-1);
            toFlip = true;
            //handles negative exponents
    }
    Expression* constantBase = 0;
    if (base->type == "integer") {              //fixed the problem for integers but nothing else
        Integer *a = (Integer *)base;
        constantBase = new Integer(a->getValue());
    }

    while (exponent->getNumerator()>1)
    	{
        base->multiply(constantBase);
        exponent->setNumerator(exponent->geteNumerator()->subtract(one));
    }
    if (toFlip) {
        Integer* one = new Integer(1);
        Rational* mouse = new Rational(one, base);
    	base = mouse;
    }

	}

}

Expression* Exponential::add(Expression* a){
    if(a->type == "euler"){

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if (ex->getBase()==this->base) {
		if (ex->getExponent()==this->exponent) {
			Integer* two = new Integer(2);
			this->multiply(two);
		}
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){

    }else if(a->type == "rational"){

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::subtract(Expression* a){
    if(a->type == "euler"){

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if (ex->getBase()==this->base) {
		if (ex->getExponent()==this->exponent) {
			Integer* zero = new Integer(0);
			this->multiply(zero);
		}
	}
    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){

    }else if(a->type == "rational"){

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::multiply(Expression* a){
	if(a->type == "euler"){
    		if (this->base->type == "euler") {
    			Rational* oneRat = new Rational(1, 1);
			this->exponent->add(oneRat);

    	}

    }else if(a->type == "exponential"){
		Exponential* ex = (Exponential *) a;
	if (this->base == ex->getBase()) {
		this->exponent->add(ex->getExponent());
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){
    	if (this->base->type == "pi") {
    			Rational* oneRat = new Rational(1, 1);
			this->exponent->add(oneRat);
    	}

    }else if(a->type == "rational"){
	Rational* r = (Rational *) a;
	Expression* numToSet = r->geteNumerator();
	numToSet->multiply(this);
	r->setNumerator(numToSet);
	return r;

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::divide(Expression* a){
	if(a->type == "euler"){
		if (this->base->type == "euler") {
    				Rational* oneRat = new Rational(1, 1);
				this->exponent->subtract(oneRat);
    		}

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if (this->base == ex->getBase()) {
		this->exponent->subtract(ex->getExponent());
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){
    	if (this->base->type == "pi") {
    				Rational* oneRat = new Rational(1, 1);
				this->exponent->subtract(oneRat);
    		}

    }else if(a->type == "rational"){
	Rational* r = (Rational *) a;
	Expression* denToSet = r->geteDenominator();
	denToSet->multiply(this);
	r->setDenominator(denToSet);
	return r;

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}

Rational* Exponential::getExponent() {
    return exponent;
}

Expression* Exponential::getBase() {
    return base;
}

Integer* Exponential::getExnu() {
	return exnu;
}

Integer* Exponential::getExde() {
	return exde;
}

void Exponential::setExnu(Integer* n) {
	exnu = n;
}

void Exponential::setExde(Integer* n) {
	exde = n;
}

void Exponential::setExponent(Rational* e) {
    exponent = e;
}

void Exponential::setBase(Expression* e) {
    base = e;
}

string Exponential::toString() {
    stringstream str;
    if(exponent->getNumerator() == 1 && exponent->getDenominator() == 1){
        str << *base;
    }
    else if(exponent->getDenominator() == 1){
        str << *base << "^" << *exponent->geteNumerator();
    }else{
        str << *base << "^" << *exponent;
    }
    return str.str();
}

ostream& Exponential::print(std::ostream& output) const{
    Exponential *a = (Exponential *)this;
    output << a->toString();
    return output;
}

bool Exponential::canAdd(Expression* b){     //use "this" as comparison. Solver will call someExpression.canAdd(&someOtherExpression)

    if (this->type == b->type && this->type != "logarithm") {
        if (this->type == "nthRoot") {
        }
        return true;
    }else if((this->type == "integer" && b->type == "rational") || (this->type == "rational" && b->type == "integer")){
        return true;
    }else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
bool Exponential::canSubtract(Expression* b){
    if (this->type == b->type) {
        return true;
    }else if((this->type == "integer" && b->type == "rational") || (this->type == "rational" && b->type == "integer")){
        return true;
    }else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
bool Exponential::canMultiply(Expression* b){
    if (this->type == b->type) {
        return true;
    }
    else if(this->type == "integer" && b->type == "rational") return true;
    else if(this->type == "rational" && b->type == "integer") return true;
    else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;

}
bool Exponential::canDivide(Expression* b){
    if (this->type == b->type) {
        return true;
    }
    else if(this->type == "integer"){
        if( b->type == "rational") return true;
    }
    else if(this->type == "rational" && b->type == "integer") return true;
    else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
<commit_msg>Fixed errors<commit_after>//
//  Exponent.cpp
//  Calculator
//
//  Created by Gavin Scheele on 3/27/14.
//  Copyright (c) 2014 Gavin Scheele. All rights reserved.
//

#include "Exponential.h"

Exponential::Exponential(Expression* base, Rational* exponent){
    this->type = "exponential";
    this->base = base;
    this->exponent = exponent;
    this->exde = new Integer(exponent->getDenominator());
    if (exde->getValue() != 1) {
    	//if the denominator of the exponent is not 1, make the base a root of the denominator, then setting the denominator equal to 1
    	Integer* baseAsInteger = (Integer *) base;
        base = new nthRoot(exde->getValue(), baseAsInteger->getValue(), 1);
        Integer* one = new Integer(1);
        exponent->setDenominator(one);
    }
    this->exnu = new Integer(exponent->getNumerator());
    if (canExponentiate()) {
    	exponentiate();
    }
}
Exponential::~Exponential(){

}

bool Exponential::canExponentiate() {
    if(base->type == "euler"){
        return false;

    }else if(base->type == "exponential"){
	Exponential* ex = (Exponential *) base;
	this->exponent->multiply(ex->getExponent());
	Integer* numSum = new Integer (1);
	ex->getExponent()->setNumerator(numSum);
        return false;
        // false is returned because the base itself would have already been exponentiated if it were possible

    }else if(base->type == "integer"){
        return true;

    }else if(base->type == "logarithm"){
        return false;

    }else if(base->type == "nthRoot"){
	nthRoot* nr = (nthRoot *) base;
	Rational* r = new Rational(this->exponent->getNumerator(), nr->getRoot()*this->exponent->getDenominator());
	//makes a new exponent, multiplying the denominator by the root, allowing the root to be simplified to one
	this->exponent = r;
	nr->setRoot(1);
	return false;

    }else if(base->type == "pi"){
        return false;

    }else if(base->type == "rational"){
        Rational* r = (Rational *) base;
        if (r->geteNumerator()->type == "integer" && r->geteDenominator()->type == "integer") {
          Exponential* nu = new Exponential(r->geteNumerator(), this->exponent);
          r->setNumerator(nu);
          Exponential* de = new Exponential(r->geteDenominator(), this->exponent);
          r->setDenominator(de);
        }

    }else{
        cout << "type not recognized" << endl;
    }
    return false;
}

void Exponential::exponentiate(){
	Integer* one = new Integer(1);
        Rational* oneRat = new Rational(1, 1);
	if (this->base->type == "rational") {
		Rational* ratBase = (Rational *) this->base;
		Exponential* numAsExponential = new Exponential (&(ratBase->geteNumerator()), &(this->exponent));  //no matching constructor for exponential
		Exponential* denAsExponential = new Exponential (&(ratBase->geteDenominator()), &(this->exponent));    //same error
		Rational* newRatBase = new Rational(numAsExponential, denAsExponential);
		this->base = newRatBase;
		this->exponent = oneRat;
	}
	else {

    if (this->exponent->getNumerator()==0) {

       this->exponent=oneRat;
       this->base=one;

    }
    bool toFlip = false;
    if (exnu->getValue()<0) {
	    exnu->setValue(exnu->getValue()*-1);
            toFlip = true;
            //handles negative exponents
    }
    Expression* constantBase = 0;
    if (base->type == "integer") {              //fixed the problem for integers but nothing else
        Integer *a = (Integer *)base;
        constantBase = new Integer(a->getValue());
    }

    while (exponent->getNumerator()>1)
    	{
        base->multiply(constantBase);
        exponent->setNumerator(exponent->geteNumerator()->subtract(one));
    }
    if (toFlip) {
        Integer* one = new Integer(1);
        Rational* mouse = new Rational(one, base);
    	base = mouse;
    }

	}

}

Expression* Exponential::add(Expression* a){
    if(a->type == "euler"){

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if (ex->getBase()==this->base) {
		if (ex->getExponent()==this->exponent) {
			Integer* two = new Integer(2);
			this->multiply(two);
		}
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){

    }else if(a->type == "rational"){

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::subtract(Expression* a){
    if(a->type == "euler"){

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if (ex->getBase()==this->base) {
		if (ex->getExponent()==this->exponent) {
			Integer* zero = new Integer(0);
			this->multiply(zero);
		}
	}
    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){

    }else if(a->type == "rational"){

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::multiply(Expression* a){
	if(a->type == "euler"){
    		if (this->base->type == "euler") {
    			Rational* oneRat = new Rational(1, 1);
			this->exponent->add(oneRat);

    	}

    }else if(a->type == "exponential"){
		Exponential* ex = (Exponential *) a;
	if (this->base == ex->getBase()) {
		this->exponent->add(ex->getExponent());
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){
    	if (this->base->type == "pi") {
    			Rational* oneRat = new Rational(1, 1);
			this->exponent->add(oneRat);
    	}

    }else if(a->type == "rational"){
	Rational* r = (Rational *) a;
	Expression* numToSet = r->geteNumerator();
	numToSet->multiply(this);
	r->setNumerator(numToSet);
	return r;

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::divide(Expression* a){
	if(a->type == "euler"){
		if (this->base->type == "euler") {
    				Rational* oneRat = new Rational(1, 1);
				this->exponent->subtract(oneRat);
    		}

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if (this->base == ex->getBase()) {
		this->exponent->subtract(ex->getExponent());
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){
    	if (this->base->type == "pi") {
    				Rational* oneRat = new Rational(1, 1);
				this->exponent->subtract(oneRat);
    		}

    }else if(a->type == "rational"){
	Rational* r = (Rational *) a;
	Expression* denToSet = r->geteDenominator();
	denToSet->multiply(this);
	r->setDenominator(denToSet);
	return r;

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}

Rational* Exponential::getExponent() {
    return exponent;
}

Expression* Exponential::getBase() {
    return base;
}

Integer* Exponential::getExnu() {
	return exnu;
}

Integer* Exponential::getExde() {
	return exde;
}

void Exponential::setExnu(Integer* n) {
	exnu = n;
}

void Exponential::setExde(Integer* n) {
	exde = n;
}

void Exponential::setExponent(Rational* e) {
    exponent = e;
}

void Exponential::setBase(Expression* e) {
    base = e;
}

string Exponential::toString() {
    stringstream str;
    if(exponent->getNumerator() == 1 && exponent->getDenominator() == 1){
        str << *base;
    }
    else if(exponent->getDenominator() == 1){
        str << *base << "^" << *exponent->geteNumerator();
    }else{
        str << *base << "^" << *exponent;
    }
    return str.str();
}

ostream& Exponential::print(std::ostream& output) const{
    Exponential *a = (Exponential *)this;
    output << a->toString();
    return output;
}

bool Exponential::canAdd(Expression* b){     //use "this" as comparison. Solver will call someExpression.canAdd(&someOtherExpression)

    if (this->type == b->type && this->type != "logarithm") {
        if (this->type == "nthRoot") {
        }
        return true;
    }else if((this->type == "integer" && b->type == "rational") || (this->type == "rational" && b->type == "integer")){
        return true;
    }else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
bool Exponential::canSubtract(Expression* b){
    if (this->type == b->type) {
        return true;
    }else if((this->type == "integer" && b->type == "rational") || (this->type == "rational" && b->type == "integer")){
        return true;
    }else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
bool Exponential::canMultiply(Expression* b){
    if (this->type == b->type) {
        return true;
    }
    else if(this->type == "integer" && b->type == "rational") return true;
    else if(this->type == "rational" && b->type == "integer") return true;
    else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;

}
bool Exponential::canDivide(Expression* b){
    if (this->type == b->type) {
        return true;
    }
    else if(this->type == "integer"){
        if( b->type == "rational") return true;
    }
    else if(this->type == "rational" && b->type == "integer") return true;
    else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
<|endoftext|>


<commit_before>/*
 * #%L
 * %%
 * Copyright (C) 2011 - 2017 BMW Car IT GmbH
 * %%
 * 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.
 * #L%
 */

#include "libjoynrclustercontroller/mqtt/MosquittoConnection.h"

#include "joynr/ClusterControllerSettings.h"
#include "joynr/MessagingSettings.h"
#include "joynr/Util.h"
#include "joynr/exceptions/JoynrException.h"

namespace joynr
{

MosquittoConnection::MosquittoConnection(const MessagingSettings& messagingSettings,
                                         const ClusterControllerSettings& ccSettings,
                                         const std::string& clientId)
        : mosquittopp(clientId.c_str(), false),
          messagingSettings(messagingSettings),
          host(messagingSettings.getBrokerUrl().getBrokerChannelsBaseUrl().getHost()),
          port(messagingSettings.getBrokerUrl().getBrokerChannelsBaseUrl().getPort()),
          channelId(),
          subscribeChannelMid(),
          topic(),
          additionalTopics(),
          additionalTopicsMutex(),
          isConnected(false),
          isRunning(false),
          isChannelIdRegistered(false),
          subscribedToChannelTopic(false),
          readyToSend(false),
          onMessageReceived(),
          onReadyToSendChangedMutex(),
          onReadyToSendChanged()
{
    JOYNR_LOG_INFO(logger(), "Init mosquitto connection using MQTT client ID: {}", clientId);
    mosqpp::lib_init();

    if (ccSettings.isMqttTlsEnabled()) {
        const std::string mqttCertificateAuthorityPemFilename =
                ccSettings.getMqttCertificateAuthorityPemFilename();
        const std::string mqttCertificateAuthorityCertificateFolderPath =
                ccSettings.getMqttCertificateAuthorityCertificateFolderPath();

        const char* mqttCertificateAuthorityPemFilename_cstr = nullptr;
        if (!mqttCertificateAuthorityPemFilename.empty()) {
            mqttCertificateAuthorityPemFilename_cstr = mqttCertificateAuthorityPemFilename.c_str();
        }

        const char* mqttCertificateAuthorityCertificateFolderPath_cstr = nullptr;
        if (!mqttCertificateAuthorityCertificateFolderPath.empty()) {
            mqttCertificateAuthorityCertificateFolderPath_cstr =
                    mqttCertificateAuthorityCertificateFolderPath.c_str();
        }

        int rc = tls_set(mqttCertificateAuthorityPemFilename_cstr,
                         mqttCertificateAuthorityCertificateFolderPath_cstr,
                         ccSettings.getMqttCertificatePemFilename().c_str(),
                         ccSettings.getMqttPrivateKeyPemFilename().c_str());

        if (rc != MOSQ_ERR_SUCCESS) {
            const std::string errorString(getErrorString(rc));
            mosqpp::lib_cleanup();
            JOYNR_LOG_FATAL(
                    logger(), "fatal failure to initialize TLS connection - {}", errorString);
        }
    } else {
        JOYNR_LOG_DEBUG(logger(), "MQTT connection not encrypted");
    }
}

MosquittoConnection::~MosquittoConnection()
{
    stop();
    stopLoop(true);

    mosqpp::lib_cleanup();
}

std::string MosquittoConnection::getErrorString(int rc)
{
    // Do not use mosq::strerror() in case of MOSQ_ERR_ERRNO
    // since it calls the MT-unsafe API strerror()
    if (rc != MOSQ_ERR_ERRNO) {
        return std::string(mosqpp::strerror(rc));
    }

    const int storedErrno = errno;
    return joynr::util::getErrorString(storedErrno);
}

void MosquittoConnection::on_disconnect(int rc)
{
    const std::string errorString(getErrorString(rc));
    setReadyToSend(false);
    if (!isConnected) {
        // In case we didn't connect yet
        JOYNR_LOG_ERROR(
                logger(), "Not yet connected to tcp://{}:{}, error: {}", host, port, errorString);
        return;
    }

    // There was indeed a disconnect...set connect to false
    isConnected = false;

    if (rc == MOSQ_ERR_SUCCESS) {
        JOYNR_LOG_INFO(logger(), "Disconnected from tcp://{}:{}", host, port);
        stopLoop();
    } else {
        JOYNR_LOG_ERROR(logger(),
                        "Unexpectedly disconnected from tcp://{}:{}, error: {}",
                        host,
                        port,
                        errorString);
        reconnect();
    }
}

void MosquittoConnection::on_log(int level, const char* str)
{
    if (level == MOSQ_LOG_ERR) {
        JOYNR_LOG_ERROR(logger(), "Mosquitto Log: {}", str);
    } else if (level == MOSQ_LOG_WARNING) {
        JOYNR_LOG_WARN(logger(), "Mosquitto Log: {}", str);
    } else if (level == MOSQ_LOG_INFO) {
        JOYNR_LOG_INFO(logger(), "Mosquitto Log: {}", str);
    } else {
        // MOSQ_LOG_NOTICE || MOSQ_LOG_DEBUG || any other log level
        JOYNR_LOG_DEBUG(logger(), "Mosquitto Log: {}", str);
    }
}

std::uint16_t MosquittoConnection::getMqttQos() const
{
    return mqttQos;
}

std::string MosquittoConnection::getMqttPrio() const
{
    static const std::string value("low");
    return value;
}

bool MosquittoConnection::isMqttRetain() const
{
    return mqttRetain;
}

void MosquittoConnection::start()
{
    JOYNR_LOG_TRACE(
            logger(), "Start called with isRunning: {}, isConnected: {}", isRunning, isConnected);

    JOYNR_LOG_INFO(logger(), "Try to connect to tcp://{}:{}", host, port);
    connect_async(host.c_str(), port, messagingSettings.getMqttKeepAliveTimeSeconds().count());

    reconnect_delay_set(messagingSettings.getMqttReconnectDelayTimeSeconds().count(),
                        messagingSettings.getMqttReconnectMaxDelayTimeSeconds().count(),
                        messagingSettings.getMqttExponentialBackoffEnabled());

    startLoop();
}

void MosquittoConnection::startLoop()
{
    int rc = loop_start();
    if (rc == MOSQ_ERR_SUCCESS) {
        JOYNR_LOG_INFO(logger(), "Mosquitto loop started");
        isRunning = true;
    } else {
        const std::string errorString(getErrorString(rc));
        JOYNR_LOG_ERROR(logger(),
                        "Mosquitto loop start failed: error: {} ({})",
                        std::to_string(rc),
                        errorString);
    }
}

void MosquittoConnection::stop()
{
    if (isConnected) {
        int rc = disconnect();

        if (rc == MOSQ_ERR_SUCCESS) {
            JOYNR_LOG_INFO(logger(), "Mosquitto Connection disconnected");
        } else {
            const std::string errorString(getErrorString(rc));
            JOYNR_LOG_ERROR(logger(),
                            "Mosquitto disconnect failed: error: {} ({})",
                            std::to_string(rc),
                            errorString);
            stopLoop(true);
        }
    } else if (isRunning) {
        stopLoop(true);
    }
    setReadyToSend(false);
}

void MosquittoConnection::stopLoop(bool force)
{
    int rc = loop_stop(force);

    if (rc == MOSQ_ERR_SUCCESS) {
        isRunning = false;
        JOYNR_LOG_INFO(logger(), "Mosquitto loop stopped");
    } else {
        const std::string errorString(getErrorString(rc));
        JOYNR_LOG_ERROR(logger(),
                        "Mosquitto loop stop failed: error: {} ({})",
                        std::to_string(rc),
                        errorString);
    }
}

void MosquittoConnection::on_connect(int rc)
{
    if (rc == MOSQ_ERR_SUCCESS) {
        JOYNR_LOG_INFO(logger(), "Mosquitto Connection established");
        isConnected = true;

        createSubscriptions();
    } else {
        const std::string errorString(getErrorString(rc));
        JOYNR_LOG_ERROR(
                logger(), "Mosquitto Connection Error: {} ({})", std::to_string(rc), errorString);
    }
}

void MosquittoConnection::createSubscriptions()
{
    while (!isChannelIdRegistered && isRunning) {
        std::this_thread::sleep_for(std::chrono::milliseconds(25));
    }
    try {
        subscribeToTopicInternal(topic, true);
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        for (const std::string& additionalTopic : additionalTopics) {
            subscribeToTopicInternal(additionalTopic);
        }
    } catch (const exceptions::JoynrRuntimeException& error) {
        JOYNR_LOG_ERROR(logger(), "Error subscribing to Mqtt topic, error: ", error.getMessage());
    }
}

void MosquittoConnection::subscribeToTopicInternal(const std::string& topic,
                                                   const bool isChannelTopic)
{
    int* mid = nullptr;
    if (isChannelTopic) {
        mid = &subscribeChannelMid;
    }
    int rc = subscribe(mid, topic.c_str(), getMqttQos());
    switch (rc) {
    case (MOSQ_ERR_SUCCESS):
        JOYNR_LOG_INFO(logger(), "Subscribed to {}", topic);
        break;
    case (MOSQ_ERR_NO_CONN): {
        const std::string errorString(getErrorString(rc));
        JOYNR_LOG_DEBUG(logger(),
                        "Subscription to {} failed: error: {} ({}). "
                        "Subscription will be restored on connect.",
                        topic,
                        std::to_string(rc),
                        errorString);
        break;
    }
    default: {
        // MOSQ_ERR_INVAL, MOSQ_ERR_NOMEM
        const std::string errorString(getErrorString(rc));
        std::string errorMsg = "Subscription to " + topic + " failed: error: " +
                               std::to_string(rc) + " (" + errorString + ")";
        throw exceptions::JoynrRuntimeException(errorMsg);
    }
    }
}

void MosquittoConnection::subscribeToTopic(const std::string& topic)
{
    if (!isChannelIdRegistered) {
        std::string errorMsg = "No channelId registered, cannot subscribe to topic " + topic;
        throw exceptions::JoynrRuntimeException(errorMsg);
    }

    {
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        if (additionalTopics.find(topic) != additionalTopics.end()) {
            JOYNR_LOG_DEBUG(logger(), "Already subscribed to topic {}", topic);
            return;
        }

        subscribeToTopicInternal(topic);

        additionalTopics.insert(topic);
    }
}

void MosquittoConnection::unsubscribeFromTopic(const std::string& topic)
{
    if (isChannelIdRegistered) {
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        if (additionalTopics.find(topic) == additionalTopics.end()) {
            JOYNR_LOG_DEBUG(logger(), "Unsubscribe called for non existing topic {}", topic);
            return;
        }
        additionalTopics.erase(topic);
        if (isConnected && isRunning) {
            int rc = unsubscribe(nullptr, topic.c_str());
            if (rc == MOSQ_ERR_SUCCESS) {
                JOYNR_LOG_INFO(logger(), "Unsubscribed from {}", topic);
            } else {
                // MOSQ_ERR_INVAL || MOSQ_ERR_NOMEM || MOSQ_ERR_NO_CONN
                const std::string errorString(getErrorString(rc));
                JOYNR_LOG_ERROR(logger(),
                                "Unsubscribe from {} failed: error: {} ({})",
                                topic,
                                std::to_string(rc),
                                errorString);
            }
        }
    }
}

void MosquittoConnection::publishMessage(
        const std::string& topic,
        const int qosLevel,
        const std::function<void(const exceptions::JoynrRuntimeException&)>& onFailure,
        uint32_t payloadlen = 0,
        const void* payload = nullptr)
{
    JOYNR_LOG_DEBUG(logger(), "Publish message of length {} to {}", payloadlen, topic);

    int mid;
    int rc = publish(&mid, topic.c_str(), payloadlen, payload, qosLevel, isMqttRetain());
    if (!(rc == MOSQ_ERR_SUCCESS)) {
        const std::string errorString(getErrorString(rc));
        if (rc == MOSQ_ERR_INVAL || rc == MOSQ_ERR_PAYLOAD_SIZE) {
            onFailure(exceptions::JoynrMessageNotSentException(
                    "message could not be sent: mid (mqtt message id): " + std::to_string(mid) +
                    ", error: " + std::to_string(rc) + " (" + errorString + ")"));
            return;
        }
        // MOSQ_ERR_NOMEM || MOSQ_ERR_NO_CONN || MOSQ_ERR_PROTOCOL ||| unexpected errors
        onFailure(exceptions::JoynrDelayMessageException(
                "error sending message: mid (mqtt message id): " + std::to_string(mid) +
                ", error: " + std::to_string(rc) + " (" + errorString + ")"));
        return;
    }
    JOYNR_LOG_TRACE(logger(), "published message with mqtt message id {}", std::to_string(mid));
}

void MosquittoConnection::registerChannelId(const std::string& channelId)
{
    this->channelId = channelId;
    topic = channelId + "/" + getMqttPrio() + "/" + "#";
    isChannelIdRegistered = true;
}

void MosquittoConnection::registerReceiveCallback(
        std::function<void(smrf::ByteVector&&)> onMessageReceived)
{
    this->onMessageReceived = onMessageReceived;
}

void MosquittoConnection::registerReadyToSendChangedCallback(
        std::function<void(bool)> onReadyToSendChanged)
{
    std::lock_guard<std::mutex> lock(onReadyToSendChangedMutex);
    this->onReadyToSendChanged = std::move(onReadyToSendChanged);
}

bool MosquittoConnection::isSubscribedToChannelTopic() const
{
    return subscribedToChannelTopic;
}

bool MosquittoConnection::isReadyToSend() const
{
    return readyToSend;
}

void MosquittoConnection::on_subscribe(int mid, int qos_count, const int* granted_qos)
{
    JOYNR_LOG_DEBUG(logger(), "Subscribed (mid: {} with granted QOS {}", mid, granted_qos[0]);

    for (int i = 1; i < qos_count; i++) {
        JOYNR_LOG_DEBUG(logger(), "QOS: {} granted {}", i, granted_qos[i]);
    }

    if (mid == subscribeChannelMid) {
        subscribedToChannelTopic = true;
        setReadyToSend(isConnected);
    }
}

void MosquittoConnection::on_message(const mosquitto_message* message)
{
    if (!onMessageReceived) {
        JOYNR_LOG_ERROR(logger(),
                        "Discarding received message, since onMessageReceived callback is empty.");
        return;
    }

    if (!message || message->payloadlen <= 0) {
        JOYNR_LOG_ERROR(
                logger(),
                "Discarding received message: invalid message or non-positive payload's length.");
        return;
    }

    std::uint8_t* data = static_cast<std::uint8_t*>(message->payload);

    // convert address of data into integral type
    std::uintptr_t integralAddress = reinterpret_cast<std::uintptr_t>(data);
    const bool overflow =
            joynr::util::isAdditionOnPointerSafe(integralAddress, message->payloadlen);
    if (overflow) {
        JOYNR_LOG_ERROR(logger(), "Discarding received message, since there is an overflow.");
        return;
    }

    smrf::ByteVector rawMessage(data, data + message->payloadlen);
    onMessageReceived(std::move(rawMessage));
}

void MosquittoConnection::on_publish(int mid)
{
    JOYNR_LOG_TRACE(logger(), "published message with mid {}", std::to_string(mid));
}

void MosquittoConnection::setReadyToSend(bool readyToSend)
{
    if (this->readyToSend != readyToSend) {
        this->readyToSend = readyToSend;

        std::lock_guard<std::mutex> lock(onReadyToSendChangedMutex);
        if (onReadyToSendChanged) {
            onReadyToSendChanged(readyToSend);
        }
    }
}

} // namespace joynr
<commit_msg>[C++] MosquittoConnection stop() calls stopLoop() when connected<commit_after>/*
 * #%L
 * %%
 * Copyright (C) 2011 - 2017 BMW Car IT GmbH
 * %%
 * 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.
 * #L%
 */

#include "libjoynrclustercontroller/mqtt/MosquittoConnection.h"

#include "joynr/ClusterControllerSettings.h"
#include "joynr/MessagingSettings.h"
#include "joynr/Util.h"
#include "joynr/exceptions/JoynrException.h"

namespace joynr
{

MosquittoConnection::MosquittoConnection(const MessagingSettings& messagingSettings,
                                         const ClusterControllerSettings& ccSettings,
                                         const std::string& clientId)
        : mosquittopp(clientId.c_str(), false),
          messagingSettings(messagingSettings),
          host(messagingSettings.getBrokerUrl().getBrokerChannelsBaseUrl().getHost()),
          port(messagingSettings.getBrokerUrl().getBrokerChannelsBaseUrl().getPort()),
          channelId(),
          subscribeChannelMid(),
          topic(),
          additionalTopics(),
          additionalTopicsMutex(),
          isConnected(false),
          isRunning(false),
          isChannelIdRegistered(false),
          subscribedToChannelTopic(false),
          readyToSend(false),
          onMessageReceived(),
          onReadyToSendChangedMutex(),
          onReadyToSendChanged()
{
    JOYNR_LOG_INFO(logger(), "Init mosquitto connection using MQTT client ID: {}", clientId);
    mosqpp::lib_init();

    if (ccSettings.isMqttTlsEnabled()) {
        const std::string mqttCertificateAuthorityPemFilename =
                ccSettings.getMqttCertificateAuthorityPemFilename();
        const std::string mqttCertificateAuthorityCertificateFolderPath =
                ccSettings.getMqttCertificateAuthorityCertificateFolderPath();

        const char* mqttCertificateAuthorityPemFilename_cstr = nullptr;
        if (!mqttCertificateAuthorityPemFilename.empty()) {
            mqttCertificateAuthorityPemFilename_cstr = mqttCertificateAuthorityPemFilename.c_str();
        }

        const char* mqttCertificateAuthorityCertificateFolderPath_cstr = nullptr;
        if (!mqttCertificateAuthorityCertificateFolderPath.empty()) {
            mqttCertificateAuthorityCertificateFolderPath_cstr =
                    mqttCertificateAuthorityCertificateFolderPath.c_str();
        }

        int rc = tls_set(mqttCertificateAuthorityPemFilename_cstr,
                         mqttCertificateAuthorityCertificateFolderPath_cstr,
                         ccSettings.getMqttCertificatePemFilename().c_str(),
                         ccSettings.getMqttPrivateKeyPemFilename().c_str());

        if (rc != MOSQ_ERR_SUCCESS) {
            const std::string errorString(getErrorString(rc));
            mosqpp::lib_cleanup();
            JOYNR_LOG_FATAL(
                    logger(), "fatal failure to initialize TLS connection - {}", errorString);
        }
    } else {
        JOYNR_LOG_DEBUG(logger(), "MQTT connection not encrypted");
    }
}

MosquittoConnection::~MosquittoConnection()
{
    stop();
    stopLoop(true);

    mosqpp::lib_cleanup();
}

std::string MosquittoConnection::getErrorString(int rc)
{
    // Do not use mosq::strerror() in case of MOSQ_ERR_ERRNO
    // since it calls the MT-unsafe API strerror()
    if (rc != MOSQ_ERR_ERRNO) {
        return std::string(mosqpp::strerror(rc));
    }

    const int storedErrno = errno;
    return joynr::util::getErrorString(storedErrno);
}

void MosquittoConnection::on_disconnect(int rc)
{
    const std::string errorString(getErrorString(rc));
    setReadyToSend(false);
    if (!isConnected) {
        // In case we didn't connect yet
        JOYNR_LOG_ERROR(
                logger(), "Not yet connected to tcp://{}:{}, error: {}", host, port, errorString);
        return;
    }

    // There was indeed a disconnect...set connect to false
    isConnected = false;

    if (rc == MOSQ_ERR_SUCCESS) {
        JOYNR_LOG_INFO(logger(), "Disconnected from tcp://{}:{}", host, port);
        stopLoop();
    } else {
        JOYNR_LOG_ERROR(logger(),
                        "Unexpectedly disconnected from tcp://{}:{}, error: {}",
                        host,
                        port,
                        errorString);
        reconnect();
    }
}

void MosquittoConnection::on_log(int level, const char* str)
{
    if (level == MOSQ_LOG_ERR) {
        JOYNR_LOG_ERROR(logger(), "Mosquitto Log: {}", str);
    } else if (level == MOSQ_LOG_WARNING) {
        JOYNR_LOG_WARN(logger(), "Mosquitto Log: {}", str);
    } else if (level == MOSQ_LOG_INFO) {
        JOYNR_LOG_INFO(logger(), "Mosquitto Log: {}", str);
    } else {
        // MOSQ_LOG_NOTICE || MOSQ_LOG_DEBUG || any other log level
        JOYNR_LOG_DEBUG(logger(), "Mosquitto Log: {}", str);
    }
}

std::uint16_t MosquittoConnection::getMqttQos() const
{
    return mqttQos;
}

std::string MosquittoConnection::getMqttPrio() const
{
    static const std::string value("low");
    return value;
}

bool MosquittoConnection::isMqttRetain() const
{
    return mqttRetain;
}

void MosquittoConnection::start()
{
    JOYNR_LOG_TRACE(
            logger(), "Start called with isRunning: {}, isConnected: {}", isRunning, isConnected);

    JOYNR_LOG_INFO(logger(), "Try to connect to tcp://{}:{}", host, port);
    connect_async(host.c_str(), port, messagingSettings.getMqttKeepAliveTimeSeconds().count());

    reconnect_delay_set(messagingSettings.getMqttReconnectDelayTimeSeconds().count(),
                        messagingSettings.getMqttReconnectMaxDelayTimeSeconds().count(),
                        messagingSettings.getMqttExponentialBackoffEnabled());

    startLoop();
}

void MosquittoConnection::startLoop()
{
    int rc = loop_start();
    if (rc == MOSQ_ERR_SUCCESS) {
        JOYNR_LOG_INFO(logger(), "Mosquitto loop started");
        isRunning = true;
    } else {
        const std::string errorString(getErrorString(rc));
        JOYNR_LOG_ERROR(logger(),
                        "Mosquitto loop start failed: error: {} ({})",
                        std::to_string(rc),
                        errorString);
    }
}

void MosquittoConnection::stop()
{
    if (isConnected) {
        int rc = disconnect();

        if (rc == MOSQ_ERR_SUCCESS) {
            JOYNR_LOG_INFO(logger(), "Mosquitto Connection disconnected");
        } else {
            const std::string errorString(getErrorString(rc));
            JOYNR_LOG_ERROR(logger(),
                            "Mosquitto disconnect failed: error: {} ({})",
                            std::to_string(rc),
                            errorString);
        }
        stopLoop(true);
    } else if (isRunning) {
        stopLoop(true);
    }
    setReadyToSend(false);
}

void MosquittoConnection::stopLoop(bool force)
{
    int rc = loop_stop(force);

    if (rc == MOSQ_ERR_SUCCESS) {
        isRunning = false;
        JOYNR_LOG_INFO(logger(), "Mosquitto loop stopped");
    } else {
        const std::string errorString(getErrorString(rc));
        JOYNR_LOG_ERROR(logger(),
                        "Mosquitto loop stop failed: error: {} ({})",
                        std::to_string(rc),
                        errorString);
    }
}

void MosquittoConnection::on_connect(int rc)
{
    if (rc == MOSQ_ERR_SUCCESS) {
        JOYNR_LOG_INFO(logger(), "Mosquitto Connection established");
        isConnected = true;

        createSubscriptions();
    } else {
        const std::string errorString(getErrorString(rc));
        JOYNR_LOG_ERROR(
                logger(), "Mosquitto Connection Error: {} ({})", std::to_string(rc), errorString);
    }
}

void MosquittoConnection::createSubscriptions()
{
    while (!isChannelIdRegistered && isRunning) {
        std::this_thread::sleep_for(std::chrono::milliseconds(25));
    }
    try {
        subscribeToTopicInternal(topic, true);
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        for (const std::string& additionalTopic : additionalTopics) {
            subscribeToTopicInternal(additionalTopic);
        }
    } catch (const exceptions::JoynrRuntimeException& error) {
        JOYNR_LOG_ERROR(logger(), "Error subscribing to Mqtt topic, error: ", error.getMessage());
    }
}

void MosquittoConnection::subscribeToTopicInternal(const std::string& topic,
                                                   const bool isChannelTopic)
{
    int* mid = nullptr;
    if (isChannelTopic) {
        mid = &subscribeChannelMid;
    }
    int rc = subscribe(mid, topic.c_str(), getMqttQos());
    switch (rc) {
    case (MOSQ_ERR_SUCCESS):
        JOYNR_LOG_INFO(logger(), "Subscribed to {}", topic);
        break;
    case (MOSQ_ERR_NO_CONN): {
        const std::string errorString(getErrorString(rc));
        JOYNR_LOG_DEBUG(logger(),
                        "Subscription to {} failed: error: {} ({}). "
                        "Subscription will be restored on connect.",
                        topic,
                        std::to_string(rc),
                        errorString);
        break;
    }
    default: {
        // MOSQ_ERR_INVAL, MOSQ_ERR_NOMEM
        const std::string errorString(getErrorString(rc));
        std::string errorMsg = "Subscription to " + topic + " failed: error: " +
                               std::to_string(rc) + " (" + errorString + ")";
        throw exceptions::JoynrRuntimeException(errorMsg);
    }
    }
}

void MosquittoConnection::subscribeToTopic(const std::string& topic)
{
    if (!isChannelIdRegistered) {
        std::string errorMsg = "No channelId registered, cannot subscribe to topic " + topic;
        throw exceptions::JoynrRuntimeException(errorMsg);
    }

    {
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        if (additionalTopics.find(topic) != additionalTopics.end()) {
            JOYNR_LOG_DEBUG(logger(), "Already subscribed to topic {}", topic);
            return;
        }

        subscribeToTopicInternal(topic);

        additionalTopics.insert(topic);
    }
}

void MosquittoConnection::unsubscribeFromTopic(const std::string& topic)
{
    if (isChannelIdRegistered) {
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        if (additionalTopics.find(topic) == additionalTopics.end()) {
            JOYNR_LOG_DEBUG(logger(), "Unsubscribe called for non existing topic {}", topic);
            return;
        }
        additionalTopics.erase(topic);
        if (isConnected && isRunning) {
            int rc = unsubscribe(nullptr, topic.c_str());
            if (rc == MOSQ_ERR_SUCCESS) {
                JOYNR_LOG_INFO(logger(), "Unsubscribed from {}", topic);
            } else {
                // MOSQ_ERR_INVAL || MOSQ_ERR_NOMEM || MOSQ_ERR_NO_CONN
                const std::string errorString(getErrorString(rc));
                JOYNR_LOG_ERROR(logger(),
                                "Unsubscribe from {} failed: error: {} ({})",
                                topic,
                                std::to_string(rc),
                                errorString);
            }
        }
    }
}

void MosquittoConnection::publishMessage(
        const std::string& topic,
        const int qosLevel,
        const std::function<void(const exceptions::JoynrRuntimeException&)>& onFailure,
        uint32_t payloadlen = 0,
        const void* payload = nullptr)
{
    JOYNR_LOG_DEBUG(logger(), "Publish message of length {} to {}", payloadlen, topic);

    int mid;
    int rc = publish(&mid, topic.c_str(), payloadlen, payload, qosLevel, isMqttRetain());
    if (!(rc == MOSQ_ERR_SUCCESS)) {
        const std::string errorString(getErrorString(rc));
        if (rc == MOSQ_ERR_INVAL || rc == MOSQ_ERR_PAYLOAD_SIZE) {
            onFailure(exceptions::JoynrMessageNotSentException(
                    "message could not be sent: mid (mqtt message id): " + std::to_string(mid) +
                    ", error: " + std::to_string(rc) + " (" + errorString + ")"));
            return;
        }
        // MOSQ_ERR_NOMEM || MOSQ_ERR_NO_CONN || MOSQ_ERR_PROTOCOL ||| unexpected errors
        onFailure(exceptions::JoynrDelayMessageException(
                "error sending message: mid (mqtt message id): " + std::to_string(mid) +
                ", error: " + std::to_string(rc) + " (" + errorString + ")"));
        return;
    }
    JOYNR_LOG_TRACE(logger(), "published message with mqtt message id {}", std::to_string(mid));
}

void MosquittoConnection::registerChannelId(const std::string& channelId)
{
    this->channelId = channelId;
    topic = channelId + "/" + getMqttPrio() + "/" + "#";
    isChannelIdRegistered = true;
}

void MosquittoConnection::registerReceiveCallback(
        std::function<void(smrf::ByteVector&&)> onMessageReceived)
{
    this->onMessageReceived = onMessageReceived;
}

void MosquittoConnection::registerReadyToSendChangedCallback(
        std::function<void(bool)> onReadyToSendChanged)
{
    std::lock_guard<std::mutex> lock(onReadyToSendChangedMutex);
    this->onReadyToSendChanged = std::move(onReadyToSendChanged);
}

bool MosquittoConnection::isSubscribedToChannelTopic() const
{
    return subscribedToChannelTopic;
}

bool MosquittoConnection::isReadyToSend() const
{
    return readyToSend;
}

void MosquittoConnection::on_subscribe(int mid, int qos_count, const int* granted_qos)
{
    JOYNR_LOG_DEBUG(logger(), "Subscribed (mid: {} with granted QOS {}", mid, granted_qos[0]);

    for (int i = 1; i < qos_count; i++) {
        JOYNR_LOG_DEBUG(logger(), "QOS: {} granted {}", i, granted_qos[i]);
    }

    if (mid == subscribeChannelMid) {
        subscribedToChannelTopic = true;
        setReadyToSend(isConnected);
    }
}

void MosquittoConnection::on_message(const mosquitto_message* message)
{
    if (!onMessageReceived) {
        JOYNR_LOG_ERROR(logger(),
                        "Discarding received message, since onMessageReceived callback is empty.");
        return;
    }

    if (!message || message->payloadlen <= 0) {
        JOYNR_LOG_ERROR(
                logger(),
                "Discarding received message: invalid message or non-positive payload's length.");
        return;
    }

    std::uint8_t* data = static_cast<std::uint8_t*>(message->payload);

    // convert address of data into integral type
    std::uintptr_t integralAddress = reinterpret_cast<std::uintptr_t>(data);
    const bool overflow =
            joynr::util::isAdditionOnPointerSafe(integralAddress, message->payloadlen);
    if (overflow) {
        JOYNR_LOG_ERROR(logger(), "Discarding received message, since there is an overflow.");
        return;
    }

    smrf::ByteVector rawMessage(data, data + message->payloadlen);
    onMessageReceived(std::move(rawMessage));
}

void MosquittoConnection::on_publish(int mid)
{
    JOYNR_LOG_TRACE(logger(), "published message with mid {}", std::to_string(mid));
}

void MosquittoConnection::setReadyToSend(bool readyToSend)
{
    if (this->readyToSend != readyToSend) {
        this->readyToSend = readyToSend;

        std::lock_guard<std::mutex> lock(onReadyToSendChangedMutex);
        if (onReadyToSendChanged) {
            onReadyToSendChanged(readyToSend);
        }
    }
}

} // namespace joynr
<|endoftext|>


<commit_before><commit_msg>ugly hack for NS_ooxml::LN_CT_Style_type being the first attribute processed<commit_after><|endoftext|>


<commit_before>/*
//  $ g++ -O3 -o createdSortedJaccsFile createdSortedJaccsFile.cpp
//  $ ./createdSortedJaccsFile network.pairs network.jaccs threshDensity.csv
*/
#include <math.h>
#include <ctime>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <set>
#include <map>
#include <utility>   // for pairs
#include <algorithm> // for swap
#define MIN_DIFF_BW_JACCS 0.0001
using namespace std;

int main (int argc, char const *argv[]){
    //************* make sure args are present:
    if (argc != 3){
        cout << "ERROR: something wrong with the inputs" << endl;
        cout << "usage:\n    " << argv[0] << " network.jaccs sortedjaccs.csv" << endl;
        exit(1);
    }
    //************* got the args
    clock_t begin = clock();
    ifstream jaccFile;
    jaccFile.open( argv[1] );
    if (!jaccFile) {
        cout << "ERROR: unable to open jaccards file" << endl;
        exit(1); // terminate with error
    }
    int edgeId1,edgeId2;
    float jacc;
    set<float> thresholdSet;
    while ( jaccFile >> edgeId1 >> edgeId2 >> jacc ) {
        if(thresholdSet.count(jacc) == 0)
          thresholdSet.insert(jacc);
    }
    jaccFile.close();jaccFile.clear();
    cout << "Done with thresholdSet creation." << endl;
    // create the file
    // if exists then overwrite everything
    FILE * sortedJaccsFile = fopen( argv[2], "w" );
    fclose(sortedJaccsFile);
    for(thIt = thresholdSet.rbegin(); thIt!=thresholdSet.rend(); thIt++){
      threshold = *thIt;
      sortedJaccsFile = fopen( argv[2], "a" );
      fprintf( sortedJaccsFile, "%.6f \n", threshold, D);
      fclose(sortedJaccsFile);
    }
    thresholdSet.clear();
    cout << "Time taken = " << double(clock() - begin)/ CLOCKS_PER_SEC << " seconds. "<< endl;
    return 0;
}
<commit_msg>Fixed errors<commit_after>/*
//  $ g++ -O3 -o createdSortedJaccsFile createdSortedJaccsFile.cpp
//  $ ./createdSortedJaccsFile network.pairs network.jaccs threshDensity.csv
*/
#include <math.h>
#include <ctime>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <set>
#include <map>
#include <utility>   // for pairs
#include <algorithm> // for swap
#define MIN_DIFF_BW_JACCS 0.0001
using namespace std;

int main (int argc, char const *argv[]){
    //************* make sure args are present:
    if (argc != 3){
        cout << "ERROR: something wrong with the inputs" << endl;
        cout << "usage:\n    " << argv[0] << " network.jaccs sortedjaccs.csv" << endl;
        exit(1);
    }
    //************* got the args
    clock_t begin = clock();
    ifstream jaccFile;
    jaccFile.open( argv[1] );
    if (!jaccFile) {
        cout << "ERROR: unable to open jaccards file" << endl;
        exit(1); // terminate with error
    }
    int edgeId1,edgeId2;
    float jacc;
    set<float> thresholdSet;
    set<float> ::reverse_iterator thIt;
    // read each line from the jacc file
    while ( jaccFile >> edgeId1 >> edgeId2 >> jacc ) {
        // if this value of jacc does exists
        // then add inser the value
        if(thresholdSet.count(jacc) == 0)
          thresholdSet.insert(jacc);
    }
    jaccFile.close();jaccFile.clear();
    cout << "Done with thresholdSet creation. total unique jaccs:" << thresholdSet.size() << endl;
    // create the file
    // if exists then overwrite everything
    FILE * sortedJaccsFile = fopen( argv[2], "w" );
    fclose(sortedJaccsFile);
    for(thIt = thresholdSet.rbegin(); thIt!=thresholdSet.rend(); thIt++){
      jacc = *thIt;
      // open the file
      sortedJaccsFile = fopen( argv[2], "a" );
      fprintf( sortedJaccsFile, "%.6f \n", jacc);
      fclose(sortedJaccsFile);
    }
    thresholdSet.clear();
    cout << "Time taken = " << double(clock() - begin)/ CLOCKS_PER_SEC << " seconds. "<< endl;
    return 0;
}
<|endoftext|>


<commit_before>#include <common/RhodesApp.h>
#include "../../../shared/generated/cpp/AudioCaptureBase.h"
#include "AudioCapture.h"

namespace rho {
	using namespace apiGenerator;
	using namespace common;

#undef DEFAULT_LOGCATEGORY
#define DEFAULT_LOGCATEGORY "AudioCapture"

class CAudioCaptureImpl: public IRhoExtension, public CAudioCaptureBase
{
	CAudioCapture *pAudio;
public:
    CAudioCaptureImpl(const rho::String& strID): CAudioCaptureBase()
    {
		pAudio = NULL;
		pAudio = new CAudioCapture(true);
    }

	~CAudioCaptureImpl()
	{
		LOG(INFO) + "Shutting down Audio Capture ";
		if (pAudio){
			delete pAudio;
			pAudio = NULL;
		}
	}

	virtual void start( const rho::Hashtable<rho::String, rho::String>& props, rho::apiGenerator::CMethodResult& oResult){
		if (!pAudio) {return;}
		pAudio->SetCallback(NULL);
		if (oResult.hasCallback()){
			DEBUGMSG(true, (L"Callback"));
			pAudio->SetCallback(&oResult);
			CMethodResult oRes;
			setProperties(props, oRes);
			pAudio->Start();
		}
		else{
			LOG(INFO) +  L"Could not start Audio Capture. Callback is Mandatory.";
			return;
		}
    }

    virtual void stop( rho::apiGenerator::CMethodResult& oResult) {
		if (!pAudio) {return;}
		pAudio->Stop();
    }

	virtual void cancel(rho::apiGenerator::CMethodResult& oResult) {
		if (!pAudio) {return;}
		pAudio->Cancel();
    }

    virtual void onBeforeNavigate(const wchar_t* szUrlBeingNavigatedTo, const CRhoExtData& oExtData)
	{
		if (!pAudio){return;}
		pAudio->Cancel();
	}

	virtual void OnAppActivate(bool bActivate, const CRhoExtData& oExtData)
	{
		if (!pAudio) {return;}
		pAudio->ApplicationFocusChange(bActivate);
	}

	virtual void setProperty( const rho::String& propertyName,  const rho::String& propertyValue, CMethodResult& oResult)
	{
		if (!pAudio) {return;}
		LOG(INFO) +  L"Setting Property " + propertyName.c_str() + " to " + propertyValue.c_str();
		BOOL bRet = pAudio->SetPropertyOrMethod(convertToStringW(propertyName).c_str(), convertToStringW(propertyValue).c_str());
		oResult.set(bRet == TRUE);
	}

    virtual void setProperties( const rho::Hashtable<rho::String, rho::String>& propertyMap, rho::apiGenerator::CMethodResult& oResult)
	{
		//  Set multiple properties
		if (!pAudio) {return;}
		typedef std::map<rho::String, rho::String>::const_iterator it_type;
		for (it_type iterator = propertyMap.begin(); iterator != propertyMap.end(); iterator++)
		{
			LOG(INFO) +  L"Setting Property " + iterator->first.c_str() + " to " + iterator->second.c_str();
			pAudio->SetPropertyOrMethod(convertToStringW(iterator->first).c_str(), convertToStringW(iterator->second).c_str());
		}
		oResult.set(true);
	}

	virtual void getProperty( const rho::String& propertyName, rho::apiGenerator::CMethodResult& oResult)
	{
		if (!pAudio) {return;}
		if (m_hashProps.containsKey(propertyName))
			oResult.set(m_hashProps.get(propertyName));
		else
		{
			WCHAR szValue[MAX_PATH];
			BOOL bResult = pAudio->RetrieveProperty(convertToStringW(propertyName).c_str(), szValue);
			LOG(INFO) +  L"Getting Property " + convertToStringW(propertyName).c_str() + " as " + szValue;
			if (szValue && (bResult == TRUE))
			{
				rho::StringW szStringValue;
				szStringValue.insert(0, szValue);
				oResult.set(szStringValue);
			}
			else
			{
				oResult.set(L"Unavailable");
			}
		}
	}

    virtual void getProperties( const rho::Vector<rho::String>& arrayofNames, rho::apiGenerator::CMethodResult& oResult)
	{
		if (!pAudio) {return;}
		rho::Hashtable<rho::String, rho::String> propsHash;
		CMethodResult oRes;
		typedef std::vector<rho::String>::const_iterator it_type;
		for (it_type iterator = arrayofNames.begin(); iterator != arrayofNames.end(); iterator++)
		{
			getProperty(*iterator, oRes);
			propsHash.put(*iterator, convertToStringA(oRes.toString()));
		}
		oResult.set(propsHash);
	}

    virtual void getAllProperties(rho::apiGenerator::CMethodResult& oResult)
	{
		if (!pAudio) {return;}
		rho::Hashtable<rho::String, rho::String> propsHash;
		CMethodResult oRes;
		rho::Vector<rho::String> arrayofNames;
		arrayofNames.push_back("fileName");
		arrayofNames.push_back("duration");
		typedef std::vector<rho::String>::const_iterator it_type;
		for (it_type iterator = arrayofNames.begin(); iterator != arrayofNames.end(); iterator++)
		{
			getProperty(*iterator, oRes);
			propsHash.put(*iterator, convertToStringA(oRes.toString()));
		}
		oResult.set(propsHash);
	}
};

class CAudioCaptureSingleton: public CAudioCaptureSingletonBase
{
    ~CAudioCaptureSingleton(){}
    virtual rho::String getInitialDefaultID();
    virtual void enumerate(CMethodResult& oResult);
};

class CAudioCaptureFactory: public CAudioCaptureFactoryBase
{
    ~CAudioCaptureFactory(){}
    virtual IAudioCaptureSingleton* createModuleSingleton();
    virtual IAudioCapture* createModuleByID(const rho::String& strID);
};

extern "C" void Init_AudioCapture_extension()
{
    CAudioCaptureFactory::setInstance( new CAudioCaptureFactory() );
    Init_AudioCapture_API();
}

IAudioCapture* CAudioCaptureFactory::createModuleByID(const rho::String& strID)
{
    return new CAudioCaptureImpl(strID);
}

IAudioCaptureSingleton* CAudioCaptureFactory::createModuleSingleton()
{
    return new CAudioCaptureSingleton();
}

void CAudioCaptureSingleton::enumerate(CMethodResult& oResult)
{
    rho::Vector<rho::String> arIDs;
    arIDs.addElement("SC1");
    arIDs.addElement("SC2");

    oResult.set(arIDs);
}

rho::String CAudioCaptureSingleton::getInitialDefaultID()
{
    CMethodResult oRes;
    enumerate(oRes);

    rho::Vector<rho::String>& arIDs = oRes.getStringArray();

    return arIDs[0];
}

}<commit_msg>Registering the extension<commit_after>#include <common/RhodesApp.h>
#include "../../../shared/generated/cpp/AudioCaptureBase.h"
#include "AudioCapture.h"

namespace rho {
	using namespace apiGenerator;
	using namespace common;

#undef DEFAULT_LOGCATEGORY
#define DEFAULT_LOGCATEGORY "AudioCapture"

class CAudioCaptureImpl: public IRhoExtension, public CAudioCaptureBase
{
	CAudioCapture *pAudio;
public:
    CAudioCaptureImpl(const rho::String& strID): CAudioCaptureBase()
    {
		//Is this the AudioCapture instance ID?
		m_hashProps.put( "ID", strID);

		LOG(INFO) + "Initialising interface for AudioCapture " + strID;
		RHODESAPP().getExtManager().registerExtension(strID, this );

		pAudio = NULL;
		pAudio = new CAudioCapture(true);
    }

	~CAudioCaptureImpl()
	{
		LOG(INFO) + "Shutting down Audio Capture ";
		if (pAudio){
			delete pAudio;
			pAudio = NULL;
		}
	}

	virtual void start( const rho::Hashtable<rho::String, rho::String>& props, rho::apiGenerator::CMethodResult& oResult){
		if (!pAudio) {return;}
		pAudio->SetCallback(NULL);
		if (oResult.hasCallback()){
			DEBUGMSG(true, (L"Callback"));
			pAudio->SetCallback(&oResult);
			CMethodResult oRes;
			setProperties(props, oRes);
			pAudio->Start();
		}
		else{
			LOG(INFO) +  L"Could not start Audio Capture. Callback is Mandatory.";
			return;
		}
    }

    virtual void stop( rho::apiGenerator::CMethodResult& oResult) {
		if (!pAudio) {return;}
		pAudio->Stop();
    }

	virtual void cancel(rho::apiGenerator::CMethodResult& oResult) {
		if (!pAudio) {return;}
		pAudio->Cancel();
    }

    virtual void onBeforeNavigate(const wchar_t* szUrlBeingNavigatedTo, const CRhoExtData& oExtData)
	{
		if (!pAudio){return;}
		pAudio->Cancel();
	}

	virtual void OnAppActivate(bool bActivate, const CRhoExtData& oExtData)
	{
		if (!pAudio) {return;}
		pAudio->ApplicationFocusChange(bActivate);
	}

	virtual void setProperty( const rho::String& propertyName,  const rho::String& propertyValue, CMethodResult& oResult)
	{
		if (!pAudio) {return;}
		LOG(INFO) +  L"Setting Property " + propertyName.c_str() + " to " + propertyValue.c_str();
		BOOL bRet = pAudio->SetPropertyOrMethod(convertToStringW(propertyName).c_str(), convertToStringW(propertyValue).c_str());
		oResult.set(bRet == TRUE);
	}

    virtual void setProperties( const rho::Hashtable<rho::String, rho::String>& propertyMap, rho::apiGenerator::CMethodResult& oResult)
	{
		//  Set multiple properties
		if (!pAudio) {return;}
		typedef std::map<rho::String, rho::String>::const_iterator it_type;
		for (it_type iterator = propertyMap.begin(); iterator != propertyMap.end(); iterator++)
		{
			LOG(INFO) +  L"Setting Property " + iterator->first.c_str() + " to " + iterator->second.c_str();
			pAudio->SetPropertyOrMethod(convertToStringW(iterator->first).c_str(), convertToStringW(iterator->second).c_str());
		}
		oResult.set(true);
	}

	virtual void getProperty( const rho::String& propertyName, rho::apiGenerator::CMethodResult& oResult)
	{
		if (!pAudio) {return;}
		if (m_hashProps.containsKey(propertyName))
			oResult.set(m_hashProps.get(propertyName));
		else
		{
			WCHAR szValue[MAX_PATH];
			BOOL bResult = pAudio->RetrieveProperty(convertToStringW(propertyName).c_str(), szValue);
			LOG(INFO) +  L"Getting Property " + convertToStringW(propertyName).c_str() + " as " + szValue;
			if (szValue && (bResult == TRUE))
			{
				rho::StringW szStringValue;
				szStringValue.insert(0, szValue);
				oResult.set(szStringValue);
			}
			else
			{
				oResult.set(L"Unavailable");
			}
		}
	}

    virtual void getProperties( const rho::Vector<rho::String>& arrayofNames, rho::apiGenerator::CMethodResult& oResult)
	{
		if (!pAudio) {return;}
		rho::Hashtable<rho::String, rho::String> propsHash;
		CMethodResult oRes;
		typedef std::vector<rho::String>::const_iterator it_type;
		for (it_type iterator = arrayofNames.begin(); iterator != arrayofNames.end(); iterator++)
		{
			getProperty(*iterator, oRes);
			propsHash.put(*iterator, convertToStringA(oRes.toString()));
		}
		oResult.set(propsHash);
	}

    virtual void getAllProperties(rho::apiGenerator::CMethodResult& oResult)
	{
		if (!pAudio) {return;}
		rho::Hashtable<rho::String, rho::String> propsHash;
		CMethodResult oRes;
		rho::Vector<rho::String> arrayofNames;
		arrayofNames.push_back("fileName");
		arrayofNames.push_back("duration");
		typedef std::vector<rho::String>::const_iterator it_type;
		for (it_type iterator = arrayofNames.begin(); iterator != arrayofNames.end(); iterator++)
		{
			getProperty(*iterator, oRes);
			propsHash.put(*iterator, convertToStringA(oRes.toString()));
		}
		oResult.set(propsHash);
	}
};

class CAudioCaptureSingleton: public CAudioCaptureSingletonBase
{
    ~CAudioCaptureSingleton(){}
    virtual rho::String getInitialDefaultID();
    virtual void enumerate(CMethodResult& oResult);
};

class CAudioCaptureFactory: public CAudioCaptureFactoryBase
{
    ~CAudioCaptureFactory(){}
    virtual IAudioCaptureSingleton* createModuleSingleton();
    virtual IAudioCapture* createModuleByID(const rho::String& strID);
};

extern "C" void Init_AudioCapture_extension()
{
    CAudioCaptureFactory::setInstance( new CAudioCaptureFactory() );
    Init_AudioCapture_API();
}

IAudioCapture* CAudioCaptureFactory::createModuleByID(const rho::String& strID)
{
    return new CAudioCaptureImpl(strID);
}

IAudioCaptureSingleton* CAudioCaptureFactory::createModuleSingleton()
{
    return new CAudioCaptureSingleton();
}

void CAudioCaptureSingleton::enumerate(CMethodResult& oResult)
{
    rho::Vector<rho::String> arIDs;
    arIDs.addElement("AudioCapture");
    oResult.set(arIDs);
}

rho::String CAudioCaptureSingleton::getInitialDefaultID()
{
    CMethodResult oRes;
    enumerate(oRes);

    rho::Vector<rho::String>& arIDs = oRes.getStringArray();

    return arIDs[0];
}

}<|endoftext|>


<commit_before>//
//  TapePRG.hpp
//  Clock Signal
//
//  Created by Thomas Harte on 14/08/2016.
//  Copyright © 2016 Thomas Harte. All rights reserved.
//

#ifndef Storage_Tape_PRG_hpp
#define Storage_Tape_PRG_hpp

#include "../Tape.hpp"
#include "../../FileHolder.hpp"
#include <cstdint>

namespace Storage {
namespace Tape {

/*!
	Provides a @c Tape containing a .PRG, which is a direct local file.
*/
class PRG: public Tape, public Storage::FileHolder {
	public:
		/*!
			Constructs a @c T64 containing content from the file with name @c file_name, of type @c type.

			@param file_name The name of the file to load.
			@param type The type of data the file should contain.
			@throws ErrorBadFormat if this file could not be opened and recognised as the specified type.
		*/
		PRG(const char *file_name);

		enum {
			ErrorBadFormat
		};

		// implemented to satisfy @c Tape
		bool is_at_end();

	private:
		Pulse virtual_get_next_pulse();
		void virtual_reset();

		uint16_t load_address_;
		uint16_t length_;

		enum FilePhase {
			FilePhaseLeadIn,
			FilePhaseHeader,
			FilePhaseHeaderDataGap,
			FilePhaseData,
			FilePhaseAtEnd
		} file_phase_;
		int phase_offset_;

		int bit_phase_;
		enum OutputToken {
			Leader,
			Zero,
			One,
			WordMarker,
			EndOfBlock,
			Silence
		} output_token_;
		void get_next_output_token();
		uint8_t output_byte_;
		uint8_t check_digit_;
		uint8_t copy_mask_;
};

}
}

#endif /* T64_hpp */
<commit_msg>Removed reference to a parameter long-since dead.<commit_after>//
//  TapePRG.hpp
//  Clock Signal
//
//  Created by Thomas Harte on 14/08/2016.
//  Copyright © 2016 Thomas Harte. All rights reserved.
//

#ifndef Storage_Tape_PRG_hpp
#define Storage_Tape_PRG_hpp

#include "../Tape.hpp"
#include "../../FileHolder.hpp"
#include <cstdint>

namespace Storage {
namespace Tape {

/*!
	Provides a @c Tape containing a .PRG, which is a direct local file.
*/
class PRG: public Tape, public Storage::FileHolder {
	public:
		/*!
			Constructs a @c T64 containing content from the file with name @c file_name, of type @c type.

			@param file_name The name of the file to load.
			@throws ErrorBadFormat if this file could not be opened and recognised as the specified type.
		*/
		PRG(const char *file_name);

		enum {
			ErrorBadFormat
		};

		// implemented to satisfy @c Tape
		bool is_at_end();

	private:
		Pulse virtual_get_next_pulse();
		void virtual_reset();

		uint16_t load_address_;
		uint16_t length_;

		enum FilePhase {
			FilePhaseLeadIn,
			FilePhaseHeader,
			FilePhaseHeaderDataGap,
			FilePhaseData,
			FilePhaseAtEnd
		} file_phase_;
		int phase_offset_;

		int bit_phase_;
		enum OutputToken {
			Leader,
			Zero,
			One,
			WordMarker,
			EndOfBlock,
			Silence
		} output_token_;
		void get_next_output_token();
		uint8_t output_byte_;
		uint8_t check_digit_;
		uint8_t copy_mask_;
};

}
}

#endif /* T64_hpp */
<|endoftext|>


<commit_before>//
// Copyright (c) 2018, University of Edinburgh
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//  * 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.
//  * Neither the name of  nor the names of its contributors may be used to
//    endorse or promote products derived from this software without specific
//    prior written permission.
//
// 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 OWNER 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 <exotica_ompl_solver/ompl_solver.h>

#include <ompl/util/Console.h>
#include <ompl/util/RandomNumbers.h>

namespace exotica
{
template <class ProblemType>
OMPLSolver<ProblemType>::OMPLSolver() = default;

template <class ProblemType>
OMPLSolver<ProblemType>::~OMPLSolver() = default;

template <class ProblemType>
void OMPLSolver<ProblemType>::SpecifyProblem(PlanningProblemPtr pointer)
{
    MotionSolver::SpecifyProblem(pointer);
    prob_ = std::static_pointer_cast<ProblemType>(pointer);
    if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::FIXED)
        state_space_.reset(new OMPLRNStateSpace(init_));
    else if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::PLANAR)
    {
        if (init_.IsDubinsStateSpace)
            state_space_.reset(new OMPLDubinsRNStateSpace(init_));
        else
            state_space_.reset(new OMPLRNStateSpace(init_));  // NB: We have a dedicated OMPLSE2RNStateSpace, however, for now we cannot set orientation bounds on it - as thus we are using the RN here. Cf. issue #629.
    }
    else if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::FLOATING)
        state_space_.reset(new OMPLSE3RNStateSpace(init_));
    else
        ThrowNamed("Unsupported base type " << prob_->GetScene()->GetKinematicTree().GetControlledBaseType());
    ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(state_space_));
    ompl_simple_setup_->setStateValidityChecker(ompl::base::StateValidityCheckerPtr(new OMPLStateValidityChecker(ompl_simple_setup_->getSpaceInformation(), prob_)));
    ompl_simple_setup_->setPlannerAllocator(boost::bind(planner_allocator_, _1, algorithm_));

    if (init_.Projection.rows() > 0)
    {
        std::vector<int> project_vars(init_.Projection.rows());
        for (int i = 0; i < init_.Projection.rows(); ++i)
        {
            project_vars[i] = (int)init_.Projection(i);
            if (project_vars[i] < 0 || project_vars[i] >= prob_->N) ThrowNamed("Invalid projection index! " << project_vars[i]);
        }
        if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::FIXED)
            ompl_simple_setup_->getStateSpace()->registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(new OMPLRNProjection(state_space_, project_vars)));
        else if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::PLANAR)
            ompl_simple_setup_->getStateSpace()->registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(new OMPLSE2RNProjection(state_space_, project_vars)));
        else if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::FLOATING)
            ompl_simple_setup_->getStateSpace()->registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(new OMPLSE3RNProjection(state_space_, project_vars)));
    }
}

template <class ProblemType>
int OMPLSolver<ProblemType>::GetRandomSeed() const
{
    return ompl::RNG::getSeed();
}

template <class ProblemType>
void OMPLSolver<ProblemType>::PreSolve()
{
    // Clear previously computed solutions
    if (!multi_query_)
    {
        ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths();
        const ompl::base::PlannerPtr planner = ompl_simple_setup_->getPlanner();
        if (planner)
            planner->clear();
        ompl_simple_setup_->getPlanner()->setProblemDefinition(ompl_simple_setup_->getProblemDefinition());
    }
    ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
}

template <class ProblemType>
void OMPLSolver<ProblemType>::PostSolve()
{
    ompl_simple_setup_->clearStartStates();
    int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount();
    int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
    if (debug_) CONSOLE_BRIDGE_logDebug("There were %d valid motions and %d invalid motions.", v, iv);

    if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution())
        CONSOLE_BRIDGE_logWarn("Computed solution is approximate");
}

template <class ProblemType>
void OMPLSolver<ProblemType>::SetGoalState(Eigen::VectorXdRefConst qT, const double eps)
{
    ompl::base::ScopedState<> gs(state_space_);
    state_space_->as<OMPLStateSpace>()->ExoticaToOMPLState(qT, gs.get());
    if (!ompl_simple_setup_->getStateValidityChecker()->isValid(gs.get()))
    {
        ThrowNamed("Goal state is not valid!");
    }

    if (!ompl_simple_setup_->getSpaceInformation()->satisfiesBounds(gs.get()))
    {
        state_space_->as<OMPLStateSpace>()->StateDebug(qT);

        // Debug state and bounds
        auto bounds = prob_->GetBounds();
        std::string out_of_bounds_joint_ids = "";
        for (int i = 0; i < qT.rows(); ++i)
            if (qT(i) < bounds[i] || qT(i) > bounds[i + qT.rows()])
                out_of_bounds_joint_ids += "[j" + std::to_string(i) + "=" + std::to_string(qT(i)) + ", ll=" + std::to_string(bounds[i]) + ", ul=" + std::to_string(bounds[i + qT.rows()]) + "]\n";

        ThrowNamed("Invalid goal state [Invalid joint bounds for joint indices: \n"
                   << out_of_bounds_joint_ids << "]");
    }
    ompl_simple_setup_->setGoalState(gs, eps);
}

template <class ProblemType>
void OMPLSolver<ProblemType>::GetPath(Eigen::MatrixXd &traj, ompl::base::PlannerTerminationCondition &ptc)
{
    ompl::geometric::PathSimplifierPtr psf = ompl_simple_setup_->getPathSimplifier();
    const ompl::base::SpaceInformationPtr &si = ompl_simple_setup_->getSpaceInformation();

    ompl::geometric::PathGeometric pg = ompl_simple_setup_->getSolutionPath();
    if (init_.Smooth)
    {
        bool try_more = true;
        int times = 0;
        while (init_.ReduceVertices && times < init_.SimplifyTryCnt && try_more && ptc == false)
        {
            pg.interpolate(init_.SimplifyInterpolationLength);
            try_more = psf->reduceVertices(pg, 0, 0, init_.RangeRatio);
            ++times;
        }
        if (init_.ShortcutPath && si->getStateSpace()->isMetricSpace())
        {
            times = 0;
            while (times < init_.SimplifyTryCnt && try_more && ptc == false)
            {
                pg.interpolate(init_.SimplifyInterpolationLength);
                try_more = psf->shortcutPath(pg, 0, 0, init_.RangeRatio, init_.SnapToVertex);
                ++times;
            }
        }
    }
    std::vector<ompl::base::State *> &states = pg.getStates();
    unsigned int length = 0;
    if (init_.FinalInterpolationLength > 3)
    {
        length = init_.FinalInterpolationLength;
    }
    else
    {
        const int n1 = states.size() - 1;
        for (int i = 0; i < n1; ++i)
            length += si->getStateSpace()->validSegmentCount(states[i], states[i + 1]);
    }
    pg.interpolate(int(length * init_.SmoothnessFactor));

    traj.resize(pg.getStateCount(), prob_->GetSpaceDim());
    Eigen::VectorXd tmp(prob_->GetSpaceDim());

    for (int i = 0; i < static_cast<int>(pg.getStateCount()); ++i)
    {
        state_space_->as<OMPLStateSpace>()->OMPLToExoticaState(pg.getState(i), tmp);
        traj.row(i) = tmp.transpose();
    }
}

template <class ProblemType>
void OMPLSolver<ProblemType>::Solve(Eigen::MatrixXd &solution)
{
    // Set log level
    ompl::msg::setLogLevel(debug_ ? ompl::msg::LogLevel::LOG_DEBUG : ompl::msg::LogLevel::LOG_WARN);

    Eigen::VectorXd q0 = prob_->ApplyStartState();

    // check joint limits
    const std::vector<double> bounds = prob_->GetBounds();
    for (const double l : bounds)
    {
        if (!std::isfinite(l))
        {
            std::cerr << "Detected non-finite joint limits:" << std::endl;
            const size_t nlim = bounds.size() / 2;
            for (uint i = 0; i < nlim; ++i)
            {
                std::cout << bounds[i] << ", " << bounds[nlim + i] << std::endl;
            }
            throw std::runtime_error("All joint limits need to be finite!");
        }
    }

    if (!state_space_->as<OMPLStateSpace>()->isLocked())
    {
        state_space_->as<OMPLStateSpace>()->SetBounds(prob_);
        bounds_ = prob_->GetBounds();
    }
    else if (!bounds_.empty() && bounds_ != prob_->GetBounds())
    {
        ThrowPretty("Cannot set new bounds on locked state space!");
    }

    ompl_simple_setup_->getSpaceInformation()->setup();

    ompl_simple_setup_->setup();

    if (ompl_simple_setup_->getPlanner()->params().hasParam("Range"))
        ompl_simple_setup_->getPlanner()->params().setParam("Range", init_.Range);
    if (ompl_simple_setup_->getPlanner()->params().hasParam("GoalBias"))
        ompl_simple_setup_->getPlanner()->params().setParam("GoalBias", init_.GoalBias);

    if (init_.RandomSeed > -1)
    {
        HIGHLIGHT_NAMED(algorithm_, "Setting random seed to " << init_.RandomSeed);
        ompl::RNG::setSeed(static_cast<long unsigned int>(init_.RandomSeed));
    }

    SetGoalState(prob_->GetGoalState(), init_.Epsilon);

    ompl::base::ScopedState<> ompl_start_state(state_space_);

    state_space_->as<OMPLStateSpace>()->ExoticaToOMPLState(q0, ompl_start_state.get());
    ompl_simple_setup_->setStartState(ompl_start_state);

    PreSolve();
    ompl::time::point start = ompl::time::now();
    ompl::base::PlannerTerminationCondition ptc = ompl::base::timedPlannerTerminationCondition(init_.Timeout - ompl::time::seconds(ompl::time::now() - start));

    Timer t;
    if (ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION && ompl_simple_setup_->haveSolutionPath())
    {
        GetPath(solution, ptc);
    }
    std::cout << "Finished simple setup solve" << std::endl;
    planning_time_ = t.GetDuration();
    PostSolve();
}

template class OMPLSolver<SamplingProblem>;
}
<commit_msg>[exotica_ompl_solver] Remove extraneous debug<commit_after>//
// Copyright (c) 2018, University of Edinburgh
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//  * 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.
//  * Neither the name of  nor the names of its contributors may be used to
//    endorse or promote products derived from this software without specific
//    prior written permission.
//
// 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 OWNER 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 <exotica_ompl_solver/ompl_solver.h>

#include <ompl/util/Console.h>
#include <ompl/util/RandomNumbers.h>

namespace exotica
{
template <class ProblemType>
OMPLSolver<ProblemType>::OMPLSolver() = default;

template <class ProblemType>
OMPLSolver<ProblemType>::~OMPLSolver() = default;

template <class ProblemType>
void OMPLSolver<ProblemType>::SpecifyProblem(PlanningProblemPtr pointer)
{
    MotionSolver::SpecifyProblem(pointer);
    prob_ = std::static_pointer_cast<ProblemType>(pointer);
    if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::FIXED)
        state_space_.reset(new OMPLRNStateSpace(init_));
    else if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::PLANAR)
    {
        if (init_.IsDubinsStateSpace)
            state_space_.reset(new OMPLDubinsRNStateSpace(init_));
        else
            state_space_.reset(new OMPLRNStateSpace(init_));  // NB: We have a dedicated OMPLSE2RNStateSpace, however, for now we cannot set orientation bounds on it - as thus we are using the RN here. Cf. issue #629.
    }
    else if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::FLOATING)
        state_space_.reset(new OMPLSE3RNStateSpace(init_));
    else
        ThrowNamed("Unsupported base type " << prob_->GetScene()->GetKinematicTree().GetControlledBaseType());
    ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(state_space_));
    ompl_simple_setup_->setStateValidityChecker(ompl::base::StateValidityCheckerPtr(new OMPLStateValidityChecker(ompl_simple_setup_->getSpaceInformation(), prob_)));
    ompl_simple_setup_->setPlannerAllocator(boost::bind(planner_allocator_, _1, algorithm_));

    if (init_.Projection.rows() > 0)
    {
        std::vector<int> project_vars(init_.Projection.rows());
        for (int i = 0; i < init_.Projection.rows(); ++i)
        {
            project_vars[i] = (int)init_.Projection(i);
            if (project_vars[i] < 0 || project_vars[i] >= prob_->N) ThrowNamed("Invalid projection index! " << project_vars[i]);
        }
        if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::FIXED)
            ompl_simple_setup_->getStateSpace()->registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(new OMPLRNProjection(state_space_, project_vars)));
        else if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::PLANAR)
            ompl_simple_setup_->getStateSpace()->registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(new OMPLSE2RNProjection(state_space_, project_vars)));
        else if (prob_->GetScene()->GetKinematicTree().GetControlledBaseType() == BaseType::FLOATING)
            ompl_simple_setup_->getStateSpace()->registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(new OMPLSE3RNProjection(state_space_, project_vars)));
    }
}

template <class ProblemType>
int OMPLSolver<ProblemType>::GetRandomSeed() const
{
    return ompl::RNG::getSeed();
}

template <class ProblemType>
void OMPLSolver<ProblemType>::PreSolve()
{
    // Clear previously computed solutions
    if (!multi_query_)
    {
        ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths();
        const ompl::base::PlannerPtr planner = ompl_simple_setup_->getPlanner();
        if (planner)
            planner->clear();
        ompl_simple_setup_->getPlanner()->setProblemDefinition(ompl_simple_setup_->getProblemDefinition());
    }
    ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
}

template <class ProblemType>
void OMPLSolver<ProblemType>::PostSolve()
{
    ompl_simple_setup_->clearStartStates();
    int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount();
    int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
    if (debug_) CONSOLE_BRIDGE_logDebug("There were %d valid motions and %d invalid motions.", v, iv);

    if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution())
        CONSOLE_BRIDGE_logWarn("Computed solution is approximate");
}

template <class ProblemType>
void OMPLSolver<ProblemType>::SetGoalState(Eigen::VectorXdRefConst qT, const double eps)
{
    ompl::base::ScopedState<> gs(state_space_);
    state_space_->as<OMPLStateSpace>()->ExoticaToOMPLState(qT, gs.get());
    if (!ompl_simple_setup_->getStateValidityChecker()->isValid(gs.get()))
    {
        ThrowNamed("Goal state is not valid!");
    }

    if (!ompl_simple_setup_->getSpaceInformation()->satisfiesBounds(gs.get()))
    {
        state_space_->as<OMPLStateSpace>()->StateDebug(qT);

        // Debug state and bounds
        auto bounds = prob_->GetBounds();
        std::string out_of_bounds_joint_ids = "";
        for (int i = 0; i < qT.rows(); ++i)
            if (qT(i) < bounds[i] || qT(i) > bounds[i + qT.rows()])
                out_of_bounds_joint_ids += "[j" + std::to_string(i) + "=" + std::to_string(qT(i)) + ", ll=" + std::to_string(bounds[i]) + ", ul=" + std::to_string(bounds[i + qT.rows()]) + "]\n";

        ThrowNamed("Invalid goal state [Invalid joint bounds for joint indices: \n"
                   << out_of_bounds_joint_ids << "]");
    }
    ompl_simple_setup_->setGoalState(gs, eps);
}

template <class ProblemType>
void OMPLSolver<ProblemType>::GetPath(Eigen::MatrixXd &traj, ompl::base::PlannerTerminationCondition &ptc)
{
    ompl::geometric::PathSimplifierPtr psf = ompl_simple_setup_->getPathSimplifier();
    const ompl::base::SpaceInformationPtr &si = ompl_simple_setup_->getSpaceInformation();

    ompl::geometric::PathGeometric pg = ompl_simple_setup_->getSolutionPath();
    if (init_.Smooth)
    {
        bool try_more = true;
        int times = 0;
        while (init_.ReduceVertices && times < init_.SimplifyTryCnt && try_more && ptc == false)
        {
            pg.interpolate(init_.SimplifyInterpolationLength);
            try_more = psf->reduceVertices(pg, 0, 0, init_.RangeRatio);
            ++times;
        }
        if (init_.ShortcutPath && si->getStateSpace()->isMetricSpace())
        {
            times = 0;
            while (times < init_.SimplifyTryCnt && try_more && ptc == false)
            {
                pg.interpolate(init_.SimplifyInterpolationLength);
                try_more = psf->shortcutPath(pg, 0, 0, init_.RangeRatio, init_.SnapToVertex);
                ++times;
            }
        }
    }
    std::vector<ompl::base::State *> &states = pg.getStates();
    unsigned int length = 0;
    if (init_.FinalInterpolationLength > 3)
    {
        length = init_.FinalInterpolationLength;
    }
    else
    {
        const int n1 = states.size() - 1;
        for (int i = 0; i < n1; ++i)
            length += si->getStateSpace()->validSegmentCount(states[i], states[i + 1]);
    }
    pg.interpolate(int(length * init_.SmoothnessFactor));

    traj.resize(pg.getStateCount(), prob_->GetSpaceDim());
    Eigen::VectorXd tmp(prob_->GetSpaceDim());

    for (int i = 0; i < static_cast<int>(pg.getStateCount()); ++i)
    {
        state_space_->as<OMPLStateSpace>()->OMPLToExoticaState(pg.getState(i), tmp);
        traj.row(i) = tmp.transpose();
    }
}

template <class ProblemType>
void OMPLSolver<ProblemType>::Solve(Eigen::MatrixXd &solution)
{
    // Set log level
    ompl::msg::setLogLevel(debug_ ? ompl::msg::LogLevel::LOG_DEBUG : ompl::msg::LogLevel::LOG_WARN);

    Eigen::VectorXd q0 = prob_->ApplyStartState();

    // check joint limits
    const std::vector<double> bounds = prob_->GetBounds();
    for (const double l : bounds)
    {
        if (!std::isfinite(l))
        {
            std::cerr << "Detected non-finite joint limits:" << std::endl;
            const size_t nlim = bounds.size() / 2;
            for (uint i = 0; i < nlim; ++i)
            {
                std::cout << bounds[i] << ", " << bounds[nlim + i] << std::endl;
            }
            throw std::runtime_error("All joint limits need to be finite!");
        }
    }

    if (!state_space_->as<OMPLStateSpace>()->isLocked())
    {
        state_space_->as<OMPLStateSpace>()->SetBounds(prob_);
        bounds_ = prob_->GetBounds();
    }
    else if (!bounds_.empty() && bounds_ != prob_->GetBounds())
    {
        ThrowPretty("Cannot set new bounds on locked state space!");
    }

    ompl_simple_setup_->getSpaceInformation()->setup();

    ompl_simple_setup_->setup();

    if (ompl_simple_setup_->getPlanner()->params().hasParam("Range"))
        ompl_simple_setup_->getPlanner()->params().setParam("Range", init_.Range);
    if (ompl_simple_setup_->getPlanner()->params().hasParam("GoalBias"))
        ompl_simple_setup_->getPlanner()->params().setParam("GoalBias", init_.GoalBias);

    if (init_.RandomSeed > -1)
    {
        HIGHLIGHT_NAMED(algorithm_, "Setting random seed to " << init_.RandomSeed);
        ompl::RNG::setSeed(static_cast<long unsigned int>(init_.RandomSeed));
    }

    SetGoalState(prob_->GetGoalState(), init_.Epsilon);

    ompl::base::ScopedState<> ompl_start_state(state_space_);

    state_space_->as<OMPLStateSpace>()->ExoticaToOMPLState(q0, ompl_start_state.get());
    ompl_simple_setup_->setStartState(ompl_start_state);

    PreSolve();
    ompl::time::point start = ompl::time::now();
    ompl::base::PlannerTerminationCondition ptc = ompl::base::timedPlannerTerminationCondition(init_.Timeout - ompl::time::seconds(ompl::time::now() - start));

    Timer t;
    if (ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION && ompl_simple_setup_->haveSolutionPath())
    {
        GetPath(solution, ptc);
    }
    planning_time_ = t.GetDuration();
    PostSolve();
}

template class OMPLSolver<SamplingProblem>;
}
<|endoftext|>