<commit_before>/*
 Flexisip, a flexible SIP proxy server with media capabilities.
 Copyright (C) 2012  Belledonne Communications SARL.

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU Affero General Public License as
 published by the Free Software Foundation, either version 3 of the
 License, or (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Affero General Public License for more details.

 You should have received a copy of the GNU Affero General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "forkcallcontext.hh"
#include "common.hh"
#include <algorithm>
#include <sofia-sip/sip_status.h>

using namespace ::std;

template<typename T>
static bool contains(const list<T> &l, T value) {
	return find(l.cbegin(), l.cend(), value) != l.cend();
}

ForkCallContext::ForkCallContext(Agent *agent, const std::shared_ptr<RequestSipEvent> &event, shared_ptr<ForkContextConfig> cfg, ForkContextListener* listener) :
		ForkContext(agent, event,cfg, listener), mShortTimer(NULL), mPushTimer(NULL), mLastResponseCodeSent(100), mCancelled(false) {
	LOGD("New ForkCallContext %p", this);
	mLog=event->getEventLog<CallLog>();
	mActivePushes = 0;
}

ForkCallContext::~ForkCallContext() {
	LOGD("Destroy ForkCallContext %p", this);
	if (mShortTimer){
		su_timer_destroy(mShortTimer);
		mShortTimer=NULL;
	}
	if (mPushTimer){
		su_timer_destroy(mPushTimer);
		mPushTimer=NULL;
	}
}

void ForkCallContext::cancel() {
	mLog->setCancelled();
	mLog->setCompleted();
	mCancelled=true;
	cancelOthers();
}

void ForkCallContext::forward(const shared_ptr<ResponseSipEvent> &ev, bool force) {
	sip_t *sip = ev->getMsgSip()->getSip();
	bool fakeSipEvent = ((mLastResponseCodeSent >= 200) && !force) || mIncoming == NULL;
	const int status = sip->sip_status->st_status;

	if (mCfg->mForkOneResponse) { // TODO: respect RFC 3261 16.7.5
		if (status == 183 || status == 180 || status == 101) {
			auto it = find(mForwardResponses.begin(), mForwardResponses.end(), status);
			if (it != mForwardResponses.end()) {
				fakeSipEvent = true;
			} else {
				mForwardResponses.push_back(status);
			}
		}
	}

	if (fakeSipEvent) {
		ev->setIncomingAgent(shared_ptr<IncomingAgent>());
	}else{
		if (mCfg->mRemoveToTag && (status == 183 || status == 180 || status == 101)) {
			SLOGD << "Removing 'to tag' ";
			msg_header_remove_param((msg_common_t *)sip->sip_to, "tag");
		}
		logResponse(ev);
	}
	mLastResponseCodeSent=status;
}

void ForkCallContext::decline(const shared_ptr<OutgoingTransaction> &transaction, shared_ptr<ResponseSipEvent> &ev) {
	if (!mCfg->mForkNoGlobalDecline) {
		cancelOthers(transaction);
		forward(ev);
	} else {
		if (mOutgoings.size() != 1) {
			store(ev);
		} else {
			forward(ev);
		}
	}
}

void ForkCallContext::cancelOthers(const shared_ptr<OutgoingTransaction> &transaction) {
	for (list<shared_ptr<OutgoingTransaction>>::iterator it = mOutgoings.begin(); it != mOutgoings.end();) {
		if (*it != transaction) {
			shared_ptr<OutgoingTransaction> tr = (*it);
			it = mOutgoings.erase(it);
			tr->cancel();
		} else {
			++it;
		}
	}
}

void ForkCallContext::onRequest(const shared_ptr<IncomingTransaction> &transaction, shared_ptr<RequestSipEvent> &event) {
	event->setOutgoingAgent(shared_ptr<OutgoingAgent>());
	const shared_ptr<MsgSip> &ms = event->getMsgSip();
	sip_t *sip = ms->getSip();
	if (sip != NULL && sip->sip_request != NULL) {
		if (sip->sip_request->rq_method == sip_method_cancel) {
			LOGD("Fork: incomingCallback cancel");
			cancel();
			/*
			 * let the event go through the list of modules for notification purpose, but do not send the cancel at the end since it is handled here.
			 * Indeed there might not be generated cancels for non-responded branches of the fork, letting other modules unnotified.
			**/
			event->setOutgoingAgent(shared_ptr<OutgoingAgent>());
		}
	}
}

bool ForkCallContext::isRetryableOrUrgent(int code){
	switch(code){
		case 401:
		case 407:
		case 415:
		case 420:
		case 484:
			return true;
		case 603:
			if (mCfg->mTreatDeclineAsUrgent)
				return true;
		default:
			return false;
	}
	return false;
}

void ForkCallContext::store(shared_ptr<ResponseSipEvent> &event) {
	bool best = false;
	int code=event->getMsgSip()->getSip()->sip_status->st_status;

	if (mBestResponse != NULL) {
		//we must give priority to 401, 407, 415, 420, 484 because they will trigger a request retry.
		int prev_resp_code=mBestResponse->getMsgSip()->getSip()->sip_status->st_status;
		int code_class=code/100;
		int prev_code_class=prev_resp_code/100;

		if (code_class < prev_code_class) {
			best = true;
		}
	}else best=true;

	if (best && isRetryableOrUrgent(code)){
		if (mShortTimer==NULL){
			mShortTimer=su_timer_create(su_root_task(mAgent->getRoot()), 0);
			su_timer_set_interval(mShortTimer, &ForkCallContext::sOnShortTimer, this, (su_duration_t)mCfg->mUrgentTimeout*1000);
		}
	}

	// Save
	if (best) {
		mBestResponse = make_shared<ResponseSipEvent>(event); // Copy event
		mBestResponse->suspendProcessing();
	}

	// Don't forward
	event->setIncomingAgent(shared_ptr<IncomingAgent>());
}

void ForkCallContext::onResponse(const shared_ptr<OutgoingTransaction> &transaction, shared_ptr<ResponseSipEvent> &event) {
	event->setIncomingAgent(mIncoming);
	const shared_ptr<MsgSip> &ms = event->getMsgSip();
	sip_t *sip = ms->getSip();

	if (sip != NULL && sip->sip_status != NULL) {
		int code=sip->sip_status->st_status;
		LOGD("Fork: outgoingCallback %d", code);
		if (code > 100 && code < 200) {
			forward(event);
		} else if (code >= 200 && code < 300) {
			if (mCfg->mForkOneResponse) // TODO: respect RFC 3261 16.7.5
				cancelOthers(transaction);
			forward(event, true);
		} else if (code >= 600 && code < 700) {
			decline(transaction, event);
		} else if (code!=503 && code!=408){ //ignore  503 and 408
			if (mOutgoings.size()<2){
				//optimization: when there is a single branch in the fork, send all the response immediately.
				forward(event,true);
			}else if (!mCancelled){
				store(event);
			}else{
				forward(event,true);
			}
		} else {// Don't forward
			event->setIncomingAgent(shared_ptr<IncomingAgent>());
			SLOGW << "ForkCallContext::onResponse " << this << " Outgoing transaction: ignore message " << code;
		}
	}
}

void ForkCallContext::sendRinging(){
	// Create response
	if (mIncoming && mLastResponseCodeSent<180){
		shared_ptr<MsgSip> msgsip(mIncoming->createResponse(SIP_180_RINGING));
		shared_ptr<ResponseSipEvent> ev(new ResponseSipEvent(dynamic_pointer_cast<OutgoingAgent>(mAgent->shared_from_this()), msgsip));
		//add a to tag, no set by sofia here.
		if (!mCfg->mRemoveToTag) {
			const char *totag=nta_agent_newtag(msgsip->getHome(),"%s",mAgent->getSofiaAgent());
			sip_to_tag(msgsip->getHome(), msgsip->getSip()->sip_to, totag);
		}
		if (mPushTimer) su_timer_destroy(mPushTimer), mPushTimer=NULL;
		if (mCfg->mPushResponseTimeout > 0) {
			mPushTimer=su_timer_create(su_root_task(mAgent->getRoot()), 0);
			su_timer_set_interval(mPushTimer, &ForkCallContext::sOnPushTimer, this, (su_duration_t)mCfg->mPushResponseTimeout*1000);
		}
		ev->setIncomingAgent(mIncoming);
		sendResponse(ev,false);
	}
}

void ForkCallContext::onNew(const shared_ptr<IncomingTransaction> &transaction) {
	ForkContext::onNew(transaction);
}

void ForkCallContext::onDestroy(const shared_ptr<IncomingTransaction> &transaction) {
	return ForkContext::onDestroy(transaction);
}

void ForkCallContext::onNew(const shared_ptr<OutgoingTransaction> &transaction) {
	ForkContext::onNew(transaction);
}

void ForkCallContext::logResponse(const shared_ptr<ResponseSipEvent> &ev){
	sip_t *sip=ev->getMsgSip()->getSip();
	mLog->setStatusCode(sip->sip_status->st_status,sip->sip_status->st_phrase);
	if (sip->sip_status->st_status>=200)
		mLog->setCompleted();
	ev->setEventLog(mLog);
}

void ForkCallContext::sendResponse(shared_ptr<ResponseSipEvent> ev, bool inject){
	logResponse(ev);
	if (inject)
		mAgent->injectResponseEvent(ev);
	else
		mAgent->sendResponseEvent(ev);
	mLastResponseCodeSent=ev->getMsgSip()->getSip()->sip_status->st_status;
}

void ForkCallContext::checkFinished(){
	if (mOutgoings.size() == 0){
		if (!isCompleted() && mBestResponse){
			/* no more outgoing transactions, but one of them replied with an explicit answer (not 503 or 408).
			 * In this case, forward this response now.
			**/
			sendResponse(mBestResponse,true);
			mBestResponse.reset();
			setFinished();
			return;
		}
		if ((mLateTimerExpired || mLateTimer==NULL)) {
			if (mIncoming != NULL && !isCompleted()) {
				if (mBestResponse == NULL) {
					// Create response
					shared_ptr<MsgSip> msgsip(mIncoming->createResponse(SIP_408_REQUEST_TIMEOUT));
					shared_ptr<ResponseSipEvent> ev(new ResponseSipEvent(dynamic_pointer_cast<OutgoingAgent>(mAgent->shared_from_this()), msgsip));
					ev->setIncomingAgent(mIncoming);
					sendResponse(ev,false);
				} else {
					sendResponse(mBestResponse,true);
				}
			}
			mBestResponse.reset();
			setFinished();
		}
	}
}

void ForkCallContext::onDestroy(const shared_ptr<OutgoingTransaction> &transaction) {
	ForkContext::onDestroy(transaction);
}

bool ForkCallContext::onNewRegister(const sip_contact_t *ctt){
	if (isCompleted()) return false;
	return ForkContext::onNewRegister(ctt);
}

bool ForkCallContext::isCompleted()const{
	return mLastResponseCodeSent>=200 || mCancelled || mIncoming==NULL;
}

void ForkCallContext::onShortTimer(){
	if (mCancelled || mLastResponseCodeSent>=180) return; /*it's ringing somewhere*/
	if (isRetryableOrUrgent(mBestResponse->getMsgSip()->getSip()->sip_status->st_status)){
		cancelOthers(static_pointer_cast<OutgoingTransaction>(mBestResponse->getOutgoingAgent()));
		sendResponse(mBestResponse,true);// send urgent reply immediately
		mBestResponse.reset();
	}
	su_timer_destroy(mShortTimer);
	mShortTimer=NULL;
}

void ForkCallContext::sOnShortTimer(su_root_magic_t *magic, su_timer_t *t, su_timer_arg_t *arg){
	ForkCallContext *zis=static_cast<ForkCallContext*>(arg);
	zis->onShortTimer();
}

void ForkCallContext::onPushTimer(){
	if (!isCompleted() && !mBestResponse && !contains(mForwardResponses, 180) && !contains(mForwardResponses, 183)) {
		SLOGD << "ForkCallContext " << this << " push timer : no uac response";
	}
	su_timer_destroy(mPushTimer);
	mPushTimer=NULL;
}

void ForkCallContext::sOnPushTimer(su_root_magic_t *magic, su_timer_t *t, su_timer_arg_t *arg){
	ForkCallContext *zis=static_cast<ForkCallContext*>(arg);
	zis->onPushTimer();
}
void ForkCallContext::onPushInitiated(const string &key) {
	++mActivePushes;
}

void ForkCallContext::onPushError(const string &key, const string &errormsg) {
	--mActivePushes;
	if (mActivePushes != 0) return;
	SLOGD << "Early fail due to all push requests having failed";
	onPushTimer();
}

<commit_msg>add more urgent status codes<commit_after>/*
 Flexisip, a flexible SIP proxy server with media capabilities.
 Copyright (C) 2012  Belledonne Communications SARL.

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU Affero General Public License as
 published by the Free Software Foundation, either version 3 of the
 License, or (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Affero General Public License for more details.

 You should have received a copy of the GNU Affero General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "forkcallcontext.hh"
#include "common.hh"
#include <algorithm>
#include <sofia-sip/sip_status.h>

using namespace ::std;

template<typename T>
static bool contains(const list<T> &l, T value) {
	return find(l.cbegin(), l.cend(), value) != l.cend();
}

ForkCallContext::ForkCallContext(Agent *agent, const std::shared_ptr<RequestSipEvent> &event, shared_ptr<ForkContextConfig> cfg, ForkContextListener* listener) :
		ForkContext(agent, event,cfg, listener), mShortTimer(NULL), mPushTimer(NULL), mLastResponseCodeSent(100), mCancelled(false) {
	LOGD("New ForkCallContext %p", this);
	mLog=event->getEventLog<CallLog>();
	mActivePushes = 0;
}

ForkCallContext::~ForkCallContext() {
	LOGD("Destroy ForkCallContext %p", this);
	if (mShortTimer){
		su_timer_destroy(mShortTimer);
		mShortTimer=NULL;
	}
	if (mPushTimer){
		su_timer_destroy(mPushTimer);
		mPushTimer=NULL;
	}
}

void ForkCallContext::cancel() {
	mLog->setCancelled();
	mLog->setCompleted();
	mCancelled=true;
	cancelOthers();
}

void ForkCallContext::forward(const shared_ptr<ResponseSipEvent> &ev, bool force) {
	sip_t *sip = ev->getMsgSip()->getSip();
	bool fakeSipEvent = ((mLastResponseCodeSent >= 200) && !force) || mIncoming == NULL;
	const int status = sip->sip_status->st_status;

	if (mCfg->mForkOneResponse) { // TODO: respect RFC 3261 16.7.5
		if (status == 183 || status == 180 || status == 101) {
			auto it = find(mForwardResponses.begin(), mForwardResponses.end(), status);
			if (it != mForwardResponses.end()) {
				fakeSipEvent = true;
			} else {
				mForwardResponses.push_back(status);
			}
		}
	}

	if (fakeSipEvent) {
		ev->setIncomingAgent(shared_ptr<IncomingAgent>());
	}else{
		if (mCfg->mRemoveToTag && (status == 183 || status == 180 || status == 101)) {
			SLOGD << "Removing 'to tag' ";
			msg_header_remove_param((msg_common_t *)sip->sip_to, "tag");
		}
		logResponse(ev);
	}
	mLastResponseCodeSent=status;
}

void ForkCallContext::decline(const shared_ptr<OutgoingTransaction> &transaction, shared_ptr<ResponseSipEvent> &ev) {
	if (!mCfg->mForkNoGlobalDecline) {
		cancelOthers(transaction);
		forward(ev);
	} else {
		if (mOutgoings.size() != 1) {
			store(ev);
		} else {
			forward(ev);
		}
	}
}

void ForkCallContext::cancelOthers(const shared_ptr<OutgoingTransaction> &transaction) {
	for (list<shared_ptr<OutgoingTransaction>>::iterator it = mOutgoings.begin(); it != mOutgoings.end();) {
		if (*it != transaction) {
			shared_ptr<OutgoingTransaction> tr = (*it);
			it = mOutgoings.erase(it);
			tr->cancel();
		} else {
			++it;
		}
	}
}

void ForkCallContext::onRequest(const shared_ptr<IncomingTransaction> &transaction, shared_ptr<RequestSipEvent> &event) {
	event->setOutgoingAgent(shared_ptr<OutgoingAgent>());
	const shared_ptr<MsgSip> &ms = event->getMsgSip();
	sip_t *sip = ms->getSip();
	if (sip != NULL && sip->sip_request != NULL) {
		if (sip->sip_request->rq_method == sip_method_cancel) {
			LOGD("Fork: incomingCallback cancel");
			cancel();
			/*
			 * let the event go through the list of modules for notification purpose, but do not send the cancel at the end since it is handled here.
			 * Indeed there might not be generated cancels for non-responded branches of the fork, letting other modules unnotified.
			**/
			event->setOutgoingAgent(shared_ptr<OutgoingAgent>());
		}
	}
}

bool ForkCallContext::isRetryableOrUrgent(int code){
	switch(code){
		case 401:
		case 407:
		case 415:
		case 420:
		case 484:
		case 488:
		case 606:
			return true;
		case 603:
			if (mCfg->mTreatDeclineAsUrgent)
				return true;
		default:
			return false;
	}
	return false;
}

void ForkCallContext::store(shared_ptr<ResponseSipEvent> &event) {
	bool best = false;
	int code=event->getMsgSip()->getSip()->sip_status->st_status;

	if (mBestResponse != NULL) {
		//we must give priority to 401, 407, 415, 420, 484 because they will trigger a request retry.
		int prev_resp_code=mBestResponse->getMsgSip()->getSip()->sip_status->st_status;
		int code_class=code/100;
		int prev_code_class=prev_resp_code/100;

		if (code_class < prev_code_class) {
			best = true;
		}
	}else best=true;

	if (best && isRetryableOrUrgent(code)){
		if (mShortTimer==NULL){
			mShortTimer=su_timer_create(su_root_task(mAgent->getRoot()), 0);
			su_timer_set_interval(mShortTimer, &ForkCallContext::sOnShortTimer, this, (su_duration_t)mCfg->mUrgentTimeout*1000);
		}
	}

	// Save
	if (best) {
		mBestResponse = make_shared<ResponseSipEvent>(event); // Copy event
		mBestResponse->suspendProcessing();
	}

	// Don't forward
	event->setIncomingAgent(shared_ptr<IncomingAgent>());
}

void ForkCallContext::onResponse(const shared_ptr<OutgoingTransaction> &transaction, shared_ptr<ResponseSipEvent> &event) {
	event->setIncomingAgent(mIncoming);
	const shared_ptr<MsgSip> &ms = event->getMsgSip();
	sip_t *sip = ms->getSip();

	if (sip != NULL && sip->sip_status != NULL) {
		int code=sip->sip_status->st_status;
		LOGD("Fork: outgoingCallback %d", code);
		if (code > 100 && code < 200) {
			forward(event);
		} else if (code >= 200 && code < 300) {
			if (mCfg->mForkOneResponse) // TODO: respect RFC 3261 16.7.5
				cancelOthers(transaction);
			forward(event, true);
		} else if (code >= 600 && code < 700) {
			decline(transaction, event);
		} else if (code!=503 && code!=408){ //ignore  503 and 408
			if (mOutgoings.size()<2){
				//optimization: when there is a single branch in the fork, send all the response immediately.
				forward(event,true);
			}else if (!mCancelled){
				store(event);
			}else{
				forward(event,true);
			}
		} else {// Don't forward
			event->setIncomingAgent(shared_ptr<IncomingAgent>());
			SLOGW << "ForkCallContext::onResponse " << this << " Outgoing transaction: ignore message " << code;
		}
	}
}

void ForkCallContext::sendRinging(){
	// Create response
	if (mIncoming && mLastResponseCodeSent<180){
		shared_ptr<MsgSip> msgsip(mIncoming->createResponse(SIP_180_RINGING));
		shared_ptr<ResponseSipEvent> ev(new ResponseSipEvent(dynamic_pointer_cast<OutgoingAgent>(mAgent->shared_from_this()), msgsip));
		//add a to tag, no set by sofia here.
		if (!mCfg->mRemoveToTag) {
			const char *totag=nta_agent_newtag(msgsip->getHome(),"%s",mAgent->getSofiaAgent());
			sip_to_tag(msgsip->getHome(), msgsip->getSip()->sip_to, totag);
		}
		if (mPushTimer) su_timer_destroy(mPushTimer), mPushTimer=NULL;
		if (mCfg->mPushResponseTimeout > 0) {
			mPushTimer=su_timer_create(su_root_task(mAgent->getRoot()), 0);
			su_timer_set_interval(mPushTimer, &ForkCallContext::sOnPushTimer, this, (su_duration_t)mCfg->mPushResponseTimeout*1000);
		}
		ev->setIncomingAgent(mIncoming);
		sendResponse(ev,false);
	}
}

void ForkCallContext::onNew(const shared_ptr<IncomingTransaction> &transaction) {
	ForkContext::onNew(transaction);
}

void ForkCallContext::onDestroy(const shared_ptr<IncomingTransaction> &transaction) {
	return ForkContext::onDestroy(transaction);
}

void ForkCallContext::onNew(const shared_ptr<OutgoingTransaction> &transaction) {
	ForkContext::onNew(transaction);
}

void ForkCallContext::logResponse(const shared_ptr<ResponseSipEvent> &ev){
	sip_t *sip=ev->getMsgSip()->getSip();
	mLog->setStatusCode(sip->sip_status->st_status,sip->sip_status->st_phrase);
	if (sip->sip_status->st_status>=200)
		mLog->setCompleted();
	ev->setEventLog(mLog);
}

void ForkCallContext::sendResponse(shared_ptr<ResponseSipEvent> ev, bool inject){
	logResponse(ev);
	if (inject)
		mAgent->injectResponseEvent(ev);
	else
		mAgent->sendResponseEvent(ev);
	mLastResponseCodeSent=ev->getMsgSip()->getSip()->sip_status->st_status;
}

void ForkCallContext::checkFinished(){
	if (mOutgoings.size() == 0){
		if (!isCompleted() && mBestResponse){
			/* no more outgoing transactions, but one of them replied with an explicit answer (not 503 or 408).
			 * In this case, forward this response now.
			**/
			sendResponse(mBestResponse,true);
			mBestResponse.reset();
			setFinished();
			return;
		}
		if ((mLateTimerExpired || mLateTimer==NULL)) {
			if (mIncoming != NULL && !isCompleted()) {
				if (mBestResponse == NULL) {
					// Create response
					shared_ptr<MsgSip> msgsip(mIncoming->createResponse(SIP_408_REQUEST_TIMEOUT));
					shared_ptr<ResponseSipEvent> ev(new ResponseSipEvent(dynamic_pointer_cast<OutgoingAgent>(mAgent->shared_from_this()), msgsip));
					ev->setIncomingAgent(mIncoming);
					sendResponse(ev,false);
				} else {
					sendResponse(mBestResponse,true);
				}
			}
			mBestResponse.reset();
			setFinished();
		}
	}
}

void ForkCallContext::onDestroy(const shared_ptr<OutgoingTransaction> &transaction) {
	ForkContext::onDestroy(transaction);
}

bool ForkCallContext::onNewRegister(const sip_contact_t *ctt){
	if (isCompleted()) return false;
	return ForkContext::onNewRegister(ctt);
}

bool ForkCallContext::isCompleted()const{
	return mLastResponseCodeSent>=200 || mCancelled || mIncoming==NULL;
}

void ForkCallContext::onShortTimer(){
	if (mCancelled || mLastResponseCodeSent>=180) return; /*it's ringing somewhere*/
	if (isRetryableOrUrgent(mBestResponse->getMsgSip()->getSip()->sip_status->st_status)){
		cancelOthers(static_pointer_cast<OutgoingTransaction>(mBestResponse->getOutgoingAgent()));
		sendResponse(mBestResponse,true);// send urgent reply immediately
		mBestResponse.reset();
	}
	su_timer_destroy(mShortTimer);
	mShortTimer=NULL;
}

void ForkCallContext::sOnShortTimer(su_root_magic_t *magic, su_timer_t *t, su_timer_arg_t *arg){
	ForkCallContext *zis=static_cast<ForkCallContext*>(arg);
	zis->onShortTimer();
}

void ForkCallContext::onPushTimer(){
	if (!isCompleted() && !mBestResponse && !contains(mForwardResponses, 180) && !contains(mForwardResponses, 183)) {
		SLOGD << "ForkCallContext " << this << " push timer : no uac response";
	}
	su_timer_destroy(mPushTimer);
	mPushTimer=NULL;
}

void ForkCallContext::sOnPushTimer(su_root_magic_t *magic, su_timer_t *t, su_timer_arg_t *arg){
	ForkCallContext *zis=static_cast<ForkCallContext*>(arg);
	zis->onPushTimer();
}
void ForkCallContext::onPushInitiated(const string &key) {
	++mActivePushes;
}

void ForkCallContext::onPushError(const string &key, const string &errormsg) {
	--mActivePushes;
	if (mActivePushes != 0) return;
	SLOGD << "Early fail due to all push requests having failed";
	onPushTimer();
}

<|endoftext|>


<commit_before>/*
 * Perform All-Reduce in the hypercube for the workers.
 * Author: Robert Hangu
 */
//#include "controller.hpp"
#include "mock-network.hpp"
#include <iostream>
#include <thread>
#include <vector>
#include <sstream>

std::vector<std::thread> threads;

// Number of workers in the network
int worker_count;

void worker(c7a::MockNetwork& net, size_t id, int local_value) {
    c7a::MockSelect client(net, id);

    // For each dimension of the hypercube, exchange data between workers with
    // different bits at position d
    for (int d = 1; d <= worker_count; d <<= 1) {
        // Convert the local int to string
        std::ostringstream local_value_string;
        local_value_string << local_value;
        // Send local_value to worker with id id ^ d
        client.sendToWorkerString(id ^ d, local_value_string.str());

        // Receive local_value from worker with id id ^ d
        size_t from_whom;
        std::string recv_data;
        client.receiveFromAnyString(&from_whom, &recv_data);
        std::cout << "Worker " << id << ": Received " << recv_data
                  << " from worker " << from_whom << "\n";
    }
}

int main() {
    c7a::MockNetwork net;

    for (int i = 0; i < 8; ++i)
        threads.push_back(std::thread(worker, std::ref(net), i, 2 * (i + 1)));

    for (int i = 0; i < threads.size(); ++i)
        threads[i].join();

    return 0;
}
<commit_msg>Test for hypercube algorithm belongs into tests.<commit_after>/*
 * Perform All-Reduce in the hypercube for the workers.
 * Author: Robert Hangu
 */
//#include "controller.hpp"
#include "mock-network.hpp"
#include <iostream>
#include <thread>
#include <vector>
#include <sstream>

#if THIS_BELONGS_INTO_TESTS

std::vector<std::thread> threads;

// Number of workers in the network
int worker_count;

void worker(c7a::MockNetwork& net, size_t id, int local_value) {
    c7a::MockSelect client(net, id);

    // For each dimension of the hypercube, exchange data between workers with
    // different bits at position d
    for (int d = 1; d <= worker_count; d <<= 1) {
        // Convert the local int to string
        std::ostringstream local_value_string;
        local_value_string << local_value;
        // Send local_value to worker with id id ^ d
        client.sendToWorkerString(id ^ d, local_value_string.str());

        // Receive local_value from worker with id id ^ d
        size_t from_whom;
        std::string recv_data;
        client.receiveFromAnyString(&from_whom, &recv_data);
        std::cout << "Worker " << id << ": Received " << recv_data
                  << " from worker " << from_whom << "\n";
    }
}

int main() {
    c7a::MockNetwork net;

    for (int i = 0; i < 8; ++i)
        threads.push_back(std::thread(worker, std::ref(net), i, 2 * (i + 1)));

    for (int i = 0; i < threads.size(); ++i)
        threads[i].join();

    return 0;
}

#endif // THIS_BELONGS_INTO_TESTS
<|endoftext|>


<commit_before>/* arduino_lorawan.cpp	Tue Oct 25 2016 03:59:08 tmm */

/*

Module:  arduino_lorawan.cpp

Function:
	Constructor: Arduino_LoRaWAN::Arduino_LoRaWAN()

Version:
	V0.1.0	Tue Oct 25 2016 03:59:08 tmm	Edit level 1

Copyright notice:
	This file copyright (C) 2016 by

		MCCI Corporation
		3520 Krums Corners Road
		Ithaca, NY  14850

	An unpublished work.  All rights reserved.

	This file is proprietary information, and may not be disclosed or
	copied without the prior permission of MCCI Corporation.

Author:
	Terry Moore, MCCI Corporation	October 2016

Revision history:
   0.1.0  Tue Oct 25 2016 03:59:08  tmm
	Module created.

*/

#include <Arduino_LoRaWAN.h>

/****************************************************************************\
|
|		Manifest constants & typedefs.
|
|	This is strictly for private types and constants which will not
|	be exported.
|
\****************************************************************************/

/****************************************************************************\
|
|	Read-only data.
|
|	If program is to be ROM-able, these must all be tagged read-only
|	using the ROM storage class; they may be global.
|
\****************************************************************************/

/****************************************************************************\
|
|	VARIABLES:
|
|	If program is to be ROM-able, these must be initialized
|	using the BSS keyword.  (This allows for compilers that require
|	every variable to have an initializer.)  Note that only those
|	variables owned by this module should be declared here, using the BSS
|	keyword; this allows for linkers that dislike multiple declarations
|	of objects.
|
\****************************************************************************/

Arduino_LoRaWAN::Arduino_LoRaWAN()
        {
        memset(&this->m_lmic_pins, lmic_pinmap::LMIC_UNUSED_PIN, sizeof(this->m_lmic_pins));
        this->m_lmic_pins.rxtx_rx_active = 0;
        this->m_lmic_pins.spi_freq = 0;	/* use default */
        }
<commit_msg>Add comments to arduino_lorawan.cpp<commit_after>/* arduino_lorawan.cpp	Tue Oct 25 2016 03:59:08 tmm */

/*

Module:  arduino_lorawan.cpp

Function:
	Constructor: Arduino_LoRaWAN::Arduino_LoRaWAN()

Version:
	V0.1.0	Tue Oct 25 2016 03:59:08 tmm	Edit level 1

Copyright notice:
	This file copyright (C) 2016 by

		MCCI Corporation
		3520 Krums Corners Road
		Ithaca, NY  14850

	An unpublished work.  All rights reserved.

	This file is proprietary information, and may not be disclosed or
	copied without the prior permission of MCCI Corporation.

Author:
	Terry Moore, MCCI Corporation	October 2016

Revision history:
   0.1.0  Tue Oct 25 2016 03:59:08  tmm
	Module created.

*/

#include <Arduino_LoRaWAN.h>

/****************************************************************************\
|
|		Manifest constants & typedefs.
|
|	This is strictly for private types and constants which will not
|	be exported.
|
\****************************************************************************/

/****************************************************************************\
|
|	Read-only data.
|
|	If program is to be ROM-able, these must all be tagged read-only
|	using the ROM storage class; they may be global.
|
\****************************************************************************/

/****************************************************************************\
|
|	VARIABLES:
|
|	If program is to be ROM-able, these must be initialized
|	using the BSS keyword.  (This allows for compilers that require
|	every variable to have an initializer.)  Note that only those
|	variables owned by this module should be declared here, using the BSS
|	keyword; this allows for linkers that dislike multiple declarations
|	of objects.
|
\****************************************************************************/

Arduino_LoRaWAN::Arduino_LoRaWAN()
        {
	// initialize the pin structure, which mostly needs to be
	// lmic_pinmap::LMIC_UNUSED_PIN
        memset(&this->m_lmic_pins, lmic_pinmap::LMIC_UNUSED_PIN, sizeof(this->m_lmic_pins));

	// However, a few fields need to be different.
	// By default, we want the RX/TX pin to be high for TX.
        this->m_lmic_pins.rxtx_rx_active = 0;

	// By default, use the SPI frequency that comes from the LMIC
	// configuration file.
        this->m_lmic_pins.spi_freq = 0;	/* use default */
        }
<|endoftext|>


<commit_before>#ifndef TELEGRAMSERVERUSER_HPP
#define TELEGRAMSERVERUSER_HPP

#include <QObject>
#include <QVector>

#include "ServerNamespace.hpp"
#include "TLTypes.hpp"

namespace Telegram {

namespace Server {

class Session;
class User;

class MessageRecipient
{
public:
    virtual ~MessageRecipient() = default;
    virtual quint32 addMessage(const TLMessage &message, Session *excludeSession = nullptr) = 0;

    virtual Peer toPeer() const = 0;
    TLPeer toTLPeer() const;
};

class RemoteUser : public MessageRecipient
{
public:
    virtual quint32 id() const = 0;
    virtual QString phoneNumber() const = 0;
    virtual QString firstName() const = 0;
    virtual QString lastName() const = 0;
    virtual bool isOnline() const = 0;
    virtual quint32 dcId() const = 0;
    virtual QVector<quint32> contactList() const = 0;

    Peer toPeer() const override { return Peer::fromUserId(id()); }
    UserContact toContact() const;
};

class User : public QObject, public RemoteUser
{
    Q_OBJECT
public:
    explicit User(QObject *parent = nullptr);

    quint32 id() const { return m_id; }
    QString phoneNumber() const { return m_phoneNumber; }
    void setPhoneNumber(const QString &phoneNumber);

    QString firstName() const { return m_firstName; }
    void setFirstName(const QString &firstName);

    QString lastName() const { return m_lastName; }
    void setLastName(const QString &lastName);

    bool isOnline() const;

    quint32 dcId() const { return m_dcId; }
    void setDcId(quint32 id);

    Session *getSession(quint64 authId) const;
    QVector<Session*> sessions() const { return m_sessions; }
    QVector<Session*> activeSessions() const;
    bool hasActiveSession() const;
    void addSession(Session *session);

    bool hasPassword() const { return !m_passwordSalt.isEmpty() && !m_passwordHash.isEmpty(); }
    QByteArray passwordSalt() const { return m_passwordSalt; }
    QByteArray passwordHash() const { return m_passwordHash; }

    void setPlainPassword(const QString &password);
    void setPassword(const QByteArray &salt, const QByteArray &hash);

    QString passwordHint() const { return QString(); }

    quint32 addMessage(const TLMessage &message, Session *excludeSession = nullptr) override;
    const TLMessage *getMessage(quint32 messageId) const;

    TLVector<TLMessage> getHistory(const Telegram::Peer &peer, quint32 offsetId, quint32 offsetDate, quint32 addOffset, quint32 limit, quint32 maxId, quint32 minId, quint32 hash) const;

    quint32 pts() const { return m_pts; }
    quint32 addPts();

    void importContact(const UserContact &contact);
    QVector<quint32> contactList() const override { return m_contactList; }
    const QVector<UserDialog *> dialogs() const { return m_dialogs; }

    QVector<UserContact> importedContacts() const { return m_importedContacts; }

signals:
    void sessionAdded(Session *newSession);
    void sessionDestroyed(Session *destroyedSession);

protected:
    UserDialog *ensureDialog(const Telegram::Peer &peer);

    quint32 m_id = 0;
    QString m_phoneNumber;
    QString m_firstName;
    QString m_lastName;
    QString m_userName;
    QByteArray m_passwordSalt;
    QByteArray m_passwordHash;
    QVector<Session*> m_sessions;
    quint32 m_dcId = 0;

    quint32 m_pts = 0;
    QVector<TLMessage> m_messages;
    QVector<UserDialog *> m_dialogs;
    QVector<quint32> m_contactList; // Contains only registered users from the added contacts
    QVector<UserContact> m_importedContacts; // Contains phone + name of all added contacts (including not registered yet)
};

} // Server

} // Telegram

#endif // TELEGRAMSERVERUSER_HPP
<commit_msg>Server: Add User::userName() getter<commit_after>#ifndef TELEGRAMSERVERUSER_HPP
#define TELEGRAMSERVERUSER_HPP

#include <QObject>
#include <QVector>

#include "ServerNamespace.hpp"
#include "TLTypes.hpp"

namespace Telegram {

namespace Server {

class Session;
class User;

class MessageRecipient
{
public:
    virtual ~MessageRecipient() = default;
    virtual quint32 addMessage(const TLMessage &message, Session *excludeSession = nullptr) = 0;

    virtual Peer toPeer() const = 0;
    TLPeer toTLPeer() const;
};

class RemoteUser : public MessageRecipient
{
public:
    virtual quint32 id() const = 0;
    virtual QString phoneNumber() const = 0;
    virtual QString firstName() const = 0;
    virtual QString lastName() const = 0;
    virtual bool isOnline() const = 0;
    virtual quint32 dcId() const = 0;
    virtual QVector<quint32> contactList() const = 0;

    Peer toPeer() const override { return Peer::fromUserId(id()); }
    UserContact toContact() const;
};

class User : public QObject, public RemoteUser
{
    Q_OBJECT
public:
    explicit User(QObject *parent = nullptr);

    quint32 id() const { return m_id; }
    QString phoneNumber() const { return m_phoneNumber; }
    void setPhoneNumber(const QString &phoneNumber);

    QString userName() const { return m_userName; }

    QString firstName() const { return m_firstName; }
    void setFirstName(const QString &firstName);

    QString lastName() const { return m_lastName; }
    void setLastName(const QString &lastName);

    bool isOnline() const;

    quint32 dcId() const { return m_dcId; }
    void setDcId(quint32 id);

    Session *getSession(quint64 authId) const;
    QVector<Session*> sessions() const { return m_sessions; }
    QVector<Session*> activeSessions() const;
    bool hasActiveSession() const;
    void addSession(Session *session);

    bool hasPassword() const { return !m_passwordSalt.isEmpty() && !m_passwordHash.isEmpty(); }
    QByteArray passwordSalt() const { return m_passwordSalt; }
    QByteArray passwordHash() const { return m_passwordHash; }

    void setPlainPassword(const QString &password);
    void setPassword(const QByteArray &salt, const QByteArray &hash);

    QString passwordHint() const { return QString(); }

    quint32 addMessage(const TLMessage &message, Session *excludeSession = nullptr) override;
    const TLMessage *getMessage(quint32 messageId) const;

    TLVector<TLMessage> getHistory(const Telegram::Peer &peer, quint32 offsetId, quint32 offsetDate, quint32 addOffset, quint32 limit, quint32 maxId, quint32 minId, quint32 hash) const;

    quint32 pts() const { return m_pts; }
    quint32 addPts();

    void importContact(const UserContact &contact);
    QVector<quint32> contactList() const override { return m_contactList; }
    const QVector<UserDialog *> dialogs() const { return m_dialogs; }

    QVector<UserContact> importedContacts() const { return m_importedContacts; }

signals:
    void sessionAdded(Session *newSession);
    void sessionDestroyed(Session *destroyedSession);

protected:
    UserDialog *ensureDialog(const Telegram::Peer &peer);

    quint32 m_id = 0;
    QString m_phoneNumber;
    QString m_firstName;
    QString m_lastName;
    QString m_userName;
    QByteArray m_passwordSalt;
    QByteArray m_passwordHash;
    QVector<Session*> m_sessions;
    quint32 m_dcId = 0;

    quint32 m_pts = 0;
    QVector<TLMessage> m_messages;
    QVector<UserDialog *> m_dialogs;
    QVector<quint32> m_contactList; // Contains only registered users from the added contacts
    QVector<UserContact> m_importedContacts; // Contains phone + name of all added contacts (including not registered yet)
};

} // Server

} // Telegram

#endif // TELEGRAMSERVERUSER_HPP
<|endoftext|>


<commit_before>//
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio and is mainly inspired
// by software hpp-model-urdf
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_collada_to_fcl_hpp__
#define __se3_collada_to_fcl_hpp__

#include <limits>

#include <boost/filesystem/fstream.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>

#include <assimp/DefaultLogger.hpp>
#include <assimp/IOStream.hpp>
#include <assimp/IOSystem.hpp>
#include <assimp/scene.h>
#include <assimp/Importer.hpp>
#include <assimp/postprocess.h>

#include <hpp/fcl/BV/OBBRSS.h>
#include <hpp/fcl/BVH/BVH_model.h>

typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;

struct TriangleAndVertices
{
  void clear()
  {
    vertices_.clear ();
    triangles_.clear ();
  }
  std::vector <fcl::Vec3f> vertices_;
  std::vector <fcl::Triangle> triangles_;
};

/**
 * @brief      Recursive procedure for building a mesh
 *
 * @param[in]  scale           Scale to apply when reading the ressource
 * @param[in]  scene           Pointer to the assimp scene
 * @param[in]  node            Current node of the scene
 * @param      subMeshIndexes  Submesh triangles indexes interval
 * @param[in]  mesh            The mesh that must be built
 * @param      tv              Triangles and Vertices of the mesh submodels
 */
inline void buildMesh (const ::urdf::Vector3 & scale,
                       const aiScene* scene,
                       const aiNode* node,
                       std::vector<unsigned> & subMeshIndexes,
                       const PolyhedronPtrType & mesh,
                       TriangleAndVertices & tv)
{
  if (!node) return;

  aiMatrix4x4 transform = node->mTransformation;
  aiNode *pnode = node->mParent;
  while (pnode)
  {
    // Don't convert to y-up orientation, which is what the root node in
    // Assimp does
    if (pnode->mParent != NULL)
    {
      transform = pnode->mTransformation * transform;
    }
    pnode = pnode->mParent;
  }

  for (uint32_t i = 0; i < node->mNumMeshes; i++)
  {
    aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];

    unsigned oldNbPoints = mesh->num_vertices;
    unsigned oldNbTriangles = mesh->num_tris;

    // Add the vertices
    for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
    {
      aiVector3D p = input_mesh->mVertices[j];
      p *= transform;
      tv.vertices_.push_back (fcl::Vec3f (p.x * scale.x,
                                       p.y * scale.y,
                                       p.z * scale.z));
    }

    // add the indices
    for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
    {
      aiFace& face = input_mesh->mFaces[j];
      // FIXME: can add only triangular faces.
      tv.triangles_.push_back (fcl::Triangle( oldNbPoints + face.mIndices[0],
                                           oldNbPoints + face.mIndices[1],
                                           oldNbPoints + face.mIndices[2]));
    }

    // Save submesh triangles indexes interval.
    if (subMeshIndexes.size () == 0)
    {
      subMeshIndexes.push_back (0);
    }

    subMeshIndexes.push_back (oldNbTriangles + input_mesh->mNumFaces);
  }

  for (uint32_t i=0; i < node->mNumChildren; ++i)
  {
    buildMesh(scale, scene, node->mChildren[i], subMeshIndexes, mesh, tv);
  }
}

/**
 * @brief      Convert an assimp scene to a mesh
 *
 * @param[in]  name   File (ressource) transformed into an assimp scene in loa
 * @param[in]  scale  Scale to apply when reading the ressource
 * @param[in]  scene  Pointer to the assimp scene
 * @param[in]  mesh   The mesh that must be built
 */
inline void meshFromAssimpScene (const std::string & name,
                                 const ::urdf::Vector3 & scale,
                                 const aiScene* scene,
                                 const PolyhedronPtrType & mesh)
  {
    TriangleAndVertices tv;

    if (!scene->HasMeshes())
    {
      throw std::runtime_error (std::string ("No meshes found in file ")+
        name);
    }

    std::vector<unsigned> subMeshIndexes;
    int res = mesh->beginModel ();

    if (res != fcl::BVH_OK)
    {
      std::ostringstream error;
      error << "fcl BVHReturnCode = " << res;
      throw std::runtime_error (error.str ());
    }

    tv.clear();

    buildMesh (scale, scene, scene->mRootNode, subMeshIndexes, mesh, tv);
    mesh->addSubModel (tv.vertices_, tv.triangles_);

    mesh->endModel ();
  }

/**
 * @brief      Read a mesh file and convert it to a polyhedral mesh
 *
 * @param[in]  resource_path  Path to the ressource mesh file to be read
 * @param[in]  scale          Scale to apply when reading the ressource
 * @param[in]  polyhedron     The resulted polyhedron
 */
inline void loadPolyhedronFromResource (const std::string & resource_path,
                                        const ::urdf::Vector3 & scale,
                                        const PolyhedronPtrType & polyhedron)
{
  Assimp::Importer importer;
  const aiScene* scene = importer.ReadFile(resource_path.c_str(), aiProcess_SortByPType| aiProcess_GenNormals|
                                                          aiProcess_Triangulate|aiProcess_GenUVCoords|
                                                          aiProcess_FlipUVs);
  if (!scene)
  {
    throw std::runtime_error (std::string ("Could not load resource ") + resource_path + std::string ("\n") +
                              importer.GetErrorString ());
  }

  meshFromAssimpScene (resource_path, scale, scene, polyhedron);
}

/**
 * @brief      Transform a cURL readable path (package://..) to an absolute path for urdf collision path
 *
 * @param[in]      urdf_mesh_path  The path given in the urdf file (package://..)
 * @param[in]  meshRootDir     Root path to the directory where meshes are located
 *
 * @return     The absolute path to the mesh file
 */
inline std::string fromURDFMeshPathToAbsolutePath(const std::string & urdf_mesh_path, const std::string & meshRootDir)
{

  std::string absolutePath = std::string(meshRootDir +
                              urdf_mesh_path.substr(10, urdf_mesh_path.size()));

  return absolutePath;
}

#endif // __se3_collada_to_fcl_hpp__
<commit_msg>[C++][Bug Fixed] Throw exception instead of a runtime error when file is missing<commit_after>//
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio and is mainly inspired
// by software hpp-model-urdf
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_collada_to_fcl_hpp__
#define __se3_collada_to_fcl_hpp__

#include <limits>

#include <boost/filesystem/fstream.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>

#include <assimp/DefaultLogger.hpp>
#include <assimp/IOStream.hpp>
#include <assimp/IOSystem.hpp>
#include <assimp/scene.h>
#include <assimp/Importer.hpp>
#include <assimp/postprocess.h>

#include <hpp/fcl/BV/OBBRSS.h>
#include <hpp/fcl/BVH/BVH_model.h>

#include <exception>

typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;

struct TriangleAndVertices
{
  void clear()
  {
    vertices_.clear ();
    triangles_.clear ();
  }
  std::vector <fcl::Vec3f> vertices_;
  std::vector <fcl::Triangle> triangles_;
};

/**
 * @brief      Recursive procedure for building a mesh
 *
 * @param[in]  scale           Scale to apply when reading the ressource
 * @param[in]  scene           Pointer to the assimp scene
 * @param[in]  node            Current node of the scene
 * @param      subMeshIndexes  Submesh triangles indexes interval
 * @param[in]  mesh            The mesh that must be built
 * @param      tv              Triangles and Vertices of the mesh submodels
 */
inline void buildMesh (const ::urdf::Vector3 & scale,
                       const aiScene* scene,
                       const aiNode* node,
                       std::vector<unsigned> & subMeshIndexes,
                       const PolyhedronPtrType & mesh,
                       TriangleAndVertices & tv)
{
  if (!node) return;

  aiMatrix4x4 transform = node->mTransformation;
  aiNode *pnode = node->mParent;
  while (pnode)
  {
    // Don't convert to y-up orientation, which is what the root node in
    // Assimp does
    if (pnode->mParent != NULL)
    {
      transform = pnode->mTransformation * transform;
    }
    pnode = pnode->mParent;
  }

  for (uint32_t i = 0; i < node->mNumMeshes; i++)
  {
    aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];

    unsigned oldNbPoints = mesh->num_vertices;
    unsigned oldNbTriangles = mesh->num_tris;

    // Add the vertices
    for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
    {
      aiVector3D p = input_mesh->mVertices[j];
      p *= transform;
      tv.vertices_.push_back (fcl::Vec3f (p.x * scale.x,
                                       p.y * scale.y,
                                       p.z * scale.z));
    }

    // add the indices
    for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
    {
      aiFace& face = input_mesh->mFaces[j];
      // FIXME: can add only triangular faces.
      tv.triangles_.push_back (fcl::Triangle( oldNbPoints + face.mIndices[0],
                                           oldNbPoints + face.mIndices[1],
                                           oldNbPoints + face.mIndices[2]));
    }

    // Save submesh triangles indexes interval.
    if (subMeshIndexes.size () == 0)
    {
      subMeshIndexes.push_back (0);
    }

    subMeshIndexes.push_back (oldNbTriangles + input_mesh->mNumFaces);
  }

  for (uint32_t i=0; i < node->mNumChildren; ++i)
  {
    buildMesh(scale, scene, node->mChildren[i], subMeshIndexes, mesh, tv);
  }
}

/**
 * @brief      Convert an assimp scene to a mesh
 *
 * @param[in]  name   File (ressource) transformed into an assimp scene in loa
 * @param[in]  scale  Scale to apply when reading the ressource
 * @param[in]  scene  Pointer to the assimp scene
 * @param[in]  mesh   The mesh that must be built
 */
inline void meshFromAssimpScene (const std::string & name,
                                 const ::urdf::Vector3 & scale,
                                 const aiScene* scene,
                                 const Polyhedron_ptr & mesh) throw (std::invalid_argument)
  {
    TriangleAndVertices tv;

    if (!scene->HasMeshes())
    {
      throw std::invalid_argument (std::string ("No meshes found in file ")+
        name);
    }

    std::vector<unsigned> subMeshIndexes;
    int res = mesh->beginModel ();

    if (res != fcl::BVH_OK)
    {
      std::ostringstream error;
      error << "fcl BVHReturnCode = " << res;
      throw std::runtime_error (error.str ());
    }

    tv.clear();

    buildMesh (scale, scene, scene->mRootNode, subMeshIndexes, mesh, tv);
    mesh->addSubModel (tv.vertices_, tv.triangles_);

    mesh->endModel ();
  }

/**
 * @brief      Read a mesh file and convert it to a polyhedral mesh
 *
 * @param[in]  resource_path  Path to the ressource mesh file to be read
 * @param[in]  scale          Scale to apply when reading the ressource
 * @param[in]  polyhedron     The resulted polyhedron
 */
inline void loadPolyhedronFromResource (const std::string & resource_path,
                                        const ::urdf::Vector3 & scale,
                                        const Polyhedron_ptr & polyhedron) throw (std::invalid_argument)
{
  Assimp::Importer importer;
  const aiScene* scene = importer.ReadFile(resource_path.c_str(), aiProcess_SortByPType| aiProcess_GenNormals|
                                                          aiProcess_Triangulate|aiProcess_GenUVCoords|
                                                          aiProcess_FlipUVs);
  if (!scene)
  {
    const std::string exception_message (std::string ("Could not load resource ") + resource_path + std::string("\n") +
                                         importer.GetErrorString () + std::string("\n") +
                                         "Hint: the mesh directory may be wrong.");
    throw std::invalid_argument(exception_message);
  }

  meshFromAssimpScene (resource_path, scale, scene, polyhedron);
}

/**
 * @brief      Transform a cURL readable path (package://..) to an absolute path for urdf collision path
 *
 * @param[in]      urdf_mesh_path  The path given in the urdf file (package://..)
 * @param[in]  meshRootDir     Root path to the directory where meshes are located
 *
 * @return     The absolute path to the mesh file
 */
inline std::string fromURDFMeshPathToAbsolutePath(const std::string & urdf_mesh_path, const std::string & meshRootDir)
{

  std::string absolutePath = std::string(meshRootDir +
                              urdf_mesh_path.substr(10, urdf_mesh_path.size()));

  return absolutePath;
}

#endif // __se3_collada_to_fcl_hpp__
<|endoftext|>


<commit_before>#include "screen.h"

#include <cstdlib>
#include <cstring>
#include <ctime>
#include <QDebug>
#include "memory.h"

namespace gameboy {
Screen::Screen(Memory *memory) :
    clocks(0),
    memory(memory) {
    framebuffer = new util::Color[160 * 144];

    reset();
}

Screen::~Screen() {
    delete[] framebuffer;
}

void Screen::reset() {
    drawFlag = false;
    line = 0;
    mode = OAM;

    for (int i = 0; i < 160*144; ++i) {
        framebuffer[i].set(0xFF, 0xFF, 0xFF);
    }
}

util::Color* Screen::getFramebuffer() {
    return framebuffer;
}

bool Screen::drawFlagSet() {
    return drawFlag;
}

void Screen::step(unsigned int lastClocks) {
    clocks += lastClocks;
    drawFlag = false;

    switch (mode) {
        case OAM:
            if (clocks >= 20) {
                clocks = 0;
                mode = VRAM;
                memory->write(0xFF41, memory->read(0xFF41) | 0x03); // Indicate VRAM Mode 11
            }
        break;
        case VRAM:
            if (clocks >= 43) {
                clocks = 0;
                mode = HBLANK;
                renderScanLine();
                memory->write(0xFF41, memory->read(0xFF41) & 0xFC); // Indicate H-Blank Mode 00

                if (memory->read(0xFF41) & 0x08) { // Mode 00 Interrupt
                    memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
                }
            }
        break;
        case HBLANK:
            if (clocks >= 51) {
                clocks = 0;
                ++line;
                if (line == 144) {
                    mode = VBLANK;
                    drawFlag = true;
                    memory->write(0xFF41, memory->read(0xFF41) & 0xFD); // Indicate V-Blank Mode 01
                    memory->write(0xFF41, memory->read(0xFF41) | 0x01);

                    if (memory->read(0xFF41) & 0x10) { // Mode 01 Interrupt
                        memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
                    }

                    memory->write(0xFF0F, memory->read(0xFF0F) | 0x01); //Set V-Blank Interrupt Flag
                } else {
                    mode = OAM;
                    memory->write(0xFF41, memory->read(0xFF41) & 0xFE); // Indicate OAM Mode 10
                    memory->write(0xFF41, memory->read(0xFF41) | 0x02);

                    if (memory->read(0xFF41) & 0x20) { // Mode 10 Interrupt
                        memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
                    }
                }
            }
        break;
        case VBLANK:
            if (clocks >= 114) {
                clocks = 0;
                ++line;
                if (line >= 153) {
                    mode = OAM;
                    line = 0;
                    memory->write(0xFF41, memory->read(0xFF41) & 0xFE); // Indicate OAM Mode 10
                    memory->write(0xFF41, memory->read(0xFF41) | 0x02);

                    if (memory->read(0xFF41) & 0x20) { // Mode 10 Interrupt
                        memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
                    }
                }
            }
        break;
    }
    //FF44 - LY - LCDC Y-Coordinate (R)
    memory->write(0xFF44, static_cast<uint8_t>(line));

    // FF41 (STAT)
    uint8_t stat = memory->read(0xFF41);

    uint8_t ly = memory->read(0xFF44);
    uint8_t lyc = memory->read(0xFF45);
    if (lyc == ly) {
        memory->write(0xFF41, stat | 0x04); // Set Coincidence Flag

        if (stat & 0x40) { // LYC=LY Coincidence Interrupt enabled
            memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
        }
    } else {
        memory->write(0xFF41, stat & 0xFB); // Clear Coincidence Flag
    }
}

void Screen::renderScanLine() {
    // FF40 - LCDC - LCD Control
    uint8_t lcdc = memory->read(0xFF40);

    // Bit 7 - LCD Display Enable (0=Off, 1=On)
    if (lcdc & 0x80) {
        // Bit 0 - BG Display (0=Off, 1=On)
        if (lcdc & 0x01) {
            renderBackground();
        }

        // Bit 5 - Window Display Enable (0=Off, 1=On)
        if (lcdc & 0x20) {
            renderWindow();
        }

        // Bit 1 - OBJ (Sprite) Display Enable (0=Off, 1=On)
        if (lcdc & 0x02) {
            renderSprites();
        }
    }
}

void Screen::renderBackground() {
    uint8_t palette = memory->read(0xFF47);

    for (int i = 0; i < 4; ++i) {
        bgPalette[i] = util::Color::PALETTE[(palette >> (i*2)) & 0x3];
    }

    // FF40 - LCDC - LCD Control
    uint8_t lcdc = memory->read(0xFF40);
    // Bit 4 - BG & Window Tile Data Select
    uint16_t tileSet = (lcdc & 0x10) ? 0x8000 : 0x9000;
    // Bit 3 - BG Tile Map Display Select
    uint16_t tileMap = (lcdc & 0x08) ? 0x9C00 : 0x9800;

    // Coordinates of background to be displayed in the left upper corner
    uint8_t scrollX = memory->read(0xFF43);
    uint8_t scrollY = memory->read(0xFF42);

    int num;
    uint16_t tileRow = ((scrollY + line) % 256) / 8;
    uint16_t inTileY = ((scrollY + line) % 256) % 8;
    for (int x = 0; x < 160; ++x) {
        uint16_t tileColumn = ((scrollX + x) % 256) / 8;
        uint16_t inTileX = ((scrollX + x) % 256) % 8;

        uint16_t tileNumLocation = tileMap + tileRow * 32 + tileColumn;

        if (tileSet == 0x8000) {
             num = static_cast<uint8_t>(memory->read(tileNumLocation));
        } else {
            num = static_cast<int8_t>(memory->read(tileNumLocation));
        }

        framebuffer[line*160+x] = bgPalette[readTile(tileSet+num*16, inTileX, inTileY)];
    }
}

void Screen::renderWindow() {
    int16_t wndY = memory->read(0xFF4A);
    int16_t wndX = memory->read(0xFF4B) - 0x7;

    // Check if the window is displayed
    if (wndY < 0
        || wndY >= 144
        || wndX < -7
        || wndX >= 160) {
        return;
    }

    // Check if the window is on the current line
    if (wndY > static_cast<int16_t>(line)) {
        return;
    }

    uint8_t palette = memory->read(0xFF47);

    for (int i = 0; i < 4; ++i) {
        bgPalette[i] = util::Color::PALETTE[(palette >> (i*2)) & 0x3];
    }

    // FF40 - LCDC - LCD Control
    uint8_t lcdc = memory->read(0xFF40);
    // Bit 4 - BG & Window Tile Data Select
    uint16_t tileSet = (lcdc & 0x10) ? 0x8000 : 0x9000;
    // Bit 6 - Window Tile Map Display Select
    uint16_t tileMap = (lcdc & 0x40) ? 0x9C00 : 0x9800;

    int num;
    uint16_t tileRow = (line - wndY) / 8;
    uint16_t inTileY = (line - wndY) % 8;

    for (int x = (wndX < 0) ? 0 : wndX; x < 160; ++x) {
        uint16_t tileColumn = (x - wndX) / 8;
        uint16_t inTileX = (x - wndX) % 8;

        uint16_t tileNumLocation = tileMap + tileRow * 32 + tileColumn;

        if (tileSet == 0x8000) {
             num = static_cast<uint8_t>(memory->read(tileNumLocation));
        } else {
            num = static_cast<int8_t>(memory->read(tileNumLocation));
        }

        framebuffer[line*160+x] = bgPalette[readTile(tileSet+num*16, inTileX, inTileY)];
    }
}

void Screen::renderSprites() {
    // Bit 2 - OBJ (Sprite) Size (0=8x8, 1=8x16)
    bool largeSprites = memory->read(0xFF40) & 0x4;

    for (int h = 39; h >= 0; --h) { // Respect sprite priority
        uint16_t currentOAMAddr = 0xFE00 + 0x4 * h;

        // Top left Corner
        // Byte 0 - Y Position - Vertical position on the screen (minus 16).
        int16_t yCoord = memory->read(currentOAMAddr) - 0x10;
        // Byte 1 - X Position - Horizontal position on the screen (minus 8)
        int16_t xCoord = memory->read(currentOAMAddr+1) - 0x8;

        // Check if the sprite is not on the screen
        if (xCoord >= 160
            || xCoord <= -8
            || yCoord >= 144
            || yCoord <= -(largeSprites ? 16 : 8)) {
            continue;
        }

        uint8_t inTileY = line - yCoord;

        // Check if it is not on the current line
        if (yCoord > static_cast<int16_t>(line) || inTileY >= (largeSprites ? 16 : 8)) {
            continue;
        }

        uint8_t optionsByte = memory->read(currentOAMAddr+3);
        bool priority = !(optionsByte & 0x80);
        bool yFlip = optionsByte & 0x40;
        bool xFlip = optionsByte & 0x20;
        uint8_t palette = memory->read((optionsByte & 0x10) ? 0xFF49 : 0xFF48);

        for (int i = 0; i < 4; ++i) {
            spritePalette[i] = util::Color::PALETTE[(palette >> (i*2)) & 0x3];
        }

        // Byte 2 - Pattern number
        uint8_t spriteNum = memory->read(currentOAMAddr+2);

        // Find tile address
        uint16_t tileAddr;

        if (largeSprites) {
            if (yFlip) {
                inTileY = 15 - inTileY;
            }

            // Check which 8x8 tile has to be used
            if (inTileY < 8) {
                tileAddr = 0x8000 + 0x10 * (spriteNum & 0xFE); // Upper tile, ignore LSB
            } else {
                tileAddr = 0x8000 + 0x10 * (spriteNum | 0x01); // Lower tile, set LSB
                inTileY -= 8;
            }
        } else {
            if (yFlip) {
                inTileY = 7 - inTileY;
            }

            tileAddr = 0x8000 + 0x10 * spriteNum;
        }

        for (int x = 0; x < 8; ++x) {
            if (xCoord + x < 0 || xCoord + x >= 160) {
                continue;
            }

            uint8_t pixel = readTile(tileAddr, xFlip ? (7-x) : x, inTileY); // 0 = transparency

            if (pixel && (priority || (framebuffer[line*160+xCoord+x] == bgPalette[0]))) {
                framebuffer[line*160+xCoord+x] = spritePalette[pixel];
            }
        }
    }
}

uint8_t Screen::readTile(uint16_t address, uint8_t x, uint8_t y) {
    uint8_t lsBits = memory->read(address+(y*2));
    uint8_t msBits = memory->read(address+(y*2)+1);

    uint8_t lsb = (lsBits >> (7-x)) & 0x1;
    uint8_t msb = (msBits >> (7-x)) & 0x1;
    uint8_t col = (msb << 1) | lsb;

    return col;
}
}
<commit_msg>Screen timing improved<commit_after>#include "screen.h"

#include <cstdlib>
#include <cstring>
#include <ctime>
#include <QDebug>
#include "memory.h"

namespace gameboy {
Screen::Screen(Memory *memory) :
    clocks(0),
    memory(memory) {
    framebuffer = new util::Color[160 * 144];

    reset();
}

Screen::~Screen() {
    delete[] framebuffer;
}

void Screen::reset() {
    drawFlag = false;
    line = 0;
    mode = OAM;

    for (int i = 0; i < 160*144; ++i) {
        framebuffer[i].set(0xFF, 0xFF, 0xFF);
    }
}

util::Color* Screen::getFramebuffer() {
    return framebuffer;
}

bool Screen::drawFlagSet() {
    return drawFlag;
}

void Screen::step(unsigned int lastClocks) {
    clocks += lastClocks;
    drawFlag = false;

    switch (mode) {
        case OAM: // Mode 10
            if (clocks >= 20) {
                clocks = 0;
                mode = VRAM;
                memory->write(0xFF41, memory->read(0xFF41) | 0x03); // Indicate VRAM Mode 11
            }
        break;
        case VRAM: // Mode 11
            if (clocks >= 43) {
                clocks = 0;
                mode = HBLANK;
                renderScanLine();
                memory->write(0xFF41, memory->read(0xFF41) & 0xFC); // Indicate H-Blank Mode 00

                if (memory->read(0xFF41) & 0x08) { // Mode 00 Interrupt
                    memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
                }
            }
        break;
        case HBLANK: // Mode 00
            if (clocks >= 51) {
                clocks = 0;
                ++line;
                if (line == 144) {
                    mode = VBLANK;
                    drawFlag = true;
                    memory->write(0xFF41, memory->read(0xFF41) & 0xFD); // Indicate V-Blank Mode 01
                    memory->write(0xFF41, memory->read(0xFF41) | 0x01);

                    if (memory->read(0xFF41) & 0x10) { // Mode 01 Interrupt
                        memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
                    }

                    memory->write(0xFF0F, memory->read(0xFF0F) | 0x01); //Set V-Blank Interrupt Flag
                } else {
                    mode = OAM;
                    memory->write(0xFF41, memory->read(0xFF41) & 0xFE); // Indicate OAM Mode 10
                    memory->write(0xFF41, memory->read(0xFF41) | 0x02);

                    if (memory->read(0xFF41) & 0x20) { // Mode 10 Interrupt
                        memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
                    }
                }
            }
        break;
        case VBLANK: // Mode 01
            if (clocks >= 114) {
                clocks = 0;
                ++line;
                if (line > 153) {
                    mode = OAM;
                    line = 0;
                    memory->write(0xFF41, memory->read(0xFF41) & 0xFE); // Indicate OAM Mode 10
                    memory->write(0xFF41, memory->read(0xFF41) | 0x02);

                    if (memory->read(0xFF41) & 0x20) { // Mode 10 Interrupt
                        memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
                    }
                }
            }
        break;
    }
    //FF44 - LY - LCDC Y-Coordinate (R)
    memory->write(0xFF44, static_cast<uint8_t>(line));

    // FF41 (STAT)
    uint8_t stat = memory->read(0xFF41);

    uint8_t ly = memory->read(0xFF44);
    uint8_t lyc = memory->read(0xFF45);
    if (lyc == ly) {
        memory->write(0xFF41, stat | 0x04); // Set Coincidence Flag

        if (stat & 0x40) { // LYC=LY Coincidence Interrupt enabled
            memory->write(0xFF0F, memory->read(0xFF0F) | 0x02); // Set LCD-STAT Interrupt Flag
        }
    } else {
        memory->write(0xFF41, stat & 0xFB); // Clear Coincidence Flag
    }
}

void Screen::renderScanLine() {
    // FF40 - LCDC - LCD Control
    uint8_t lcdc = memory->read(0xFF40);

    // Bit 7 - LCD Display Enable (0=Off, 1=On)
    if (lcdc & 0x80) {
        // Bit 0 - BG Display (0=Off, 1=On)
        if (lcdc & 0x01) {
            renderBackground();
        }

        // Bit 5 - Window Display Enable (0=Off, 1=On)
        if (lcdc & 0x20) {
            renderWindow();
        }

        // Bit 1 - OBJ (Sprite) Display Enable (0=Off, 1=On)
        if (lcdc & 0x02) {
            renderSprites();
        }
    }
}

void Screen::renderBackground() {
    uint8_t palette = memory->read(0xFF47);

    for (int i = 0; i < 4; ++i) {
        bgPalette[i] = util::Color::PALETTE[(palette >> (i*2)) & 0x3];
    }

    // FF40 - LCDC - LCD Control
    uint8_t lcdc = memory->read(0xFF40);
    // Bit 4 - BG & Window Tile Data Select
    uint16_t tileSet = (lcdc & 0x10) ? 0x8000 : 0x9000;
    // Bit 3 - BG Tile Map Display Select
    uint16_t tileMap = (lcdc & 0x08) ? 0x9C00 : 0x9800;

    // Coordinates of background to be displayed in the left upper corner
    uint8_t scrollX = memory->read(0xFF43);
    uint8_t scrollY = memory->read(0xFF42);

    int num;
    uint16_t tileRow = ((scrollY + line) % 256) / 8;
    uint16_t inTileY = ((scrollY + line) % 256) % 8;
    for (int x = 0; x < 160; ++x) {
        uint16_t tileColumn = ((scrollX + x) % 256) / 8;
        uint16_t inTileX = ((scrollX + x) % 256) % 8;

        uint16_t tileNumLocation = tileMap + tileRow * 32 + tileColumn;

        if (tileSet == 0x8000) {
             num = static_cast<uint8_t>(memory->read(tileNumLocation));
        } else {
            num = static_cast<int8_t>(memory->read(tileNumLocation));
        }

        framebuffer[line*160+x] = bgPalette[readTile(tileSet+num*16, inTileX, inTileY)];
    }
}

void Screen::renderWindow() {
    int16_t wndY = memory->read(0xFF4A);
    int16_t wndX = memory->read(0xFF4B) - 0x7;

    // Check if the window is displayed
    if (wndY < 0
        || wndY >= 144
        || wndX < -7
        || wndX >= 160) {
        return;
    }

    // Check if the window is on the current line
    if (wndY > static_cast<int16_t>(line)) {
        return;
    }

    uint8_t palette = memory->read(0xFF47);

    for (int i = 0; i < 4; ++i) {
        bgPalette[i] = util::Color::PALETTE[(palette >> (i*2)) & 0x3];
    }

    // FF40 - LCDC - LCD Control
    uint8_t lcdc = memory->read(0xFF40);
    // Bit 4 - BG & Window Tile Data Select
    uint16_t tileSet = (lcdc & 0x10) ? 0x8000 : 0x9000;
    // Bit 6 - Window Tile Map Display Select
    uint16_t tileMap = (lcdc & 0x40) ? 0x9C00 : 0x9800;

    int num;
    uint16_t tileRow = (line - wndY) / 8;
    uint16_t inTileY = (line - wndY) % 8;

    for (int x = (wndX < 0) ? 0 : wndX; x < 160; ++x) {
        uint16_t tileColumn = (x - wndX) / 8;
        uint16_t inTileX = (x - wndX) % 8;

        uint16_t tileNumLocation = tileMap + tileRow * 32 + tileColumn;

        if (tileSet == 0x8000) {
             num = static_cast<uint8_t>(memory->read(tileNumLocation));
        } else {
            num = static_cast<int8_t>(memory->read(tileNumLocation));
        }

        framebuffer[line*160+x] = bgPalette[readTile(tileSet+num*16, inTileX, inTileY)];
    }
}

void Screen::renderSprites() {
    // Bit 2 - OBJ (Sprite) Size (0=8x8, 1=8x16)
    bool largeSprites = memory->read(0xFF40) & 0x4;

    for (int h = 39; h >= 0; --h) { // Respect sprite priority
        uint16_t currentOAMAddr = 0xFE00 + 0x4 * h;

        // Top left Corner
        // Byte 0 - Y Position - Vertical position on the screen (minus 16).
        int16_t yCoord = memory->read(currentOAMAddr) - 0x10;
        // Byte 1 - X Position - Horizontal position on the screen (minus 8)
        int16_t xCoord = memory->read(currentOAMAddr+1) - 0x8;

        // Check if the sprite is not on the screen
        if (xCoord >= 160
            || xCoord <= -8
            || yCoord >= 144
            || yCoord <= -(largeSprites ? 16 : 8)) {
            continue;
        }

        uint8_t inTileY = line - yCoord;

        // Check if it is not on the current line
        if (yCoord > static_cast<int16_t>(line) || inTileY >= (largeSprites ? 16 : 8)) {
            continue;
        }

        uint8_t optionsByte = memory->read(currentOAMAddr+3);
        bool priority = !(optionsByte & 0x80);
        bool yFlip = optionsByte & 0x40;
        bool xFlip = optionsByte & 0x20;
        uint8_t palette = memory->read((optionsByte & 0x10) ? 0xFF49 : 0xFF48);

        for (int i = 0; i < 4; ++i) {
            spritePalette[i] = util::Color::PALETTE[(palette >> (i*2)) & 0x3];
        }

        // Byte 2 - Pattern number
        uint8_t spriteNum = memory->read(currentOAMAddr+2);

        // Find tile address
        uint16_t tileAddr;

        if (largeSprites) {
            if (yFlip) {
                inTileY = 15 - inTileY;
            }

            // Check which 8x8 tile has to be used
            if (inTileY < 8) {
                tileAddr = 0x8000 + 0x10 * (spriteNum & 0xFE); // Upper tile, ignore LSB
            } else {
                tileAddr = 0x8000 + 0x10 * (spriteNum | 0x01); // Lower tile, set LSB
                inTileY -= 8;
            }
        } else {
            if (yFlip) {
                inTileY = 7 - inTileY;
            }

            tileAddr = 0x8000 + 0x10 * spriteNum;
        }

        for (int x = 0; x < 8; ++x) {
            if (xCoord + x < 0 || xCoord + x >= 160) {
                continue;
            }

            uint8_t pixel = readTile(tileAddr, xFlip ? (7-x) : x, inTileY); // 0 = transparency

            if (pixel && (priority || (framebuffer[line*160+xCoord+x] == bgPalette[0]))) {
                framebuffer[line*160+xCoord+x] = spritePalette[pixel];
            }
        }
    }
}

uint8_t Screen::readTile(uint16_t address, uint8_t x, uint8_t y) {
    uint8_t lsBits = memory->read(address+(y*2));
    uint8_t msBits = memory->read(address+(y*2)+1);

    uint8_t lsb = (lsBits >> (7-x)) & 0x1;
    uint8_t msb = (msBits >> (7-x)) & 0x1;
    uint8_t col = (msb << 1) | lsb;

    return col;
}
}
<|endoftext|>


<commit_before>#pragma once

#include <cassert>
#include <type_traits>
#include <variant>

#include <athena/DNA.hpp>

/*
 * The TypedVariant system is a type-safe union implementation capable of selecting
 * a participating field type based on an enumeration constant. As an extension of
 * std::variant, the usage pattern is similar to that of std::visit. A key difference
 * with TypedVariant is the monostate is implicitly supplied as the first argument.
 * The monostate will cause the visit implementation to function as a no-op, returning
 * a specified default value.
 */

/* Example user code:

enum class ChunkType {
  Invalid = 0,
  TYP1 = SBIG('TYP1'),
  TYP2 = SBIG('TYP2'),
  TYP3 = SBIG('TYP3'),
};

struct TYP1 : TypedRecord<ChunkType::TYP1> {
  static void PrintMe() { std::cout << "TYP1" << std::endl; }
};
struct TYP2 : TypedRecord<ChunkType::TYP2> {
  static void PrintMe() { std::cout << "TYP2" << std::endl; }
};
struct TYP3 : TypedRecord<ChunkType::TYP3> {
  static void PrintMe() { std::cout << "TYP3" << std::endl; }
};

using ChunkVariant = TypedVariant<TYP1, TYP2, TYP3>;

int main(int argc, char** argv) {
  volatile ChunkType tp = ChunkType::TYP2;
  auto var = ChunkVariant::Build(tp);
  var.visit([](auto& arg) { arg.PrintMe(); });
  return 0;
}

 */

namespace hecl {

template<auto _Type>
struct TypedRecord {
  using TypedType = std::integral_constant<decltype(_Type), _Type>;
  static constexpr auto variant_type() { return _Type; }
  static constexpr bool is_monostate() { return false; }
};

template<typename _Enum>
struct TypedMonostate : TypedRecord<_Enum(0)> {
  static constexpr bool is_monostate() { return true; }
  bool operator==(const TypedMonostate& other) const { return true; }
  bool operator!=(const TypedMonostate& other) const { return false; }
};

template<typename... _Types>
class _TypedVariant : public std::variant<_Types...> {
public:
  using base = std::variant<_Types...>;
  using EnumType = typename std::variant_alternative_t<0, std::variant<_Types...>>::TypedType::value_type;

private:
  template<typename _Type, typename _Variant>
  struct _Match;

  template<typename _Type, typename _First, typename... _Rest>
  struct _Match<_Type, std::variant<_First, _Rest...>>
      : _Match<_Type, std::variant<_Rest...>> {};

  template<typename _First, typename... _Rest>
  struct _Match<typename _First::TypedType, std::variant<_First, _Rest...>>
  { using type = _First; };

public:
  template<auto _Type>
  using Match = typename _Match<std::integral_constant<decltype(_Type), _Type>, std::variant<_Types...>>::type;

private:
  template<typename _First, typename... _Rest>
  struct _Builder {
    template<typename... _Args>
    static constexpr _TypedVariant _Build(EnumType tp, _Args&&... args) {
      //static_assert(std::is_constructible_v<_First, _Args...>,
      //              "All variant types must be constructible with the same parameters.");
      if (_First::TypedType::value == tp) {
        if constexpr (std::is_constructible_v<_First, _Args...>) {
          return {_First(std::forward<_Args>(args)...)};
        } else {
          assert(false && "Variant not constructible with supplied args.");
          return {};
        }
      }
      else if constexpr (sizeof...(_Rest) > 0)
        return _Builder<_Rest...>::template _Build<_Args...>(tp, std::forward<_Args>(args)...);
      return {};
    }
    template<typename... _Args>
    static constexpr _TypedVariant _BuildSkip(EnumType tp, _Args&&... args) {
      /* This prevents selecting the monostate explicitly (so constructor arguments aren't passed) */
      if constexpr (sizeof...(_Rest) > 0)
        return _Builder<_Rest...>::template _Build<_Args...>(tp, std::forward<_Args>(args)...);
      return {};
    }
  };

public:
  template<typename... _Args>
  static constexpr _TypedVariant Build(EnumType tp, _Args&&... args) {
    return _TypedVariant::_Builder<_Types...>::template _BuildSkip<_Args...>(tp, std::forward<_Args>(args)...);
  }

  template<typename _Return, typename _Visitor>
  constexpr auto visit(_Visitor&& visitor, _Return&& def) {
    return std::visit([&](auto& arg) {
      using T = std::decay_t<decltype(arg)>;
      if constexpr (!T::is_monostate())
        return visitor(arg);
      return def;
    }, static_cast<base&>(*this));
  }

  template<typename _Visitor>
  constexpr void visit(_Visitor&& visitor) {
    std::visit([&](auto& arg) {
      using T = std::decay_t<decltype(arg)>;
      if constexpr (!T::is_monostate())
        visitor(arg);
    }, static_cast<base&>(*this));
  }

  template<typename _Return, typename _Visitor>
  constexpr auto visit(_Visitor&& visitor, _Return&& def) const {
    return std::visit([&](const auto& arg) {
      using T = std::decay_t<decltype(arg)>;
      if constexpr (!T::is_monostate())
        return visitor(arg);
      return def;
    }, static_cast<const base&>(*this));
  }

  template<typename _Visitor>
  constexpr void visit(_Visitor&& visitor) const {
    std::visit([&](const auto& arg) {
      using T = std::decay_t<decltype(arg)>;
      if constexpr (!T::is_monostate())
        visitor(arg);
    }, static_cast<const base&>(*this));
  }

  template<typename T>
  constexpr T& get() { return std::get<T>(*this); }

  template<typename T>
  constexpr const T& get() const { return std::get<T>(*this); }

  template<typename T>
  constexpr std::add_pointer_t<T> get_if() noexcept { return std::get_if<T>(this); }

  template<typename T>
  constexpr std::add_pointer_t<const T> get_if() const noexcept { return std::get_if<T>(this); }

  template<typename T>
  constexpr bool holds_alternative() const noexcept { return std::holds_alternative<T>(*this); }

  constexpr EnumType variant_type() const {
    return std::visit([](const auto& arg) {
      return arg.variant_type();
    }, static_cast<const base&>(*this));
  }

  constexpr explicit operator bool() const noexcept {
    return !std::holds_alternative<typename std::variant_alternative_t<0, std::variant<_Types...>>>(*this);
  }
};

template<typename... _Types>
using TypedVariant = _TypedVariant<TypedMonostate<typename std::variant_alternative_t
    <0, std::variant<_Types...>>::TypedType::value_type>, _Types...>;

/* DNA support below here */

template<auto _Type>
struct TypedRecordBigDNA : BigDNA, TypedRecord<_Type> {};

template<typename... _Types>
struct TypedVariantBigDNA : BigDNA, TypedVariant<_Types...> {
  AT_DECL_EXPLICIT_DNA_YAML
  template<typename... _Args>
  static constexpr TypedVariantBigDNA Build(typename TypedVariant<_Types...>::EnumType tp, _Args&&... args) {
    return TypedVariantBigDNA(TypedVariant<_Types...>::Build(tp, std::forward<_Args>(args)...));
  }
  TypedVariantBigDNA() = default;
private:
  TypedVariantBigDNA(TypedVariant<_Types...> var) : TypedVariant<_Types...>(std::move(var)) {}
};

#define AT_SPECIALIZE_TYPED_VARIANT_BIGDNA(...)                                                                        \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Big>::Read>(                    \
      typename Read::StreamT & r) {                                                                                \
    EnumType variant_type = {};                                                                                \
    Do<athena::io::DNA<athena::Big>::Read>(athena::io::PropId("variant_type"), variant_type, r);                       \
    static_cast<TypedVariant<__VA_ARGS__>&>(*this) = Build(variant_type);                                              \
    visit([&](auto& var) { var.read(r); });                                                                            \
  }                                                                                \
                                                                                \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Big>::Write>(                   \
      typename Write::StreamT & w) {                                                                                \
    visit([&](auto& var) {                                                                                \
      using T = std::decay_t<decltype(var)>;                                                                           \
      EnumType variant_type = T::variant_type();                                                                       \
      Do<athena::io::DNA<athena::Big>::Write>(athena::io::PropId("variant_type"), variant_type, w);                    \
      var.write(w);                                                                                \
    });                                                                                \
  }                                                                                \
                                                                                \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Big>::BinarySize>(              \
      typename BinarySize::StreamT & sz) {                                                                             \
    visit([&](auto& var) {                                                                                \
      using T = std::decay_t<decltype(var)>;                                                                           \
      EnumType variant_type = T::variant_type();                                                                       \
      Do<athena::io::DNA<athena::Big>::BinarySize>(athena::io::PropId("variant_type"), variant_type, sz);              \
      var.binarySize(sz);                                                                                \
    });                                                                                \
  }                                                                                \
  template <>                                                                                \
  inline const char* hecl::TypedVariantBigDNA<__VA_ARGS__>::DNAType() {                                                \
    return "hecl::TypedVariantBigDNA<" #__VA_ARGS__ ">";                                                               \
  }

#define AT_SPECIALIZE_TYPED_VARIANT_BIGDNA_YAML(...)                                                                   \
  AT_SPECIALIZE_TYPED_VARIANT_BIGDNA(__VA_ARGS__)                                                                      \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Big>::ReadYaml>(                \
      typename ReadYaml::StreamT & r) {                                                                                \
    EnumType variant_type = {};                                                                                \
    Do<athena::io::DNA<athena::Big>::ReadYaml>(athena::io::PropId("variant_type"), variant_type, r);                   \
    static_cast<TypedVariant<__VA_ARGS__>&>(*this) = Build(variant_type);                                              \
    visit([&](auto& var) { var.read(r); });                                                                            \
  }                                                                                \
                                                                                \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Big>::WriteYaml>(               \
      typename WriteYaml::StreamT & w) {                                                                               \
    visit([&](auto& var) {                                                                                \
      using T = std::decay_t<decltype(var)>;                                                                           \
      EnumType variant_type = T::variant_type();                                                                       \
      Do<athena::io::DNA<athena::Big>::WriteYaml>(athena::io::PropId("variant_type"), variant_type, w);                \
      var.write(w);                                                                                \
    });                                                                                \
  }

}
<commit_msg>TypedVariant: Be explicit about athena's Endian type<commit_after>#pragma once

#include <cassert>
#include <type_traits>
#include <variant>

#include <athena/DNA.hpp>

/*
 * The TypedVariant system is a type-safe union implementation capable of selecting
 * a participating field type based on an enumeration constant. As an extension of
 * std::variant, the usage pattern is similar to that of std::visit. A key difference
 * with TypedVariant is the monostate is implicitly supplied as the first argument.
 * The monostate will cause the visit implementation to function as a no-op, returning
 * a specified default value.
 */

/* Example user code:

enum class ChunkType {
  Invalid = 0,
  TYP1 = SBIG('TYP1'),
  TYP2 = SBIG('TYP2'),
  TYP3 = SBIG('TYP3'),
};

struct TYP1 : TypedRecord<ChunkType::TYP1> {
  static void PrintMe() { std::cout << "TYP1" << std::endl; }
};
struct TYP2 : TypedRecord<ChunkType::TYP2> {
  static void PrintMe() { std::cout << "TYP2" << std::endl; }
};
struct TYP3 : TypedRecord<ChunkType::TYP3> {
  static void PrintMe() { std::cout << "TYP3" << std::endl; }
};

using ChunkVariant = TypedVariant<TYP1, TYP2, TYP3>;

int main(int argc, char** argv) {
  volatile ChunkType tp = ChunkType::TYP2;
  auto var = ChunkVariant::Build(tp);
  var.visit([](auto& arg) { arg.PrintMe(); });
  return 0;
}

 */

namespace hecl {

template<auto _Type>
struct TypedRecord {
  using TypedType = std::integral_constant<decltype(_Type), _Type>;
  static constexpr auto variant_type() { return _Type; }
  static constexpr bool is_monostate() { return false; }
};

template<typename _Enum>
struct TypedMonostate : TypedRecord<_Enum(0)> {
  static constexpr bool is_monostate() { return true; }
  bool operator==(const TypedMonostate& other) const { return true; }
  bool operator!=(const TypedMonostate& other) const { return false; }
};

template<typename... _Types>
class _TypedVariant : public std::variant<_Types...> {
public:
  using base = std::variant<_Types...>;
  using EnumType = typename std::variant_alternative_t<0, std::variant<_Types...>>::TypedType::value_type;

private:
  template<typename _Type, typename _Variant>
  struct _Match;

  template<typename _Type, typename _First, typename... _Rest>
  struct _Match<_Type, std::variant<_First, _Rest...>>
      : _Match<_Type, std::variant<_Rest...>> {};

  template<typename _First, typename... _Rest>
  struct _Match<typename _First::TypedType, std::variant<_First, _Rest...>>
  { using type = _First; };

public:
  template<auto _Type>
  using Match = typename _Match<std::integral_constant<decltype(_Type), _Type>, std::variant<_Types...>>::type;

private:
  template<typename _First, typename... _Rest>
  struct _Builder {
    template<typename... _Args>
    static constexpr _TypedVariant _Build(EnumType tp, _Args&&... args) {
      //static_assert(std::is_constructible_v<_First, _Args...>,
      //              "All variant types must be constructible with the same parameters.");
      if (_First::TypedType::value == tp) {
        if constexpr (std::is_constructible_v<_First, _Args...>) {
          return {_First(std::forward<_Args>(args)...)};
        } else {
          assert(false && "Variant not constructible with supplied args.");
          return {};
        }
      }
      else if constexpr (sizeof...(_Rest) > 0)
        return _Builder<_Rest...>::template _Build<_Args...>(tp, std::forward<_Args>(args)...);
      return {};
    }
    template<typename... _Args>
    static constexpr _TypedVariant _BuildSkip(EnumType tp, _Args&&... args) {
      /* This prevents selecting the monostate explicitly (so constructor arguments aren't passed) */
      if constexpr (sizeof...(_Rest) > 0)
        return _Builder<_Rest...>::template _Build<_Args...>(tp, std::forward<_Args>(args)...);
      return {};
    }
  };

public:
  template<typename... _Args>
  static constexpr _TypedVariant Build(EnumType tp, _Args&&... args) {
    return _TypedVariant::_Builder<_Types...>::template _BuildSkip<_Args...>(tp, std::forward<_Args>(args)...);
  }

  template<typename _Return, typename _Visitor>
  constexpr auto visit(_Visitor&& visitor, _Return&& def) {
    return std::visit([&](auto& arg) {
      using T = std::decay_t<decltype(arg)>;
      if constexpr (!T::is_monostate())
        return visitor(arg);
      return def;
    }, static_cast<base&>(*this));
  }

  template<typename _Visitor>
  constexpr void visit(_Visitor&& visitor) {
    std::visit([&](auto& arg) {
      using T = std::decay_t<decltype(arg)>;
      if constexpr (!T::is_monostate())
        visitor(arg);
    }, static_cast<base&>(*this));
  }

  template<typename _Return, typename _Visitor>
  constexpr auto visit(_Visitor&& visitor, _Return&& def) const {
    return std::visit([&](const auto& arg) {
      using T = std::decay_t<decltype(arg)>;
      if constexpr (!T::is_monostate())
        return visitor(arg);
      return def;
    }, static_cast<const base&>(*this));
  }

  template<typename _Visitor>
  constexpr void visit(_Visitor&& visitor) const {
    std::visit([&](const auto& arg) {
      using T = std::decay_t<decltype(arg)>;
      if constexpr (!T::is_monostate())
        visitor(arg);
    }, static_cast<const base&>(*this));
  }

  template<typename T>
  constexpr T& get() { return std::get<T>(*this); }

  template<typename T>
  constexpr const T& get() const { return std::get<T>(*this); }

  template<typename T>
  constexpr std::add_pointer_t<T> get_if() noexcept { return std::get_if<T>(this); }

  template<typename T>
  constexpr std::add_pointer_t<const T> get_if() const noexcept { return std::get_if<T>(this); }

  template<typename T>
  constexpr bool holds_alternative() const noexcept { return std::holds_alternative<T>(*this); }

  constexpr EnumType variant_type() const {
    return std::visit([](const auto& arg) {
      return arg.variant_type();
    }, static_cast<const base&>(*this));
  }

  constexpr explicit operator bool() const noexcept {
    return !std::holds_alternative<typename std::variant_alternative_t<0, std::variant<_Types...>>>(*this);
  }
};

template<typename... _Types>
using TypedVariant = _TypedVariant<TypedMonostate<typename std::variant_alternative_t
    <0, std::variant<_Types...>>::TypedType::value_type>, _Types...>;

/* DNA support below here */

template<auto _Type>
struct TypedRecordBigDNA : BigDNA, TypedRecord<_Type> {};

template<typename... _Types>
struct TypedVariantBigDNA : BigDNA, TypedVariant<_Types...> {
  AT_DECL_EXPLICIT_DNA_YAML
  template<typename... _Args>
  static constexpr TypedVariantBigDNA Build(typename TypedVariant<_Types...>::EnumType tp, _Args&&... args) {
    return TypedVariantBigDNA(TypedVariant<_Types...>::Build(tp, std::forward<_Args>(args)...));
  }
  TypedVariantBigDNA() = default;
private:
  TypedVariantBigDNA(TypedVariant<_Types...> var) : TypedVariant<_Types...>(std::move(var)) {}
};

#define AT_SPECIALIZE_TYPED_VARIANT_BIGDNA(...)                                                                        \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Endian::Big>::Read>(            \
      typename Read::StreamT & r) {                                                                                \
    EnumType variant_type = {};                                                                                \
    Do<athena::io::DNA<athena::Endian::Big>::Read>(athena::io::PropId("variant_type"), variant_type, r);               \
    static_cast<TypedVariant<__VA_ARGS__>&>(*this) = Build(variant_type);                                              \
    visit([&](auto& var) { var.read(r); });                                                                            \
  }                                                                                \
                                                                                \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Endian::Big>::Write>(           \
      typename Write::StreamT & w) {                                                                                \
    visit([&](auto& var) {                                                                                \
      using T = std::decay_t<decltype(var)>;                                                                           \
      EnumType variant_type = T::variant_type();                                                                       \
      Do<athena::io::DNA<athena::Endian::Big>::Write>(athena::io::PropId("variant_type"), variant_type, w);            \
      var.write(w);                                                                                \
    });                                                                                \
  }                                                                                \
                                                                                \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Endian::Big>::BinarySize>(      \
      typename BinarySize::StreamT & sz) {                                                                             \
    visit([&](auto& var) {                                                                                \
      using T = std::decay_t<decltype(var)>;                                                                           \
      EnumType variant_type = T::variant_type();                                                                       \
      Do<athena::io::DNA<athena::Endian::Big>::BinarySize>(athena::io::PropId("variant_type"), variant_type, sz);      \
      var.binarySize(sz);                                                                                \
    });                                                                                \
  }                                                                                \
  template <>                                                                                \
  inline const char* hecl::TypedVariantBigDNA<__VA_ARGS__>::DNAType() {                                                \
    return "hecl::TypedVariantBigDNA<" #__VA_ARGS__ ">";                                                               \
  }

#define AT_SPECIALIZE_TYPED_VARIANT_BIGDNA_YAML(...)                                                                   \
  AT_SPECIALIZE_TYPED_VARIANT_BIGDNA(__VA_ARGS__)                                                                      \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Endian::Big>::ReadYaml>(        \
      typename ReadYaml::StreamT & r) {                                                                                \
    EnumType variant_type = {};                                                                                \
    Do<athena::io::DNA<athena::Endian::Big>::ReadYaml>(athena::io::PropId("variant_type"), variant_type, r);           \
    static_cast<TypedVariant<__VA_ARGS__>&>(*this) = Build(variant_type);                                              \
    visit([&](auto& var) { var.read(r); });                                                                            \
  }                                                                                \
                                                                                \
  template <>                                                                                \
  template <>                                                                                \
  inline void hecl::TypedVariantBigDNA<__VA_ARGS__>::Enumerate<athena::io::DNA<athena::Endian::Big>::WriteYaml>(       \
      typename WriteYaml::StreamT & w) {                                                                               \
    visit([&](auto& var) {                                                                                \
      using T = std::decay_t<decltype(var)>;                                                                           \
      EnumType variant_type = T::variant_type();                                                                       \
      Do<athena::io::DNA<athena::Endian::Big>::WriteYaml>(athena::io::PropId("variant_type"), variant_type, w);        \
      var.write(w);                                                                                \
    });                                                                                \
  }

}
<|endoftext|>