<commit_before>#include <QtCore>
#include <QtNetwork>

#ifndef Q_OS_WIN
#include <unistd.h>
#endif
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/types.h>
#ifndef Q_OS_WIN
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <sys/utsname.h>
#endif
#include <errno.h>

#include "PacketDataStream.h"
#include "Timer.h"
#include "Message.h"
#include "CryptState.h"
#include "Mumble.pb.h"

class Client : public QThread {
		Q_OBJECT
	public:
		bool udp;
		bool sender;
		struct sockaddr_in srv;
		unsigned int uiSession;
		CryptState crypt;
		int rcvd;
		int socket;
		int seq;
		void run();
		void ping();
		void sendVoice();
		int numbytes;
		int ptype;
		QSslSocket *ssl;
		Client(QObject *parent, QHostAddress srvaddr, unsigned short prt, bool send, bool tcponly);
		void doUdp(const unsigned char *buffer, int size);
		void sendMessage(const ::google::protobuf::Message &msg, unsigned int msgType);
		~Client();
	public slots:
		void readyRead();
		void disconnected();
};

Client::Client(QObject *p, QHostAddress qha, unsigned short prt, bool send, bool tcponly) : QThread(p) {
	srv.sin_family = AF_INET;
	srv.sin_addr.s_addr = htonl(qha.toIPv4Address());
	srv.sin_port = htons(prt);

	udp = ! tcponly;
	sender = send;

	ssl = new QSslSocket(this);

	connect(ssl, SIGNAL(readyRead()), this, SLOT(readyRead()));
	connect(ssl, SIGNAL(disconnected()), this, SLOT(disconnected()));

	ssl->setProtocol(QSsl::TlsV1);
	ssl->connectToHostEncrypted(qha.toString(), prt);
	ssl->ignoreSslErrors();
	if (! ssl->waitForEncrypted())
		qFatal("Connection failure");

	static int ctr = 1;

#ifdef Q_OS_WIN
	DWORD pid = GetCurrentProcessId();
	wchar_t buf[64];
	DWORD bufsize = 64;
	GetComputerName(buf, &bufsize);

	QString name = QString("%1.%2").arg(QString::fromWCharArray(buf)).arg(pid * 1000 + ctr);
#else
	struct utsname uts;
	uname(&uts);

	QString name = QString("%1.%2").arg(uts.nodename).arg(getpid() * 1000 + ctr);
#endif

	ctr++;

	MumbleProto::Version mpv;
	mpv.set_release(u8(QLatin1String("1.2.1 Benchmark")));
	mpv.set_version(0x010200);

	sendMessage(mpv, MessageHandler::Version);

	MumbleProto::Authenticate mpa;
	mpa.set_username(u8(name));

	sendMessage(mpa, MessageHandler::Authenticate);

	if (udp)
		socket = ::socket(PF_INET, SOCK_DGRAM, 0);

	seq = 0;
	rcvd = 0;
	numbytes = -1;
}

Client::~Client() {
	terminate();
	wait();
}

void Client::sendMessage(const ::google::protobuf::Message &msg, unsigned int msgType) {
	unsigned char uc[4096];
	int len = msg.ByteSize();
	Q_ASSERT(len < 4090);

	* reinterpret_cast<quint16 *>(& uc[0]) = qToBigEndian(static_cast<quint16>(msgType));
	* reinterpret_cast<quint32 *>(& uc[2]) = qToBigEndian(static_cast<quint32>(len));

	msg.SerializeToArray(uc + 6, len);

	ssl->write(reinterpret_cast<const char *>(uc), len+6);
}

void Client::ping() {
	unsigned char buffer[256];
	buffer[0] = MessageHandler::UDPPing << 5;
	PacketDataStream pds(buffer + 1, 255);
	pds << 123;

	doUdp(buffer, pds.size() + 1);

	MumbleProto::Ping mpp;
	mpp.set_timestamp(123);
	sendMessage(mpp, MessageHandler::Ping);
}

void Client::sendVoice() {
	unsigned char buffer[1024];
	int len = 32 + (qrand() & 0x3f);

	// Regular voice, nothing special
	buffer[0] = 0;

	PacketDataStream ods(buffer + 1, 1024);
	ods << 1;
	ods.append(len);
	ods.skip(len);

	doUdp(buffer, ods.size() + 1);
}

void Client::doUdp(const unsigned char *buffer, int size) {
	if (! udp || ! crypt.isValid())
		return;

	unsigned char crypted[2048];

	crypt.encrypt(reinterpret_cast<const unsigned char *>(buffer), crypted, size);
	::sendto(socket, reinterpret_cast<const char *>(crypted), size+4, 0, reinterpret_cast<struct sockaddr *>(&srv), sizeof(srv));
}

void Client::run() {
	unsigned char buffer[1024];
	struct sockaddr_in addr;
	socklen_t sz;
	int len;

	if (! udp)
		return;

	forever {
		sz = sizeof(addr);
		len = ::recvfrom(socket, reinterpret_cast<char *>(buffer), 1024, 0, reinterpret_cast<struct sockaddr *>(&addr), &sz);
		if (len <= 0)
			break;
		if (len >= 32)
			rcvd++;
	}
}

void Client::readyRead() {
	forever {
		int avail = ssl->bytesAvailable();
		if (numbytes == -1) {
			if (avail < 6)
				break;
			unsigned char b[6];
			ssl->read(reinterpret_cast<char *>(b), 6);

			ptype = qFromBigEndian(* reinterpret_cast<quint16 *>(& b[0]));
			numbytes = qFromBigEndian(* reinterpret_cast<quint32 *>(& b[2]));

			avail = ssl->bytesAvailable();
		}
		if ((numbytes >= 0) && (avail >= numbytes)) {
			int want = numbytes;
			numbytes = -1;
			unsigned char buff[65536];
			Q_ASSERT(want < 65536);
			ssl->read(reinterpret_cast<char *>(buff), want);

			avail = ssl->bytesAvailable();

			switch (ptype) {
				case MessageHandler::CryptSetup: {
						MumbleProto::CryptSetup msg;
						if (! msg.ParseFromArray(buff, want))
							qFatal("Failed parse crypt");

						if (msg.has_key() && msg.has_client_nonce() && msg.has_server_nonce()) {
							const std::string &key = msg.key();
							const std::string &client_nonce = msg.client_nonce();
							const std::string &server_nonce = msg.server_nonce();
							if (key.size() == AES_BLOCK_SIZE && client_nonce.size() == AES_BLOCK_SIZE && server_nonce.size() == AES_BLOCK_SIZE)
								crypt.setKey(reinterpret_cast<const unsigned char *>(key.data()), reinterpret_cast<const unsigned char *>(client_nonce.data()), reinterpret_cast<const unsigned char *>(server_nonce.data()));
						} else {
							qFatal("Crypt resync");
						}
						break;
					}
				case MessageHandler::ServerSync: {
						MumbleProto::ServerSync msg;
						if (! msg.ParseFromArray(buff, want))
							qFatal("Failed parse sync");
						uiSession = msg.session();
						break;
					}
				case MessageHandler::UDPTunnel: {
						unsigned int msgUDPType = (buff[0] >> 5) & 0x7;
						if (msgUDPType == MessageHandler::UDPVoiceCELTAlpha)
							rcvd++;
						break;
					}
			}
		} else {
			break;
		}
	}
}

void Client::disconnected() {
	qWarning("SSL Socket disconnected");
	QCoreApplication::instance()->quit();
}

class Container : public QObject {
		Q_OBJECT
	public:
		int sent;
		int nsend, nudp, ntcp;
		int isend, iudp, itcp;
		bool live, forceping;
		QHostAddress qha;
		unsigned short port;
		QTimer qtTick;
		Timer tickPing, tickVoice, tickGo, tickSpawn;
		QList<Client *> speakers;
		QList<Client *> clients;
		Container(QHostAddress srvaddr, unsigned short port, int nsend, int nudp, int ntcp);
	public slots:
		void tick();
		void go();
};

Container::Container(QHostAddress qha, unsigned short port, int numsend, int numudp, int numtcp) {
	Timer t;

	qWarning("Spawning %d speakers and %d listeners (%d UDP, %d TCP)", numsend, numudp+numtcp, numudp, numtcp);

	nsend=numsend;
	nudp=numudp;
	ntcp=numtcp;

	isend=iudp=itcp=0;

	this->qha = qha;
	this->port = port;

	connect(&qtTick, SIGNAL(timeout()), this, SLOT(tick()));
	qtTick.start(0);

	live = false;
	forceping = false;

	tickSpawn.restart();
	sent = 0;

}

void Container::tick() {
	if (forceping || tickPing.isElapsed(5000000ULL)) {
		forceping = false;

		foreach(Client *c, clients)
			c->ping();

		if (live) {

			int lost = 0;
			quint64 totrcv = 0;
			int nrcv = 0;
			foreach(Client *c, clients) {
				if (! c->sender) {
					totrcv += c->rcvd;
					lost += sent - c->rcvd;
					nrcv++;
				}
			}
			qWarning("Sent: %8d  Rcvd: %8lld  Lost: %8d   BW: %6.1fMbit/s", sent, totrcv / nrcv, (lost + nrcv - 1) / nrcv, (totrcv * 8.0 * 123.0) / (tickGo.elapsed() * 1.0));
		} else {
			qWarning("Spawned %3d/%3d", isend+iudp+itcp,nsend+nudp+ntcp);
		}
	}

	if (live && tickVoice.isElapsed(10000ULL)) {
		foreach(Client *c, speakers) {
			sent++;
			c->sendVoice();
		}
	}
	if (! live) {
		if (isend < nsend) {
			Client *c = new Client(this, qha, port, true, false);
			speakers << c;
			c->start();
			clients << c;
			isend++;
		} else if (iudp < nudp) {
			Client *c = new Client(this, qha, port, false, false);
			c->start();
			clients << c;
			iudp++;
		} else if (itcp < ntcp) {
			Client *c = new Client(this, qha, port, false, true);
			c->start();
			clients << c;
			itcp++;
		} else {
			live = true;
			quint64 elapsed = tickSpawn.elapsed();
			qWarning("Spawning took %lld ms (%lld us per client)", elapsed / 1000ULL, elapsed / (nsend+nudp+ntcp));
			foreach(Client *c, clients)
				c->rcvd = 0;
			sent = 0;
			forceping = true;
			qtTick.start(10);
		}
	}
}

void Container::go() {
	foreach(Client *c, clients)
		c->start();
	qtTick.start(10);
	tickGo.restart();
}

int main(int argc, char **argv) {
	QCoreApplication a(argc, argv);

	qWarning("Maximum # sockets is %d", FD_SETSIZE);

	if (argc != 6)
		qFatal("Invalid # args");

	QHostAddress qha = QHostAddress(argv[1]);
	int port = atoi(argv[2]);
	int numsend = atoi(argv[3]);
	int numudp = atoi(argv[4]);
	int numtcp = atoi(argv[5]);

	Container c(qha, port, numsend, numudp, numtcp);
	c.go();
	a.exec();
}

#include "Benchmark.moc"
<commit_msg>Benchmark: Fix crypt resync handling<commit_after>#include <QtCore>
#include <QtNetwork>

#ifndef Q_OS_WIN
#include <unistd.h>
#endif
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/types.h>
#ifndef Q_OS_WIN
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <sys/utsname.h>
#endif
#include <errno.h>

#include "PacketDataStream.h"
#include "Timer.h"
#include "Message.h"
#include "CryptState.h"
#include "Mumble.pb.h"

class Client : public QThread {
		Q_OBJECT
	public:
		bool udp;
		bool sender;
		struct sockaddr_in srv;
		unsigned int uiSession;
		CryptState crypt;
		int rcvd;
		int socket;
		int seq;
		void run();
		void ping();
		void sendVoice();
		int numbytes;
		int ptype;
		QSslSocket *ssl;
		Client(QObject *parent, QHostAddress srvaddr, unsigned short prt, bool send, bool tcponly);
		void doUdp(const unsigned char *buffer, int size);
		void sendMessage(const ::google::protobuf::Message &msg, unsigned int msgType);
		~Client();
	public slots:
		void readyRead();
		void disconnected();
};

Client::Client(QObject *p, QHostAddress qha, unsigned short prt, bool send, bool tcponly) : QThread(p) {
	srv.sin_family = AF_INET;
	srv.sin_addr.s_addr = htonl(qha.toIPv4Address());
	srv.sin_port = htons(prt);

	udp = ! tcponly;
	sender = send;

	ssl = new QSslSocket(this);

	connect(ssl, SIGNAL(readyRead()), this, SLOT(readyRead()));
	connect(ssl, SIGNAL(disconnected()), this, SLOT(disconnected()));

	ssl->setProtocol(QSsl::TlsV1);
	ssl->connectToHostEncrypted(qha.toString(), prt);
	ssl->ignoreSslErrors();
	if (! ssl->waitForEncrypted())
		qFatal("Connection failure");

	static int ctr = 1;

#ifdef Q_OS_WIN
	DWORD pid = GetCurrentProcessId();
	wchar_t buf[64];
	DWORD bufsize = 64;
	GetComputerName(buf, &bufsize);

	QString name = QString("%1.%2").arg(QString::fromWCharArray(buf)).arg(pid * 1000 + ctr);
#else
	struct utsname uts;
	uname(&uts);

	QString name = QString("%1.%2").arg(uts.nodename).arg(getpid() * 1000 + ctr);
#endif

	ctr++;

	MumbleProto::Version mpv;
	mpv.set_release(u8(QLatin1String("1.2.1 Benchmark")));
	mpv.set_version(0x010200);

	sendMessage(mpv, MessageHandler::Version);

	MumbleProto::Authenticate mpa;
	mpa.set_username(u8(name));

	sendMessage(mpa, MessageHandler::Authenticate);

	if (udp)
		socket = ::socket(PF_INET, SOCK_DGRAM, 0);

	seq = 0;
	rcvd = 0;
	numbytes = -1;
}

Client::~Client() {
	terminate();
	wait();
}

void Client::sendMessage(const ::google::protobuf::Message &msg, unsigned int msgType) {
	unsigned char uc[4096];
	int len = msg.ByteSize();
	Q_ASSERT(len < 4090);

	* reinterpret_cast<quint16 *>(& uc[0]) = qToBigEndian(static_cast<quint16>(msgType));
	* reinterpret_cast<quint32 *>(& uc[2]) = qToBigEndian(static_cast<quint32>(len));

	msg.SerializeToArray(uc + 6, len);

	ssl->write(reinterpret_cast<const char *>(uc), len+6);
}

void Client::ping() {
	unsigned char buffer[256];
	buffer[0] = MessageHandler::UDPPing << 5;
	PacketDataStream pds(buffer + 1, 255);
	pds << 123;

	doUdp(buffer, pds.size() + 1);

	MumbleProto::Ping mpp;
	mpp.set_timestamp(123);
	sendMessage(mpp, MessageHandler::Ping);
}

void Client::sendVoice() {
	unsigned char buffer[1024];
	int len = 32 + (qrand() & 0x3f);

	// Regular voice, nothing special
	buffer[0] = 0;

	PacketDataStream ods(buffer + 1, 1024);
	ods << 1;
	ods.append(len);
	ods.skip(len);

	doUdp(buffer, ods.size() + 1);
}

void Client::doUdp(const unsigned char *buffer, int size) {
	if (! udp || ! crypt.isValid())
		return;

	unsigned char crypted[2048];

	crypt.encrypt(reinterpret_cast<const unsigned char *>(buffer), crypted, size);
	::sendto(socket, reinterpret_cast<const char *>(crypted), size+4, 0, reinterpret_cast<struct sockaddr *>(&srv), sizeof(srv));
}

void Client::run() {
	unsigned char buffer[1024];
	struct sockaddr_in addr;
	socklen_t sz;
	int len;

	if (! udp)
		return;

	forever {
		sz = sizeof(addr);
		len = ::recvfrom(socket, reinterpret_cast<char *>(buffer), 1024, 0, reinterpret_cast<struct sockaddr *>(&addr), &sz);
		if (len <= 0)
			break;
		if (len >= 32)
			rcvd++;
	}
}

void Client::readyRead() {
	forever {
		int avail = ssl->bytesAvailable();
		if (numbytes == -1) {
			if (avail < 6)
				break;
			unsigned char b[6];
			ssl->read(reinterpret_cast<char *>(b), 6);

			ptype = qFromBigEndian(* reinterpret_cast<quint16 *>(& b[0]));
			numbytes = qFromBigEndian(* reinterpret_cast<quint32 *>(& b[2]));

			avail = ssl->bytesAvailable();
		}
		if ((numbytes >= 0) && (avail >= numbytes)) {
			int want = numbytes;
			numbytes = -1;
			unsigned char buff[65536];
			Q_ASSERT(want < 65536);
			ssl->read(reinterpret_cast<char *>(buff), want);

			avail = ssl->bytesAvailable();

			switch (ptype) {
				case MessageHandler::CryptSetup: {
						MumbleProto::CryptSetup msg;
						if (! msg.ParseFromArray(buff, want))
							qFatal("Failed parse crypt");

						if (msg.has_key() && msg.has_client_nonce() && msg.has_server_nonce()) {
							const std::string &key = msg.key();
							const std::string &client_nonce = msg.client_nonce();
							const std::string &server_nonce = msg.server_nonce();
							if (key.size() == AES_BLOCK_SIZE && client_nonce.size() == AES_BLOCK_SIZE && server_nonce.size() == AES_BLOCK_SIZE)
								crypt.setKey(reinterpret_cast<const unsigned char *>(key.data()), reinterpret_cast<const unsigned char *>(client_nonce.data()), reinterpret_cast<const unsigned char *>(server_nonce.data()));
						} else if (msg.has_server_nonce()) {
							const std::string &server_nonce = msg.server_nonce();
							if (server_nonce.size() == AES_BLOCK_SIZE) {
								crypt.uiResync++;
								memcpy(crypt.decrypt_iv, server_nonce.data(), AES_BLOCK_SIZE);
							}
						} else {
							MumbleProto::CryptSetup mpcs;
							mpcs.set_client_nonce(std::string(reinterpret_cast<const char *>(crypt.encrypt_iv), AES_BLOCK_SIZE));
							sendMessage(mpcs, MessageHandler::CryptSetup);
						}
						break;
					}
				case MessageHandler::ServerSync: {
						MumbleProto::ServerSync msg;
						if (! msg.ParseFromArray(buff, want))
							qFatal("Failed parse sync");
						uiSession = msg.session();
						break;
					}
				case MessageHandler::UDPTunnel: {
						unsigned int msgUDPType = (buff[0] >> 5) & 0x7;
						if (msgUDPType == MessageHandler::UDPVoiceCELTAlpha)
							rcvd++;
						break;
					}
			}
		} else {
			break;
		}
	}
}

void Client::disconnected() {
	qWarning("SSL Socket disconnected");
	QCoreApplication::instance()->quit();
}

class Container : public QObject {
		Q_OBJECT
	public:
		int sent;
		int nsend, nudp, ntcp;
		int isend, iudp, itcp;
		bool live, forceping;
		QHostAddress qha;
		unsigned short port;
		QTimer qtTick;
		Timer tickPing, tickVoice, tickGo, tickSpawn;
		QList<Client *> speakers;
		QList<Client *> clients;
		Container(QHostAddress srvaddr, unsigned short port, int nsend, int nudp, int ntcp);
	public slots:
		void tick();
		void go();
};

Container::Container(QHostAddress qha, unsigned short port, int numsend, int numudp, int numtcp) {
	Timer t;

	qWarning("Spawning %d speakers and %d listeners (%d UDP, %d TCP)", numsend, numudp+numtcp, numudp, numtcp);

	nsend=numsend;
	nudp=numudp;
	ntcp=numtcp;

	isend=iudp=itcp=0;

	this->qha = qha;
	this->port = port;

	connect(&qtTick, SIGNAL(timeout()), this, SLOT(tick()));
	qtTick.start(0);

	live = false;
	forceping = false;

	tickSpawn.restart();
	sent = 0;

}

void Container::tick() {
	if (forceping || tickPing.isElapsed(5000000ULL)) {
		forceping = false;

		foreach(Client *c, clients)
			c->ping();

		if (live) {

			int lost = 0;
			quint64 totrcv = 0;
			int nrcv = 0;
			foreach(Client *c, clients) {
				if (! c->sender) {
					totrcv += c->rcvd;
					lost += sent - c->rcvd;
					nrcv++;
				}
			}
			qWarning("Sent: %8d  Rcvd: %8lld  Lost: %8d   BW: %6.1fMbit/s", sent, totrcv / nrcv, (lost + nrcv - 1) / nrcv, (totrcv * 8.0 * 123.0) / (tickGo.elapsed() * 1.0));
		} else {
			qWarning("Spawned %3d/%3d", isend+iudp+itcp,nsend+nudp+ntcp);
		}
	}

	if (live && tickVoice.isElapsed(10000ULL)) {
		foreach(Client *c, speakers) {
			sent++;
			c->sendVoice();
		}
	}
	if (! live) {
		if (isend < nsend) {
			Client *c = new Client(this, qha, port, true, false);
			speakers << c;
			c->start();
			clients << c;
			isend++;
		} else if (iudp < nudp) {
			Client *c = new Client(this, qha, port, false, false);
			c->start();
			clients << c;
			iudp++;
		} else if (itcp < ntcp) {
			Client *c = new Client(this, qha, port, false, true);
			c->start();
			clients << c;
			itcp++;
		} else {
			live = true;
			quint64 elapsed = tickSpawn.elapsed();
			qWarning("Spawning took %lld ms (%lld us per client)", elapsed / 1000ULL, elapsed / (nsend+nudp+ntcp));
			foreach(Client *c, clients)
				c->rcvd = 0;
			sent = 0;
			forceping = true;
			qtTick.start(10);
		}
	}
}

void Container::go() {
	foreach(Client *c, clients)
		c->start();
	qtTick.start(10);
	tickGo.restart();
}

int main(int argc, char **argv) {
	QCoreApplication a(argc, argv);

	qWarning("Maximum # sockets is %d", FD_SETSIZE);

	if (argc != 6)
		qFatal("Invalid # args");

	QHostAddress qha = QHostAddress(argv[1]);
	int port = atoi(argv[2]);
	int numsend = atoi(argv[3]);
	int numudp = atoi(argv[4]);
	int numtcp = atoi(argv[5]);

	Container c(qha, port, numsend, numudp, numtcp);
	c.go();
	a.exec();
}

#include "Benchmark.moc"
<|endoftext|>


<commit_before>#include "tests-base.hpp"

#include "helpers-casemapping.hpp"
#include "helpers-strings.hpp"

TEST(SpecialCasing, GreekFinalSigma)
{
	EXPECT_STREQ("lt-LT", setlocale(LC_ALL, "lt-LT"));

	// Special case for final form of sigma

	// GREEK CAPITAL LETTER SIGMA
	EXPECT_CASEMAPPING_EQ("\xE1\xBE\xB7\xCE\xA3", "\xE1\xBE\xB7\xCF\x82", "\xCE\x91\xCD\x82\xCE\x99\xCE\xA3", "\xCE\x91\xCD\x82\xCD\x85\xCE\xA3");

	// Note: the following cases for non-final are already in the UnicodeData.txt file.

	// GREEK CAPITAL LETTER SIGMA
	EXPECT_CASEMAPPING_EQ("\xCE\xA3", "\xCF\x83", "\xCE\xA3", "\xCE\xA3");

	// GREEK SMALL LETTER SIGMA
	EXPECT_CASEMAPPING_EQ("\xCF\x83", "\xCF\x83", "\xCE\xA3", "\xCE\xA3");

	// GREEK SMALL LETTER FINAL SIGMA
	EXPECT_CASEMAPPING_EQ("\xCF\x82", "\xCF\x82", "\xCE\xA3", "\xCE\xA3");

	setlocale(LC_ALL, "C");
}

TEST(SpecialCasing, LithuanianRemoveDotAbove)
{
	EXPECT_STREQ("lt-LT", setlocale(LC_ALL, "lt-LT"));

	// Remove DOT ABOVE after "i" with upper or titlecase

	// COMBINING DOT ABOVE
	EXPECT_CASEMAPPING_EQ("i\xCC\x87", "i\xCC\x87", "I", "I");

	setlocale(LC_ALL, "C");
}

TEST(SpecialCasing, LithuanianIntroduceExplicitDot)
{
	EXPECT_STREQ("lt-LT", setlocale(LC_ALL, "lt-LT"));

	// Introduce an explicit dot above when lowercasing capital I's and J's
	// whenever there are more accents above.
	// (of the accents used in Lithuanian: grave, acute, tilde above, and ogonek)

	// LATIN CAPITAL LETTER I
	EXPECT_CASEMAPPING_EQ("I", "i", "I", "I");

	// LATIN CAPITAL LETTER J
	EXPECT_CASEMAPPING_EQ("J", "j", "J", "J");

	// LATIN CAPITAL LETTER I WITH OGONEK
	EXPECT_CASEMAPPING_EQ("\xC4\xAE", "\xC4\xAF\xCC\x87", "\xC4\xAE", "\xC4\xAE");
	EXPECT_CASEMAPPING_EQ("I\xCC\xA8", "I\xCC\xA8\xCC\x87", "I\xCC\xA8", "I\xCC\xA8");

	// LATIN CAPITAL LETTER I WITH GRAVE
	EXPECT_CASEMAPPING_EQ("\xC3\x8C", "i\xCC\x87\xCC\x80", "\xC3\x8C", "\xC3\x8C");
	EXPECT_CASEMAPPING_EQ("I\xCC\x80", "i\xCC\x87\xCC\x80", "I\xCC\x80", "I\xCC\x80");

	// LATIN CAPITAL LETTER I WITH ACUTE
	EXPECT_CASEMAPPING_EQ("\xC3\x8D", "i\xCC\x87\xCC\x81", "\xC3\x8D", "\xC3\x8D");
	EXPECT_CASEMAPPING_EQ("I\xCC\x81", "i\xCC\x87\xCC\x81", "I\xCC\x81", "I\xCC\x81");

	// LATIN CAPITAL LETTER I WITH TILDE
	EXPECT_CASEMAPPING_EQ("\xC4\xA8", "i\xCC\x87\xCC\x83", "\xC4\xA8", "\xC4\xA8");
	EXPECT_CASEMAPPING_EQ("I\xCC\x83", "i\xCC\x87\xCC\x83", "I\xCC\x83", "I\xCC\x83");

	setlocale(LC_ALL, "C");
}

TEST(SpecialCasing, TurkishDotlessI)
{
	EXPECT_STREQ("tr-TR", setlocale(LC_ALL, "tr-TR"));

	// I and i-dotless; I-dot and i are case pairs in Turkish and Azeri
	// The following rules handle those cases.

	// LATIN CAPITAL LETTER I WITH DOT ABOVE
	EXPECT_CASEMAPPING_EQ("\xC4\xB0", "i", "\xC4\xB0", "\xC4\xB0");

	// When lowercasing, remove dot_above in the sequence I + dot_above, which will turn into i.
	// This matches the behavior of the canonically equivalent I-dot_above

	// COMBINING DOT ABOVE
	EXPECT_CASEMAPPING_EQ("I\xCC\x87", "i", "I\xCC\x87", "I\xCC\x87");

	// When lowercasing, unless an I is before a dot_above, it turns into a dotless i.

	// LATIN CAPITAL LETTER I
	EXPECT_CASEMAPPING_EQ("I", "\xC4\xB1", "I", "I");

	// When uppercasing, i turns into a dotted capital I

	// LATIN SMALL LETTER I
	EXPECT_CASEMAPPING_EQ("i", "i", "\xC4\xB1", "\xC4\xB1");

	// Note: the following case is already in the UnicodeData.txt file.

	// LATIN SMALL LETTER DOTLESS I
	EXPECT_CASEMAPPING_EQ("\xC4\xB1", "\xC4\xB1", "I", "I");

	setlocale(LC_ALL, "C");
}

TEST(SpecialCasing, AzeriDotlessI)
{
	EXPECT_STREQ("az", setlocale(LC_ALL, "az"));

	// I and i-dotless; I-dot and i are case pairs in Turkish and Azeri
	// The following rules handle those cases.

	// LATIN CAPITAL LETTER I WITH DOT ABOVE
	EXPECT_CASEMAPPING_EQ("\xC4\xB0", "i", "\xC4\xB0", "\xC4\xB0");

	// When lowercasing, remove dot_above in the sequence I + dot_above, which will turn into i.
	// This matches the behavior of the canonically equivalent I-dot_above

	// COMBINING DOT ABOVE
	EXPECT_CASEMAPPING_EQ("I\xCC\x87", "i", "I\xCC\x87", "I\xCC\x87");

	// When lowercasing, unless an I is before a dot_above, it turns into a dotless i.

	// LATIN CAPITAL LETTER I
	EXPECT_CASEMAPPING_EQ("I", "\xC4\xB1", "I", "I");

	// When uppercasing, i turns into a dotted capital I

	// LATIN SMALL LETTER I
	EXPECT_CASEMAPPING_EQ("i", "i", "\xC4\xB1", "\xC4\xB1");

	// Note: the following case is already in the UnicodeData.txt file.

	// LATIN SMALL LETTER DOTLESS I
	EXPECT_CASEMAPPING_EQ("\xC4\xB1", "\xC4\xB1", "I", "I");

	setlocale(LC_ALL, "C");
}<commit_msg>Fix special casing tests for POSIX systems.<commit_after>#include "tests-base.hpp"

#include "helpers-casemapping.hpp"
#include "helpers-strings.hpp"

TEST(SpecialCasing, Greek)
{
#if _WINDOWS
	EXPECT_STREQ("greek", setlocale(LC_ALL, "greek"));
#else
	EXPECT_STREQ("el_GR", setlocale(LC_ALL, "el_GR"));
#endif

	// Special case for final form of sigma

	// GREEK CAPITAL LETTER SIGMA
	EXPECT_CASEMAPPING_EQ("\xE1\xBE\xB7\xCE\xA3", "\xE1\xBE\xB7\xCF\x82", "\xCE\x91\xCD\x82\xCE\x99\xCE\xA3", "\xCE\x91\xCD\x82\xCD\x85\xCE\xA3");

	// Note: the following cases for non-final are already in the UnicodeData.txt file.

	// GREEK CAPITAL LETTER SIGMA
	EXPECT_CASEMAPPING_EQ("\xCE\xA3", "\xCF\x83", "\xCE\xA3", "\xCE\xA3");

	// GREEK SMALL LETTER SIGMA
	EXPECT_CASEMAPPING_EQ("\xCF\x83", "\xCF\x83", "\xCE\xA3", "\xCE\xA3");

	// GREEK SMALL LETTER FINAL SIGMA
	EXPECT_CASEMAPPING_EQ("\xCF\x82", "\xCF\x82", "\xCE\xA3", "\xCE\xA3");

	setlocale(LC_ALL, "C");
}

TEST(SpecialCasing, Lithuanian)
{
#if _WINDOWS
	EXPECT_STREQ("lithuanian", setlocale(LC_ALL, "lithuanian"));
#else
	EXPECT_STREQ("lt_LT", setlocale(LC_ALL, "lt_LT"));
#endif
	// Remove DOT ABOVE after "i" with upper or titlecase

	// COMBINING DOT ABOVE
	EXPECT_CASEMAPPING_EQ("i\xCC\x87", "i\xCC\x87", "I", "I");

	// Introduce an explicit dot above when lowercasing capital I's and J's
	// whenever there are more accents above.
	// (of the accents used in Lithuanian: grave, acute, tilde above, and ogonek)

	// LATIN CAPITAL LETTER I
	EXPECT_CASEMAPPING_EQ("I", "i", "I", "I");

	// LATIN CAPITAL LETTER J
	EXPECT_CASEMAPPING_EQ("J", "j", "J", "J");

	// LATIN CAPITAL LETTER I WITH OGONEK
	EXPECT_CASEMAPPING_EQ("\xC4\xAE", "\xC4\xAF\xCC\x87", "\xC4\xAE", "\xC4\xAE");
	EXPECT_CASEMAPPING_EQ("I\xCC\xA8", "I\xCC\xA8\xCC\x87", "I\xCC\xA8", "I\xCC\xA8");

	// LATIN CAPITAL LETTER I WITH GRAVE
	EXPECT_CASEMAPPING_EQ("\xC3\x8C", "i\xCC\x87\xCC\x80", "\xC3\x8C", "\xC3\x8C");
	EXPECT_CASEMAPPING_EQ("I\xCC\x80", "i\xCC\x87\xCC\x80", "I\xCC\x80", "I\xCC\x80");

	// LATIN CAPITAL LETTER I WITH ACUTE
	EXPECT_CASEMAPPING_EQ("\xC3\x8D", "i\xCC\x87\xCC\x81", "\xC3\x8D", "\xC3\x8D");
	EXPECT_CASEMAPPING_EQ("I\xCC\x81", "i\xCC\x87\xCC\x81", "I\xCC\x81", "I\xCC\x81");

	// LATIN CAPITAL LETTER I WITH TILDE
	EXPECT_CASEMAPPING_EQ("\xC4\xA8", "i\xCC\x87\xCC\x83", "\xC4\xA8", "\xC4\xA8");
	EXPECT_CASEMAPPING_EQ("I\xCC\x83", "i\xCC\x87\xCC\x83", "I\xCC\x83", "I\xCC\x83");

	setlocale(LC_ALL, "C");
}

TEST(SpecialCasing, Turkish)
{
#if _WINDOWS
	EXPECT_STREQ("turkish", setlocale(LC_ALL, "turkish"));
#else
	EXPECT_STREQ("tr_TR", setlocale(LC_ALL, "tr_TR"));
#endif

	// I and i-dotless; I-dot and i are case pairs in Turkish and Azeri
	// The following rules handle those cases.

	// LATIN CAPITAL LETTER I WITH DOT ABOVE
	EXPECT_CASEMAPPING_EQ("\xC4\xB0", "i", "\xC4\xB0", "\xC4\xB0");

	// When lowercasing, remove dot_above in the sequence I + dot_above, which will turn into i.
	// This matches the behavior of the canonically equivalent I-dot_above

	// COMBINING DOT ABOVE
	EXPECT_CASEMAPPING_EQ("I\xCC\x87", "i", "I\xCC\x87", "I\xCC\x87");

	// When lowercasing, unless an I is before a dot_above, it turns into a dotless i.

	// LATIN CAPITAL LETTER I
	EXPECT_CASEMAPPING_EQ("I", "\xC4\xB1", "I", "I");

	// When uppercasing, i turns into a dotted capital I

	// LATIN SMALL LETTER I
	EXPECT_CASEMAPPING_EQ("i", "i", "\xC4\xB1", "\xC4\xB1");

	// Note: the following case is already in the UnicodeData.txt file.

	// LATIN SMALL LETTER DOTLESS I
	EXPECT_CASEMAPPING_EQ("\xC4\xB1", "\xC4\xB1", "I", "I");

	setlocale(LC_ALL, "C");
}

TEST(SpecialCasing, Azeri)
{
#if _WINDOWS
	EXPECT_STREQ("azeri", setlocale(LC_ALL, "azeri"));
#else
	EXPECT_STREQ("az_AZ", setlocale(LC_ALL, "az_AZ"));
#endif

	// I and i-dotless; I-dot and i are case pairs in Turkish and Azeri
	// The following rules handle those cases.

	// LATIN CAPITAL LETTER I WITH DOT ABOVE
	EXPECT_CASEMAPPING_EQ("\xC4\xB0", "i", "\xC4\xB0", "\xC4\xB0");

	// When lowercasing, remove dot_above in the sequence I + dot_above, which will turn into i.
	// This matches the behavior of the canonically equivalent I-dot_above

	// COMBINING DOT ABOVE
	EXPECT_CASEMAPPING_EQ("I\xCC\x87", "i", "I\xCC\x87", "I\xCC\x87");

	// When lowercasing, unless an I is before a dot_above, it turns into a dotless i.

	// LATIN CAPITAL LETTER I
	EXPECT_CASEMAPPING_EQ("I", "\xC4\xB1", "I", "I");

	// When uppercasing, i turns into a dotted capital I

	// LATIN SMALL LETTER I
	EXPECT_CASEMAPPING_EQ("i", "i", "\xC4\xB1", "\xC4\xB1");

	// Note: the following case is already in the UnicodeData.txt file.

	// LATIN SMALL LETTER DOTLESS I
	EXPECT_CASEMAPPING_EQ("\xC4\xB1", "\xC4\xB1", "I", "I");

	setlocale(LC_ALL, "C");
}<|endoftext|>


<commit_before>#include "gtest/gtest.h"
#include "src/common/Player.h"

TEST(Player, SubstractScore) {
    common::Player player("SpiderMan");
    player.SubstractScore(3);
    EXPECT_EQ(player.score(), -3);
}
<commit_msg>Modify Player test.<commit_after>#include "gtest/gtest.h"
#include "src/common/Player.h"

TEST(Player, SubstractScore) {
    common::Player player("SpiderMan");
    player.SubstractScore(3);
    EXPECT_EQ(player.GetScore(), -3);
}
<|endoftext|>


<commit_before>/*
* TLS Channels
* (C) 2011 Jack Lloyd
*
* Released under the terms of the Botan license
*/

#include <botan/tls_channel.h>
#include <botan/internal/tls_alerts.h>
#include <botan/internal/tls_state.h>
#include <botan/loadstor.h>

namespace Botan {

TLS_Channel::TLS_Channel(std::tr1::function<void (const byte[], size_t)> socket_output_fn,
                         std::tr1::function<void (const byte[], size_t, u16bit)> proc_fn) :
   proc_fn(proc_fn),
   writer(socket_output_fn),
   state(0),
   active(false)
   {
   }

TLS_Channel::~TLS_Channel()
   {
   close();
   delete state;
   state = 0;
   }

size_t TLS_Channel::received_data(const byte buf[], size_t buf_size)
   {
   try
      {
      reader.add_input(buf, buf_size);

      byte rec_type = CONNECTION_CLOSED;
      SecureVector<byte> record;

      while(!reader.currently_empty())
         {
         const size_t bytes_needed = reader.get_record(rec_type, record);

         if(bytes_needed > 0)
            return bytes_needed;

         if(rec_type == APPLICATION_DATA)
            {
            if(active)
               {
               /*
               * OpenSSL among others sends empty records in versions
               * before TLS v1.1 in order to randomize the IV of the
               * following record. Avoid spurious callbacks.
               */
               if(record.size() > 0)
                  proc_fn(&record[0], record.size(), NO_ALERT_TYPE);
               }
            else
               {
               throw Unexpected_Message("Application data before handshake done");
               }
            }
         else if(rec_type == HANDSHAKE || rec_type == CHANGE_CIPHER_SPEC)
            {
            read_handshake(rec_type, record);
            }
         else if(rec_type == ALERT)
            {
            Alert alert_msg(record);

            proc_fn(0, 0, alert_msg.type());

            if(alert_msg.is_fatal() || alert_msg.type() == CLOSE_NOTIFY)
               {
               if(alert_msg.type() == CLOSE_NOTIFY)
                  alert(FATAL, CLOSE_NOTIFY);
               else
                  alert(FATAL, NO_ALERT_TYPE);
               }
            }
         else
            throw Unexpected_Message("Unknown message type received");
         }

      return 0; // on a record boundary
      }
   catch(TLS_Exception& e)
      {
      alert(FATAL, e.type());
      throw;
      }
   catch(std::exception& e)
      {
      alert(FATAL, INTERNAL_ERROR);
      throw;
      }
   }

/*
* Split up and process handshake messages
*/
void TLS_Channel::read_handshake(byte rec_type,
                                 const MemoryRegion<byte>& rec_buf)
   {
   if(rec_type == HANDSHAKE)
      state->queue.write(&rec_buf[0], rec_buf.size());

   while(true)
      {
      Handshake_Type type = HANDSHAKE_NONE;
      SecureVector<byte> contents;

      if(rec_type == HANDSHAKE)
         {
         if(state->queue.size() >= 4)
            {
            byte head[4] = { 0 };
            state->queue.peek(head, 4);

            const size_t length = make_u32bit(0, head[1], head[2], head[3]);

            if(state->queue.size() >= length + 4)
               {
               type = static_cast<Handshake_Type>(head[0]);
               contents.resize(length);
               state->queue.read(head, 4);
               state->queue.read(&contents[0], contents.size());
               }
            }
         }
      else if(rec_type == CHANGE_CIPHER_SPEC)
         {
         if(state->queue.size() == 0 && rec_buf.size() == 1 && rec_buf[0] == 1)
            type = HANDSHAKE_CCS;
         else
            throw Decoding_Error("Malformed ChangeCipherSpec message");
         }
      else
         throw Decoding_Error("Unknown message type in handshake processing");

      if(type == HANDSHAKE_NONE)
         break;

      process_handshake_msg(type, contents);

      if(type == HANDSHAKE_CCS || !state)
         break;
      }
   }

void TLS_Channel::queue_for_sending(const byte buf[], size_t buf_size)
   {
   if(active)
      {
      while(!pre_handshake_write_queue.end_of_data())
         {
         SecureVector<byte> q_buf(1024);
         const size_t got = pre_handshake_write_queue.read(&q_buf[0], q_buf.size());
         writer.send(APPLICATION_DATA, &q_buf[0], got);
         }

      writer.send(APPLICATION_DATA, buf, buf_size);
      writer.flush();
      }
   else
      pre_handshake_write_queue.write(buf, buf_size);
   }

void TLS_Channel::alert(Alert_Level level, Alert_Type alert_code)
   {
   if(active && alert_code != NO_ALERT_TYPE)
      {
      try
         {
         writer.alert(level, alert_code);
         writer.flush();
         }
      catch(...) { /* swallow it */ }
      }

   if(active && level == FATAL)
      {
      reader.reset();
      writer.reset();
      delete state;
      state = 0;
      active = false;
      }
   }

}
<commit_msg>We wouldn't send an alert before handshaking was complete because active == false, which made debugging hard and caused timeouts/hangs in clients if (for instance) a ciphersuite couldn't be negotiated. Always send alerts.<commit_after>/*
* TLS Channels
* (C) 2011 Jack Lloyd
*
* Released under the terms of the Botan license
*/

#include <botan/tls_channel.h>
#include <botan/internal/tls_alerts.h>
#include <botan/internal/tls_state.h>
#include <botan/loadstor.h>

namespace Botan {

TLS_Channel::TLS_Channel(std::tr1::function<void (const byte[], size_t)> socket_output_fn,
                         std::tr1::function<void (const byte[], size_t, u16bit)> proc_fn) :
   proc_fn(proc_fn),
   writer(socket_output_fn),
   state(0),
   active(false)
   {
   }

TLS_Channel::~TLS_Channel()
   {
   close();
   delete state;
   state = 0;
   }

size_t TLS_Channel::received_data(const byte buf[], size_t buf_size)
   {
   try
      {
      reader.add_input(buf, buf_size);

      byte rec_type = CONNECTION_CLOSED;
      SecureVector<byte> record;

      while(!reader.currently_empty())
         {
         const size_t bytes_needed = reader.get_record(rec_type, record);

         if(bytes_needed > 0)
            return bytes_needed;

         if(rec_type == APPLICATION_DATA)
            {
            if(active)
               {
               /*
               * OpenSSL among others sends empty records in versions
               * before TLS v1.1 in order to randomize the IV of the
               * following record. Avoid spurious callbacks.
               */
               if(record.size() > 0)
                  proc_fn(&record[0], record.size(), NO_ALERT_TYPE);
               }
            else
               {
               throw Unexpected_Message("Application data before handshake done");
               }
            }
         else if(rec_type == HANDSHAKE || rec_type == CHANGE_CIPHER_SPEC)
            {
            read_handshake(rec_type, record);
            }
         else if(rec_type == ALERT)
            {
            Alert alert_msg(record);

            proc_fn(0, 0, alert_msg.type());

            if(alert_msg.is_fatal() || alert_msg.type() == CLOSE_NOTIFY)
               {
               if(alert_msg.type() == CLOSE_NOTIFY)
                  alert(FATAL, CLOSE_NOTIFY);
               else
                  alert(FATAL, NO_ALERT_TYPE);
               }
            }
         else
            throw Unexpected_Message("Unknown message type received");
         }

      return 0; // on a record boundary
      }
   catch(TLS_Exception& e)
      {
      alert(FATAL, e.type());
      throw;
      }
   catch(std::exception& e)
      {
      alert(FATAL, INTERNAL_ERROR);
      throw;
      }
   }

/*
* Split up and process handshake messages
*/
void TLS_Channel::read_handshake(byte rec_type,
                                 const MemoryRegion<byte>& rec_buf)
   {
   if(rec_type == HANDSHAKE)
      state->queue.write(&rec_buf[0], rec_buf.size());

   while(true)
      {
      Handshake_Type type = HANDSHAKE_NONE;
      SecureVector<byte> contents;

      if(rec_type == HANDSHAKE)
         {
         if(state->queue.size() >= 4)
            {
            byte head[4] = { 0 };
            state->queue.peek(head, 4);

            const size_t length = make_u32bit(0, head[1], head[2], head[3]);

            if(state->queue.size() >= length + 4)
               {
               type = static_cast<Handshake_Type>(head[0]);
               contents.resize(length);
               state->queue.read(head, 4);
               state->queue.read(&contents[0], contents.size());
               }
            }
         }
      else if(rec_type == CHANGE_CIPHER_SPEC)
         {
         if(state->queue.size() == 0 && rec_buf.size() == 1 && rec_buf[0] == 1)
            type = HANDSHAKE_CCS;
         else
            throw Decoding_Error("Malformed ChangeCipherSpec message");
         }
      else
         throw Decoding_Error("Unknown message type in handshake processing");

      if(type == HANDSHAKE_NONE)
         break;

      process_handshake_msg(type, contents);

      if(type == HANDSHAKE_CCS || !state)
         break;
      }
   }

void TLS_Channel::queue_for_sending(const byte buf[], size_t buf_size)
   {
   if(active)
      {
      while(!pre_handshake_write_queue.end_of_data())
         {
         SecureVector<byte> q_buf(1024);
         const size_t got = pre_handshake_write_queue.read(&q_buf[0], q_buf.size());
         writer.send(APPLICATION_DATA, &q_buf[0], got);
         }

      writer.send(APPLICATION_DATA, buf, buf_size);
      writer.flush();
      }
   else
      pre_handshake_write_queue.write(buf, buf_size);
   }

void TLS_Channel::alert(Alert_Level level, Alert_Type alert_code)
   {
   try
      {
      writer.alert(level, alert_code);
      writer.flush();
      }
   catch(...) { /* swallow it */ }

   if(active && level == FATAL)
      {
      reader.reset();
      writer.reset();
      delete state;
      state = 0;
      active = false;
      }
   }

}
<|endoftext|>


<commit_before>/*
 * Copyright 2014 National ICT Australia Limited (NICTA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <cxxtest/TestSuite.h>
#include "testmodel.hpp"
#include "../src/inner_constraint.hpp"

using namespace MadOpt;

class EConstraintTest: public CxxTest::TestSuite {
    public:

        void Tes(Expr exp,
                const vector<double> x,
                const double g,
                const vector<Idx> jac_entries,
                const vector<double> jac,
                const vector<PII> hess_entries={},
                const vector<double> hess={},
                const double delta=0.000001
                ){
            HessPosMap hess_pos_map;
            TestModel m;
            auto& simstack = m.getSimStack();
            simstack.setXSize(x.size());
            InnerConstraint e(exp, 0, 0, hess_pos_map, simstack);

            map<int, double> jacvm;
            for (Idx i=0; i<jac_entries.size(); i++)
                jacvm[jac_entries[i]] = jac[i];

            map<PII, double> hessvm;
            for (Idx i=0; i<hess_entries.size(); i++)
                hessvm[hess_entries[i]] = hess[i];

            const auto& ejace = e.getJacEntries();

            auto& cstack = m.getCStack();
            cstack.resize(simstack);
            cstack.setX(x.data());
            e.setEvals(cstack);

            map<int, double> ej;
            auto ejac = e.getJac();

            for (Idx i=0; i<e.getNNZ_Jac(); i++)
                ej[ejace[i]] = ejac[i];

            vector<double> ehess(hess_pos_map.size(), 0);
            auto hess_map = e.getHessMap();
            auto hess_res = e.getHess();
            int i=0;
            for (auto x: hess_map)
                ehess[x] += hess_res[i++];

            map<PII, double> eh;
            for (auto p: hess_pos_map)
                eh[p.first] = ehess[p.second];

            TS_ASSERT_DELTA(e.getG(), g, delta);
            TS_ASSERT_EQUALS(ej.size(), jacvm.size());
            TS_ASSERT_EQUALS(eh.size(), hessvm.size());

            if (delta == 0){
                TS_ASSERT_EQUALS(ej, jacvm);
                TS_ASSERT_EQUALS(eh, hessvm);
            } else {

                for (auto p: ej)
                    TS_ASSERT_DELTA(p.second, jacvm.at(p.first), delta);

                for (auto p: eh)
                    TS_ASSERT_DELTA(p.second, hessvm.at(p.first), delta);
            }
        }

        void testVar(){
            TestModel m;
            Var a = m.addVar("a");
            Tes(a, {3}, 3, {0}, {1});
        }

        void testParam(){
            TestModel m;
            Var a = m.addVar("a");
            Param x = m.addParam(3, "x");
            Tes(x*a, {2}, 6, {0}, {3});
            x.value(4);
            Tes(x*a, {2}, 8, {0}, {4});
        }

        void testADD(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            Var c = m.addVar("c");
            Tes(a+b, {2, 3}, 2+3, {0, 1}, {1,1});
            Tes(a+b+c, {2, 3, 4}, 2+3+4, {0, 1, 2}, {1,1,1});
        }

        void testMUL(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            //Tes(2*a, {3}, 6, {0}, {2});
            //Tes(a*b, {2,3}, 2*3, {0,1}, {3,2}, {PII(0,1)}, {1});
            double ax = 3;
            double bx = 5;
            Tes(a*a*b, {ax,bx},
                ax*ax*bx,
                {0,1}, {2*ax*bx, ax*ax},
                {PII(0,0), PII(0,1)}, {2*bx, 2*ax});
        }

        void testCOS(){
            TestModel m;
            Var a = m.addVar("a");
            Tes(cos(2*a), {3}, cos(2*3), {0}, {-2*sin(6)}, {PII(0,0)}, {-4*cos(6)});
        }

  void testLN(){
    TestModel m;
    Var a = m.addVar("a");
    Tes(ln(2*a), {3}, std::log(2*3), {0}, {2.0/6.0}, {PII(0,0)}, {-1.0/9.0});
  }

  void testLOG2(){
    TestModel m;
    Var a = m.addVar("a");
    Tes(log2(2*a), {3}, std::log2(2*3), {0}, {2.0/(6*log(2))}, {PII(0,0)}, {-2.0*2.0*log(2)/pow(2*3*log(2), 2)});
  }

        void testCOS2(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            Tes(cos(a*b), {3, 2}, cos(2*3),
                    {0, 1}, {-2*sin(6), -3*sin(6)},
                    {PII(0,0), PII(0,1), PII(1,1)},
                    {-4*cos(6), -sin(6)-6*cos(6), -9*cos(6)});
        }

        void testSIN(){
            TestModel m;
            Var a = m.addVar("a");
            Tes(sin(2*a), {3}, sin(2*3), {0}, {2*cos(6)}, {PII(0,0)}, {-4*sin(6)});
        }

        void testPOW(){
            TestModel m;
            Var a = m.addVar("a");
            Tes(pow(a,2), {3}, pow(3,2), {0}, {2*3}, {PII(0,0)}, {2});
            Tes(pow(2*a, 3), {3}, pow(6,3), {0}, {3*2*pow(2*3,2)}, {PII(0,0)}, {3*2*2*2*pow(2*3,1)});
        }

        void testMulti(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            Var c = m.addVar("c");
            Tes(2*a+b, {2, 3}, 2*2+3, {0,1}, {2,1});
            Tes(c*a+b, {2,3,4}, 4*2+3, {0,1,2}, {4,1,2}, {PII(0,2)}, {1});
            double ax = 3;
            double bx = 2;
            Tes((2*a+1.5)*b, {ax, bx}, (2*ax+1.5)*bx,
                {0,1}, {2*bx, 2*ax+1.5}, {PII(0,1)}, {2});
        }

        void testBug(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");

            double ax = 3;
            double bx = 2;
            double v = 3.0/100.0;

            Tes((a*a + 1.5*a - v)*b, {ax,bx},
                (ax*ax + 1.5*ax-v)*bx,
                {0,1}, {(2*ax+1.5)*bx, ax*ax+1.5*ax-v},
                {PII(0,0), PII(0,1)}, {2*bx, 2*ax+1.5});
        }

        void testTutorialTerm(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            Var c = m.addVar("c");

            double ax = 3.1;
            double bx = 2;
            double cx = 1;
            double v = 3.0/100;

            Tes((pow(a,2) + 1.5*a - v)*cos(b)-c, {ax,bx,cx},
                (pow(ax,2)+1.5*ax-v)*cos(bx)-cx,
                {0,1,2}, {(2*ax+1.5)*cos(bx), -(pow(ax,2)+1.5*ax-v)*sin(bx), -1},
                {PII(0,0), PII(0,1), PII(1,1)},
                {cos(bx)*2, -sin(bx)*(2*ax+1.5), -(pow(ax,2)+1.5*ax-v)*cos(bx)});
        }

        void testCaseADD(){
            for (int N=1; N<13; N++){
                TestModel m;
                Expr term(0);
                vector<double> x(N);
                vector<Idx> jac_entries(N);
                vector<double> jac(N);
                vector<PII> hess_entries(N);
                vector<double> hess(N);
                double g = 0;
                for (int i=0; i < N; ++i) {
                    term += pow(m.addVar("x"), 2);
                    x[i] = i;
                    g += i*i;
                    jac_entries[i] = i;
                    jac[i] = 2*i;
                    hess_entries[i] = PII(i,i);
                    hess[i] = 2;
                }

                Tes(term, x, g, jac_entries, jac, hess_entries, hess);
            }
        }

        void testLargeSum(){
            int N = 4;
            TestModel m;
            Expr term(0);
            vector<double> x(N);
            vector<Idx> jac_entries(N);
            vector<double> jac(N);
            vector<PII> hess_entries(N);
            vector<double> hess(N);
            double g = 0;
            for (int i=0; i < N; ++i) {
                term += pow(m.addVar("x" + std::to_string(i)), 2);
                x[i] = i;
                g += i*i;
                jac_entries[i] = i;
                jac[i] = 2*i;
                hess_entries[i] = PII(i,i);
                hess[i] = 2;
            }

            Tes(term, x, g, jac_entries, jac, hess_entries, hess);
        }

        void testLargeSum2(){
            int N = 4;
            TestModel m;
            vector<Var> vars(N);
            vector<double> x(N);
            vector<Idx> jac_entries(N);
            vector<double> jac(N);
            vector<PII> hess_entries(2*(N-1)+1);
            vector<double> hess(2*(N-1)+1);

            for (int i=0; i < N; ++i) {
                vars[i] = m.addVar("x");
                x[i] = i;
                jac_entries[i] = i;
            }

            Expr term(0);
            double g = 0;
            Idx hess_counter = 0;
            for (int i=0; i < N-1; ++i) {
                term += pow(vars[i] + vars[i+1], 2);
                g += (2*i+1)*(2*i+1);
            }

            hess_entries[hess_counter] = PII(0,0);
            hess[hess_counter++] = 2;
            hess_entries[hess_counter] = PII(0,1);
            hess[hess_counter++] = 2;
            jac[0] = 2*(0 + 1);
            for (int i=1; i < N-1; ++i) {
                jac[i] = 2*(i+i+1) + 2*(i-1+i);
                hess_entries[hess_counter] = PII(i,i);
                hess[hess_counter++] = 2+2;
                hess_entries[hess_counter] = PII(i,i+1);
                hess[hess_counter++] = 2;
            }
            jac[N-1] = 2*(N-2 + N-1);
            hess_entries[hess_counter] = PII(N-1,N-1);
            hess[hess_counter++] = 2;

            Tes(term, x, g, jac_entries, jac, hess_entries, hess);
        }

	void testXimesXTimesX(){
            TestModel m;
            Var x = m.addVar("x");
            Tes(x*x*x, {1}, 1, {0}, {3}, {PII(0,0)}, {6});
        }
};

<commit_msg>better var naming<commit_after>/*
 * Copyright 2014 National ICT Australia Limited (NICTA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <cxxtest/TestSuite.h>
#include "testmodel.hpp"
#include "../src/inner_constraint.hpp"

using namespace MadOpt;

class EConstraintTest: public CxxTest::TestSuite {
    public:

        void Tes(Expr exp,
                const vector<double> x,
                const double g,
                const vector<Idx> expected_jac_entries,
                const vector<double> expected_jac_values,
                const vector<PII> expected_hess_entries={},
                const vector<double> expected_hess_values={},
                const double delta=0.000001
                ){
            HessPosMap hess_pos_map;
            TestModel m;
            auto& simstack = m.getSimStack();
            simstack.setXSize(x.size());
            InnerConstraint constraint(exp, 0, 0, hess_pos_map, simstack);

            auto& cstack = m.getCStack();
            cstack.resize(simstack);
            cstack.setX(x.data());
            constraint.setEvals(cstack);

            map<int, double> expected_jac_map;
            for (Idx i=0; i<expected_jac_entries.size(); i++)
                expected_jac_map[expected_jac_entries[i]] = expected_jac_values[i];
	    const auto& real_jac_entries = constraint.getJacEntries();
            map<int, double> real_jac_map;
            auto real_jac_values = constraint.getJac();
            for (Idx i=0; i<constraint.getNNZ_Jac(); i++)
                real_jac_map[real_jac_entries[i]] = real_jac_values[i];

	    map<PII, double> expected_hess_map;
            for (Idx i=0; i<expected_hess_entries.size(); i++)
                expected_hess_map[expected_hess_entries[i]] = expected_hess_values[i];
	    const auto& real_hess_entries_as_index = constraint.getHessMap();
            vector<double> real_hess_values(hess_pos_map.size(), 0);
            auto real_hess_values_for_index = constraint.getHess();
            int i=0;
            for (auto id: real_hess_entries_as_index)
                real_hess_values[id] += real_hess_values_for_index[i++];
            map<PII, double> real_hess_map;
            for (auto p: hess_pos_map)
                real_hess_map[p.first] = real_hess_values[p.second];

            TS_ASSERT_DELTA(constraint.getG(), g, delta);

            TS_ASSERT_EQUALS(real_jac_entries.size(), real_jac_values.size());
            TS_ASSERT_EQUALS(real_jac_entries.size(), expected_jac_entries.size());
            TS_ASSERT_EQUALS(real_jac_values.size(), expected_jac_values.size());

            TS_ASSERT_EQUALS(real_hess_entries_as_index.size(), real_hess_values_for_index.size());
            TS_ASSERT_EQUALS(real_hess_entries_as_index.size(), expected_hess_entries.size());
            TS_ASSERT_EQUALS(real_hess_values_for_index.size(), expected_hess_values.size());
	    TS_ASSERT_EQUALS(hess_pos_map.size(), expected_hess_entries.size());

            if (delta == 0){
                TS_ASSERT_EQUALS(real_jac_map, expected_jac_map);
                TS_ASSERT_EQUALS(real_hess_map, expected_hess_map);
            } else {

                for (auto p: real_jac_map)
                    TS_ASSERT_DELTA(p.second, expected_jac_map.at(p.first), delta);

                for (auto p: real_hess_map)
                    TS_ASSERT_DELTA(p.second, expected_hess_map.at(p.first), delta);
            }
        }

        void testVar(){
            TestModel m;
            Var a = m.addVar("a");
            Tes(a, {3}, 3, {0}, {1});
        }

        void testParam(){
            TestModel m;
            Var a = m.addVar("a");
            Param x = m.addParam(3, "x");
            Tes(x*a, {2}, 6, {0}, {3});
            x.value(4);
            Tes(x*a, {2}, 8, {0}, {4});
        }

        void testADD(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            Var c = m.addVar("c");
            Tes(a+b, {2, 3}, 2+3, {0, 1}, {1,1});
            Tes(a+b+c, {2, 3, 4}, 2+3+4, {0, 1, 2}, {1,1,1});
        }

        void testMUL(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            //Tes(2*a, {3}, 6, {0}, {2});
            //Tes(a*b, {2,3}, 2*3, {0,1}, {3,2}, {PII(0,1)}, {1});
            double ax = 3;
            double bx = 5;
            Tes(a*a*b, {ax,bx},
                ax*ax*bx,
                {0,1}, {2*ax*bx, ax*ax},
                {PII(0,0), PII(0,1)}, {2*bx, 2*ax});
        }

        void testCOS(){
            TestModel m;
            Var a = m.addVar("a");
            Tes(cos(2*a), {3}, cos(2*3), {0}, {-2*sin(6)}, {PII(0,0)}, {-4*cos(6)});
        }

  void testLN(){
    TestModel m;
    Var a = m.addVar("a");
    Tes(ln(2*a), {3}, std::log(2*3), {0}, {2.0/6.0}, {PII(0,0)}, {-1.0/9.0});
  }

  void testLOG2(){
    TestModel m;
    Var a = m.addVar("a");
    Tes(log2(2*a), {3}, std::log2(2*3), {0}, {2.0/(6*log(2))}, {PII(0,0)}, {-2.0*2.0*log(2)/pow(2*3*log(2), 2)});
  }

        void testCOS2(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            Tes(cos(a*b), {3, 2}, cos(2*3),
                    {0, 1}, {-2*sin(6), -3*sin(6)},
                    {PII(0,0), PII(0,1), PII(1,1)},
                    {-4*cos(6), -sin(6)-6*cos(6), -9*cos(6)});
        }

        void testSIN(){
            TestModel m;
            Var a = m.addVar("a");
            Tes(sin(2*a), {3}, sin(2*3), {0}, {2*cos(6)}, {PII(0,0)}, {-4*sin(6)});
        }

        void testPOW(){
            TestModel m;
            Var a = m.addVar("a");
            Tes(pow(a,2), {3}, pow(3,2), {0}, {2*3}, {PII(0,0)}, {2});
            Tes(pow(2*a, 3), {3}, pow(6,3), {0}, {3*2*pow(2*3,2)}, {PII(0,0)}, {3*2*2*2*pow(2*3,1)});
        }

        void testMulti(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            Var c = m.addVar("c");
            Tes(2*a+b, {2, 3}, 2*2+3, {0,1}, {2,1});
            Tes(c*a+b, {2,3,4}, 4*2+3, {0,1,2}, {4,1,2}, {PII(0,2)}, {1});
            double ax = 3;
            double bx = 2;
            Tes((2*a+1.5)*b, {ax, bx}, (2*ax+1.5)*bx,
                {0,1}, {2*bx, 2*ax+1.5}, {PII(0,1)}, {2});
        }

        void testBug(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");

            double ax = 3;
            double bx = 2;
            double v = 3.0/100.0;

            Tes((a*a + 1.5*a - v)*b, {ax,bx},
                (ax*ax + 1.5*ax-v)*bx,
                {0,1}, {(2*ax+1.5)*bx, ax*ax+1.5*ax-v},
                {PII(0,0), PII(0,1)}, {2*bx, 2*ax+1.5});
        }

        void testTutorialTerm(){
            TestModel m;
            Var a = m.addVar("a");
            Var b = m.addVar("b");
            Var c = m.addVar("c");

            double ax = 3.1;
            double bx = 2;
            double cx = 1;
            double v = 3.0/100;

            Tes((pow(a,2) + 1.5*a - v)*cos(b)-c, {ax,bx,cx},
                (pow(ax,2)+1.5*ax-v)*cos(bx)-cx,
                {0,1,2}, {(2*ax+1.5)*cos(bx), -(pow(ax,2)+1.5*ax-v)*sin(bx), -1},
                {PII(0,0), PII(0,1), PII(1,1)},
                {cos(bx)*2, -sin(bx)*(2*ax+1.5), -(pow(ax,2)+1.5*ax-v)*cos(bx)});
        }

        void testCaseADD(){
            for (int N=1; N<13; N++){
                TestModel m;
                Expr term(0);
                vector<double> x(N);
                vector<Idx> jac_entries(N);
                vector<double> jac(N);
                vector<PII> hess_entries(N);
                vector<double> hess(N);
                double g = 0;
                for (int i=0; i < N; ++i) {
                    term += pow(m.addVar("x"), 2);
                    x[i] = i;
                    g += i*i;
                    jac_entries[i] = i;
                    jac[i] = 2*i;
                    hess_entries[i] = PII(i,i);
                    hess[i] = 2;
                }

                Tes(term, x, g, jac_entries, jac, hess_entries, hess);
            }
        }

        void testLargeSum(){
            int N = 4;
            TestModel m;
            Expr term(0);
            vector<double> x(N);
            vector<Idx> jac_entries(N);
            vector<double> jac(N);
            vector<PII> hess_entries(N);
            vector<double> hess(N);
            double g = 0;
            for (int i=0; i < N; ++i) {
                term += pow(m.addVar("x" + std::to_string(i)), 2);
                x[i] = i;
                g += i*i;
                jac_entries[i] = i;
                jac[i] = 2*i;
                hess_entries[i] = PII(i,i);
                hess[i] = 2;
            }

            Tes(term, x, g, jac_entries, jac, hess_entries, hess);
        }

        void testLargeSum2(){
            int N = 4;
            TestModel m;
            vector<Var> vars(N);
            vector<double> x(N);
            vector<Idx> jac_entries(N);
            vector<double> jac(N);
            vector<PII> hess_entries(2*(N-1)+1);
            vector<double> hess(2*(N-1)+1);

            for (int i=0; i < N; ++i) {
                vars[i] = m.addVar("x");
                x[i] = i;
                jac_entries[i] = i;
            }

            Expr term(0);
            double g = 0;
            Idx hess_counter = 0;
            for (int i=0; i < N-1; ++i) {
                term += pow(vars[i] + vars[i+1], 2);
                g += (2*i+1)*(2*i+1);
            }

            hess_entries[hess_counter] = PII(0,0);
            hess[hess_counter++] = 2;
            hess_entries[hess_counter] = PII(0,1);
            hess[hess_counter++] = 2;
            jac[0] = 2*(0 + 1);
            for (int i=1; i < N-1; ++i) {
                jac[i] = 2*(i+i+1) + 2*(i-1+i);
                hess_entries[hess_counter] = PII(i,i);
                hess[hess_counter++] = 2+2;
                hess_entries[hess_counter] = PII(i,i+1);
                hess[hess_counter++] = 2;
            }
            jac[N-1] = 2*(N-2 + N-1);
            hess_entries[hess_counter] = PII(N-1,N-1);
            hess[hess_counter++] = 2;

            Tes(term, x, g, jac_entries, jac, hess_entries, hess);
        }

	void testXimesXTimesX(){
            TestModel m;
            Var x = m.addVar("x");
            Tes(x*x*x, {1}, 1, {0}, {3}, {PII(0,0)}, {6});
        }
};

<|endoftext|>


<commit_before>#pragma once

#include "xml_doc.hpp"

#include "libxml2_error_handlers.hpp"
#include <libxml/xmlschemas.h>

#include <iosfwd>
#include <memory>

class FreeXmlSchemaParser {
public:
    auto operator( )( xmlSchemaParserCtxt *schemaParserCtxt ) const -> void {
        xmlSchemaFreeParserCtxt( schemaParserCtxt );
    }
};

class XmlSchemaParser {
public:
    XmlSchemaParser( );
    explicit XmlSchemaParser( const XmlDoc &xml );
    XmlSchemaParser( const XmlSchemaParser & ) = delete;
    XmlSchemaParser &operator=( const XmlSchemaParser & ) = delete;
    auto get( ) const -> xmlSchemaParserCtxt * { return spc_.get( ); }
    friend auto operator>>( const XmlDoc &xml, XmlSchemaParser &xmlSchemaParser )
        -> XmlSchemaParser &;
    auto errorHandler( ) const -> const IErrorHandler & { return spch_; }

private:
    using SchemaParserCtxtT = std::unique_ptr<xmlSchemaParserCtxt, FreeXmlSchemaParser>;
    SchemaParserCtxtT spc_;
    XmlSchemaParserErrorHandler spch_;
};

<commit_msg>copy move constructor and copy assignment explicitly removed<commit_after>#pragma once

#include "xml_doc.hpp"

#include "libxml2_error_handlers.hpp"
#include <libxml/xmlschemas.h>

#include <iosfwd>
#include <memory>

class FreeXmlSchemaParser {
public:
    auto operator( )( xmlSchemaParserCtxt *schemaParserCtxt ) const -> void {
        xmlSchemaFreeParserCtxt( schemaParserCtxt );
    }
};

class XmlSchemaParser {
public:
    XmlSchemaParser( );
    explicit XmlSchemaParser( const XmlDoc &xml );
    XmlSchemaParser( const XmlSchemaParser & ) = delete;
    XmlSchemaParser &operator=( const XmlSchemaParser & ) = delete;
    XmlSchemaParser( XmlSchemaParser&&) = delete;
    XmlSchemaParser &operator=( XmlSchemaParser && ) = delete;
    auto get( ) const -> xmlSchemaParserCtxt * { return spc_.get( ); }
    friend auto operator>>( const XmlDoc &xml, XmlSchemaParser &xmlSchemaParser )
        -> XmlSchemaParser &;
    auto errorHandler( ) const -> const IErrorHandler & { return spch_; }

private:
    using SchemaParserCtxtT = std::unique_ptr<xmlSchemaParserCtxt, FreeXmlSchemaParser>;
    SchemaParserCtxtT spc_;
    XmlSchemaParserErrorHandler spch_;
};

<|endoftext|>


<commit_before>/*
Copyright (c) 2016-2017 stoyan shopov

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "libtroll.hxx"

#define TYPE_DEBUG_ENABLED	0
/* the first node in the vector is the head of the type graph
 * returns the position in the vector at which the new node is placed */
int DwarfData::readType(uint32_t die_offset, std::vector<struct DwarfTypeNode> & type_cache, bool reset_recursion_detector)
{
	int index;
	if (reset_recursion_detector)
		recursion_detector.clear();
	uint32_t saved_die_offset(die_offset);
	if (TYPE_DEBUG_ENABLED) qDebug() << "reading type die at offset" << QString("$%1").arg(die_offset, 0, 16);
	if (recursion_detector.operator [](saved_die_offset))
	{
		if (TYPE_DEBUG_ENABLED) qDebug() << "!!! type chain recursion detected";
		return recursion_detector.operator [](saved_die_offset);
	}
	auto type = debug_tree_of_die(die_offset);
	if (type.size() != 1)
		DwarfUtil::panic();
	struct DwarfTypeNode node(type.at(0));
	struct Abbreviation a(debug_abbrev + node.die.abbrev_offset);
	DwarfUtil::attribute_data t;
	if ((t = a.dataForAttribute(DW_AT_abstract_origin, debug_info + node.die.offset)).form
		|| (t = a.dataForAttribute(DW_AT_import, debug_info + node.die.offset)).form
		|| (t = a.dataForAttribute(DW_AT_specification, debug_info + node.die.offset)).form
			)
	{
		return readType(DwarfUtil::formReference(t.form, t.debug_info_bytes,
		/*! \todo	this is braindamaged; whoever passes the abbreviation cache to this function
		 *		should already have information about the containing compilation unit - maybe
		 *		just pass that as an additional parameter */
			compilationUnitOffsetForOffsetInDebugInfo(saved_die_offset)
			), type_cache, false);
	}

	type_cache.push_back(node);
	recursion_detector.operator [](saved_die_offset) = index = type_cache.size() - 1;

	t = a.dataForAttribute(DW_AT_type, debug_info + type_cache.at(index).die.offset);
	if (t.form)
	{
                int i;
		/*! \todo	this is braindamaged; whoever passes the abbreviation cache to this function
		 *		should already have information about the containing compilation unit - maybe
		 *		just pass that as an additional parameter */
		uint32_t cu_offset(compilationUnitOffsetForOffsetInDebugInfo(saved_die_offset));
		/* generally, the referred type can be in another compilation unit; gcc has not been observed
		 * to do this until now (02022017), but the IAR compiler does generate such references;
		 * if this is the case, the abbreviations of the referred compilation unit must be fetched,
		 * so do fetch them in all cases */
		auto x = readTypeOffset(t.form, t.debug_info_bytes, cu_offset);
                i = readType(x, type_cache, false);
                type_cache.at(index).next = i;
                if (TYPE_DEBUG_ENABLED) qDebug() << "read type die at index " << i;
        }

	if (type_cache.at(index).die.children.size())
	{
		int i, x;
		for (i = 0; i < type_cache.at(index).die.children.size(); i ++)
		{
			if (TYPE_DEBUG_ENABLED) qDebug() << "reading type child" << i << ", offset is" << type_cache.at(index).die.children.at(i).offset;
			x = readType(type_cache.at(index).die.children.at(i).offset, type_cache, false);
			type_cache.at(index).children.push_back(x);
			if (type_cache.at(x).die.tag == DW_TAG_subrange_type)
			{
				Abbreviation a(debug_abbrev + type_cache.at(x).die.abbrev_offset);
				auto subrange = a.dataForAttribute(DW_AT_upper_bound, debug_info + type_cache.at(x).die.offset);
				if (subrange.form == 0)
					/*! \todo	at least some versions of gcc are known to omit the upper bound attribute if it is 0;
					 *		maybe have the option to store a zero here */
					type_cache.at(index).array_dimensions.push_back(-1);
				else
					type_cache.at(index).array_dimensions.push_back(DwarfUtil::formConstant(subrange) + 1);
			}
		}
	}

	if (TYPE_DEBUG_ENABLED) qDebug() << "done" << QString("$%1").arg(saved_die_offset, 0, 16);
	return index;
}

DwarfBaseType DwarfData::readBaseOrGenericType(uint32_t die_offset)
{
	if (!die_offset)
		/* Special case - a die_offset of zero means that the 'generic' type, described in the Dwarf 5 standard, should be returned.
		 * This special case is described in the Dwarf 5 standard, in the section describing 'Type conversions' */
		return DwarfBaseType(sizeof(uint32_t), DW_ATE_unsigned);
	std::vector<DwarfTypeNode> base_type;
	readType(die_offset, base_type);
	/* Make sure the type die is indeed a base type die, and the bytesize of this base type can fit in the
	 * sforth stack */
	int base_type_encoding = baseTypeEncoding(base_type), bytesize = sizeOf(base_type);
	if (base_type.size() != 1 || base_type_encoding == -1 || bytesize > sizeof(uint64_t))
		DwarfUtil::panic();
	return DwarfBaseType(bytesize, base_type_encoding);
}

bool DwarfData::isPointerType(const std::vector<DwarfTypeNode> &type, int node_number)
{
	if (node_number == -1)
		return false;
	switch (type.at(node_number).die.tag)
	{
		case DW_TAG_rvalue_reference_type:
		case DW_TAG_reference_type:
		case DW_TAG_pointer_type:
		case DW_TAG_ptr_to_member_type:
			return true;
	}
	return false;
}

bool DwarfData::isArrayType(const std::vector<DwarfTypeNode> &type, int node_number)
{
	if (node_number == -1)
		return false;
	switch (type.at(node_number).die.tag)
	{
		case DW_TAG_array_type:
			return true;
	}
	return false;
}

bool DwarfData::isSubroutineType(const std::vector<DwarfTypeNode> &type, int node_number)
{
	if (node_number == -1)
		return false;
	switch (type.at(node_number).die.tag)
	{
		case DW_TAG_subroutine_type:
			return true;
	}
	return false;
}

int DwarfData::baseTypeEncoding(const std::vector<DwarfTypeNode> &type, int node_number)
{
	if (node_number == -1)
		return false;
	const auto& die = type.at(node_number).die;
	if (die.tag != DW_TAG_base_type)
		return -1;
	Abbreviation a(debug_abbrev + die.abbrev_offset);
	auto e = a.dataForAttribute(DW_AT_encoding, debug_info + die.offset);
	if (e.form)
		return DwarfUtil::formConstant(e);
	return -1;
}

std::string DwarfData::typeString(std::vector<struct DwarfTypeNode> & type, int node_number, TypePrintFlags flags, bool reset_type_recursion_detection_flags)
{
	if (reset_type_recursion_detection_flags)
		for (auto x = type.begin(); x != type.end(); x->type_print_recursion_flag = false, x ++);
	std::string type_string;
	type_string = typeChainString(type, true, node_number, flags);
	//type_string += nameOfDie(type.at(node_number).die);
	type_string += typeChainString(type, false, node_number, flags);
	return type_string;
}
<commit_msg>Bugfix<commit_after>/*
Copyright (c) 2016-2017 stoyan shopov

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "libtroll.hxx"

#define TYPE_DEBUG_ENABLED	0
/* the first node in the vector is the head of the type graph
 * returns the position in the vector at which the new node is placed */
int DwarfData::readType(uint32_t die_offset, std::vector<struct DwarfTypeNode> & type_cache, bool reset_recursion_detector)
{
	int index;
	if (reset_recursion_detector)
		recursion_detector.clear();
	uint32_t saved_die_offset(die_offset);
	if (TYPE_DEBUG_ENABLED) qDebug() << "reading type die at offset" << QString("$%1").arg(die_offset, 0, 16);
	if (recursion_detector.operator [](saved_die_offset))
	{
		if (TYPE_DEBUG_ENABLED) qDebug() << "!!! type chain recursion detected";
		return recursion_detector.operator [](saved_die_offset);
	}
	auto type = debug_tree_of_die(die_offset);
	if (type.size() != 1)
		DwarfUtil::panic();
	struct DwarfTypeNode node(type.at(0));
	struct Abbreviation a(debug_abbrev + node.die.abbrev_offset);
	DwarfUtil::attribute_data t;
	if ((t = a.dataForAttribute(DW_AT_abstract_origin, debug_info + node.die.offset)).form
		|| (t = a.dataForAttribute(DW_AT_import, debug_info + node.die.offset)).form
		|| (t = a.dataForAttribute(DW_AT_specification, debug_info + node.die.offset)).form
			)
	{
		return readType(DwarfUtil::formReference(t.form, t.debug_info_bytes,
		/*! \todo	this is braindamaged; whoever passes the abbreviation cache to this function
		 *		should already have information about the containing compilation unit - maybe
		 *		just pass that as an additional parameter */
			compilationUnitOffsetForOffsetInDebugInfo(saved_die_offset)
			), type_cache, false);
	}

	type_cache.push_back(node);
	recursion_detector.operator [](saved_die_offset) = index = type_cache.size() - 1;

	t = a.dataForAttribute(DW_AT_type, debug_info + type_cache.at(index).die.offset);
	if (t.form)
	{
                int i;
		/*! \todo	this is braindamaged; whoever passes the abbreviation cache to this function
		 *		should already have information about the containing compilation unit - maybe
		 *		just pass that as an additional parameter */
		uint32_t cu_offset(compilationUnitOffsetForOffsetInDebugInfo(saved_die_offset));
		/* generally, the referred type can be in another compilation unit; gcc has not been observed
		 * to do this until now (02022017), but the IAR compiler does generate such references;
		 * if this is the case, the abbreviations of the referred compilation unit must be fetched,
		 * so do fetch them in all cases */
		auto x = readTypeOffset(t.form, t.debug_info_bytes, cu_offset);
                i = readType(x, type_cache, false);
                type_cache.at(index).next = i;
                if (TYPE_DEBUG_ENABLED) qDebug() << "read type die at index " << i;
        }

	if (type_cache.at(index).die.children.size())
	{
		int i, x;
		for (i = 0; i < type_cache.at(index).die.children.size(); i ++)
		{
			if (TYPE_DEBUG_ENABLED) qDebug() << "reading type child" << i << ", offset is" << type_cache.at(index).die.children.at(i).offset;
			x = readType(type_cache.at(index).die.children.at(i).offset, type_cache, false);
			type_cache.at(index).children.push_back(x);
			if (type_cache.at(x).die.tag == DW_TAG_subrange_type)
			{
				Abbreviation a(debug_abbrev + type_cache.at(x).die.abbrev_offset);
				auto subrange = a.dataForAttribute(DW_AT_upper_bound, debug_info + type_cache.at(x).die.offset);
				if (subrange.form == 0)
					/*! \todo	at least some versions of gcc are known to omit the upper bound attribute if it is 0;
					 *		maybe have the option to store a zero here */
					type_cache.at(index).array_dimensions.push_back(-1);
				else
					type_cache.at(index).array_dimensions.push_back(DwarfUtil::formConstant(subrange) + 1);
			}
		}
	}

	if (TYPE_DEBUG_ENABLED) qDebug() << "done" << QString("$%1").arg(saved_die_offset, 0, 16);
	return index;
}

DwarfBaseType DwarfData::readBaseOrGenericType(uint32_t die_offset)
{
	if (!die_offset)
		/* Special case - a die_offset of zero means that the 'generic' type, described in the Dwarf 5 standard, should be returned.
		 * This special case is described in the Dwarf 5 standard, in the section describing 'Type conversions' */
		return DwarfBaseType(sizeof(uint32_t), DW_ATE_unsigned);
	std::vector<DwarfTypeNode> base_type;
	readType(die_offset, base_type);
	/* Make sure the type die is indeed a base type die, and the bytesize of this base type can fit in the
	 * sforth stack */
	int base_type_encoding = baseTypeEncoding(base_type), bytesize = sizeOf(base_type);
	if (base_type.size() != 1 || base_type_encoding == -1 || bytesize > sizeof(uint64_t))
		DwarfUtil::panic();
	return DwarfBaseType(bytesize, base_type_encoding);
}

bool DwarfData::isPointerType(const std::vector<DwarfTypeNode> &type, int node_number)
{
	if (node_number == -1)
		return false;
	switch (type.at(node_number).die.tag)
	{
		case DW_TAG_rvalue_reference_type:
		case DW_TAG_reference_type:
		case DW_TAG_pointer_type:
		case DW_TAG_ptr_to_member_type:
			return true;
	}
	return false;
}

bool DwarfData::isArrayType(const std::vector<DwarfTypeNode> &type, int node_number)
{
	if (node_number == -1)
		return false;
	switch (type.at(node_number).die.tag)
	{
		case DW_TAG_array_type:
			return true;
	}
	return false;
}

bool DwarfData::isSubroutineType(const std::vector<DwarfTypeNode> &type, int node_number)
{
	if (node_number == -1)
		return false;
	switch (type.at(node_number).die.tag)
	{
		case DW_TAG_subroutine_type:
			return true;
	}
	return false;
}

int DwarfData::baseTypeEncoding(const std::vector<DwarfTypeNode> &type, int node_number)
{
	if (node_number == -1)
		return -1;
	const auto& die = type.at(node_number).die;
	if (die.tag != DW_TAG_base_type)
		return -1;
	Abbreviation a(debug_abbrev + die.abbrev_offset);
	auto e = a.dataForAttribute(DW_AT_encoding, debug_info + die.offset);
	if (e.form)
		return DwarfUtil::formConstant(e);
	return -1;
}

std::string DwarfData::typeString(std::vector<struct DwarfTypeNode> & type, int node_number, TypePrintFlags flags, bool reset_type_recursion_detection_flags)
{
	if (reset_type_recursion_detection_flags)
		for (auto x = type.begin(); x != type.end(); x->type_print_recursion_flag = false, x ++);
	std::string type_string;
	type_string = typeChainString(type, true, node_number, flags);
	//type_string += nameOfDie(type.at(node_number).die);
	type_string += typeChainString(type, false, node_number, flags);
	return type_string;
}
<|endoftext|>


<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

// Original version: Melonee Wise <mwise@willowgarage.com>

#include "control_toolbox/pid.h"
#include "tinyxml.h"

namespace control_toolbox {

Pid::Pid(double p, double i, double d, double i_max, double i_min) :
  p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min)
{
  this->reset();
}

Pid::~Pid()
{
}

void Pid::initPid(double p, double i, double d, double i_max, double i_min)
{
  this->setGains(p,i,d,i_max,i_min);
  reset();
}

void Pid::reset()
{
  p_error_last_ = 0.0;
  p_error_ = 0.0;
  d_error_ = 0.0;
  i_error_ = 0.0;
  cmd_ = 0.0;
}

void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
  p = p_gain_;
  i = i_gain_;
  d = d_gain_;
  i_max = i_max_;
  i_min = i_min_;
}

void Pid::setGains(double p, double i, double d, double i_max, double i_min)
{
  p_gain_ = p;
  i_gain_ = i;
  d_gain_ = d;
  i_max_ = i_max;
  i_min_ = i_min;
}

bool Pid::initParam(const std::string& prefix)
{
  ros::NodeHandle node(prefix);

  if (!node.getParam("p", p_gain_)) {
    ROS_ERROR("No p gain specified for pid.  Prefix: %s", prefix.c_str());
    return false;
  }
  node.param("i", i_gain_, 0.0) ;
  node.param("d", d_gain_, 0.0) ;
  node.param("i_clamp", i_max_, 0.0) ;
  i_min_ = -i_max_;

  reset();
  return true;
}

bool Pid::initXml(TiXmlElement *config)
{
  p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0;
  i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0;
  d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0;
  i_max_ = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
  i_min_ = -i_max_;

  reset();
  return true;
}

bool Pid::init(const ros::NodeHandle &node)
{
  ros::NodeHandle n(node);
  if (!n.getParam("p", p_gain_)) {
    ROS_ERROR("No p gain specified for pid.  Namespace: %s", n.getNamespace().c_str());
    return false;
  }
  n.param("i", i_gain_, 0.0);
  n.param("d", d_gain_, 0.0);
  n.param("i_clamp", i_max_, 0.0);
  i_min_ = -i_max_;

  reset();
  return true;
}

double Pid::setError(double error, ros::Duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error
  i_error_ = i_error_ + dt.toSec() * p_error_;

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate the derivative error
  if (dt.toSec() > 0.0)
  {
    d_error_ = (p_error_ - p_error_last_) / dt.toSec();
    p_error_last_ = p_error_;
  }
  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = p_term + i_term + d_term;

  return cmd_;
}

double Pid::updatePid(double error, ros::Duration dt)
{
  return -1.0*(this->setError(error, dt));
}

double Pid::setError(double error, double error_dot, ros::Duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget
  d_error_ = error_dot;

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error
  i_error_ = i_error_ + dt.toSec() * p_error_;

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = p_term + i_term + d_term;

  return cmd_;
}

double Pid::updatePid(double error, double error_dot, ros::Duration dt)
{
  return -1.0*(this->setError(error, error_dot, dt));
}

void Pid::setCurrentCmd(double cmd)
{
  cmd_ = cmd;
}

double Pid::getCurrentCmd()
{
  return cmd_;
}

void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
{
  *pe = p_error_;
  *ie = i_error_;
  *de = d_error_;
}

}
<commit_msg>bringing back old updatePid function contents<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

// Original version: Melonee Wise <mwise@willowgarage.com>

#include "control_toolbox/pid.h"
#include "tinyxml.h"

namespace control_toolbox {

Pid::Pid(double p, double i, double d, double i_max, double i_min) :
  p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min)
{
  this->reset();
}

Pid::~Pid()
{
}

void Pid::initPid(double p, double i, double d, double i_max, double i_min)
{
  this->setGains(p,i,d,i_max,i_min);
  reset();
}

void Pid::reset()
{
  p_error_last_ = 0.0;
  p_error_ = 0.0;
  d_error_ = 0.0;
  i_error_ = 0.0;
  cmd_ = 0.0;
}

void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
  p = p_gain_;
  i = i_gain_;
  d = d_gain_;
  i_max = i_max_;
  i_min = i_min_;
}

void Pid::setGains(double p, double i, double d, double i_max, double i_min)
{
  p_gain_ = p;
  i_gain_ = i;
  d_gain_ = d;
  i_max_ = i_max;
  i_min_ = i_min;
}

bool Pid::initParam(const std::string& prefix)
{
  ros::NodeHandle node(prefix);

  if (!node.getParam("p", p_gain_)) {
    ROS_ERROR("No p gain specified for pid.  Prefix: %s", prefix.c_str());
    return false;
  }
  node.param("i", i_gain_, 0.0) ;
  node.param("d", d_gain_, 0.0) ;
  node.param("i_clamp", i_max_, 0.0) ;
  i_min_ = -i_max_;

  reset();
  return true;
}

bool Pid::initXml(TiXmlElement *config)
{
  p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0;
  i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0;
  d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0;
  i_max_ = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
  i_min_ = -i_max_;

  reset();
  return true;
}

bool Pid::init(const ros::NodeHandle &node)
{
  ros::NodeHandle n(node);
  if (!n.getParam("p", p_gain_)) {
    ROS_ERROR("No p gain specified for pid.  Namespace: %s", n.getNamespace().c_str());
    return false;
  }
  n.param("i", i_gain_, 0.0);
  n.param("d", d_gain_, 0.0);
  n.param("i_clamp", i_max_, 0.0);
  i_min_ = -i_max_;

  reset();
  return true;
}

double Pid::setError(double error, ros::Duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error
  i_error_ = i_error_ + dt.toSec() * p_error_;

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate the derivative error
  if (dt.toSec() > 0.0)
  {
    d_error_ = (p_error_ - p_error_last_) / dt.toSec();
    p_error_last_ = p_error_;
  }
  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = p_term + i_term + d_term;

  return cmd_;
}

double Pid::updatePid(double error, ros::Duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error
  i_error_ = i_error_ + dt.toSec() * p_error_;

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate the derivative error
  if (dt.toSec() > 0.0)
  {
    d_error_ = (p_error_ - p_error_last_) / dt.toSec();
    p_error_last_ = p_error_;
  }
  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = - p_term - i_term - d_term;

  return cmd_;
}

double Pid::setError(double error, double error_dot, ros::Duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget
  d_error_ = error_dot;

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error
  i_error_ = i_error_ + dt.toSec() * p_error_;

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = p_term + i_term + d_term;

  return cmd_;
}

double Pid::updatePid(double error, double error_dot, ros::Duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget
  d_error_ = error_dot;

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error
  i_error_ = i_error_ + dt.toSec() * p_error_;

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = - p_term - i_term - d_term;

  return cmd_;
}

void Pid::setCurrentCmd(double cmd)
{
  cmd_ = cmd;
}

double Pid::getCurrentCmd()
{
  return cmd_;
}

void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
{
  *pe = p_error_;
  *ie = i_error_;
  *de = d_error_;
}

}
<|endoftext|>