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

//Prpare pour la lecture
BlueFile::BlueFile(string nom_fichier) : nom_fichier(nom_fichier)
{
	bits.reserve(BLOCK_SIZE);

	determineTaille();

	fichier_lecture = NULL;
	fichier_ecriture = NULL;

	cout << "__________/_\\__________" << endl;
	cout << "CRYPTAGE/DECRYPTAGE DE " << nom_fichier << endl;
	cout << "TAILLE: " << taille << endl << endl;
}

//Dtruit les donnes
BlueFile::~BlueFile()
{
	fichier_lecture->close();
	delete fichier_lecture;
	delete fichier_ecriture;
}

//Dtermine la taille du fichier
void BlueFile::determineTaille()
{
	ifstream in("" DOSSIER_EFFECTIF "/" + nom_fichier, ifstream::ate | ifstream::binary);
	taille = in.tellg();
}

//Lecture en binaire d'un fichier
void BlueFile::lireBinaire()
{
	if(fichier_lecture == NULL)
		fichier_lecture = new ifstream("" DOSSIER_EFFECTIF "/" + nom_fichier, ios::binary);

	//lecture caractre par caractre
	char c = '_';
	while(bits.size() < BLOCK_SIZE && (int)fichier_lecture->tellg() != taille)
	{
		*fichier_lecture >> noskipws >> c;
		//lecture bit par bit
		for(int i = 7; i >= 0; i--)
			bits.push_back(((c >> i) & 1));

	}

}

//Dtecte si le fichier a fini d'tre crypt
int BlueFile::procedureFinie()
{
	int position = (int)fichier_lecture->tellg();
	bool end = position == taille;

	//message de fin / progression
	if(end)
		cout << "CRYPTAGE/DECRYPTAGE FINI" << endl << "__________\\_/__________" << endl << endl;
	else
		cout << "Progression : " << (float)fichier_lecture->tellg() / (float) taille << "%" << endl;

	if(end)
		return 1;
	else if(position == -1)
		return -1;
	else return 0;
}

//Crypte l'ensemble des bits courants
void BlueFile::crypter()
{
	//cryptage basique: inverser les bits deux  deux
	for(size_t i = 0; i < bits.size() -1; i+=2)
	{
		char c1 = bits[i];
		bits[i] = bits[i + 1];
		bits[i + 1] = c1;
	}
}

//Dcrypte l'ensemble des bits courants
void BlueFile::decrypter()
{
	//dcryptage basique: inverser les bits deux  deux
	for(size_t i = 0; i < bits.size() - 1; i+=2)
	{
		char c1 = bits[i];
		bits[i] = bits[i + 1];
		bits[i + 1] = c1;
	}
}

//Rcris l'ensemble des bits courants dans le fichier en binaire
void BlueFile::ecrireBinaire()
{
	if(!fichier_ecriture)
		fichier_ecriture = new ofstream("" DOSSIER_EFFECTIF "/" + nom_fichier,   ios::binary);

	//lecture des bits
	for(size_t i = 0; i < bits.size(); i+=8)
	{
		//regroupement en caractre
		char c = 0;
		for(int j = 0; j < 8; j++)
			c ^= (-(int)bits[i + j] ^ c) & (1 << (7 - j));

		//si non saut de ligne spcial
		*fichier_ecriture << c;
	}
	bits.clear();
}

//Affiche les bits
void BlueFile::afficherBits()
{
	cout << endl << "AFFICHAGE BITS:" << endl;
	for(size_t i = 0; i < bits.size(); i++)
	{
		if(i % 8 == 0)
			cout << endl;
		char bit = (bits[i] ? '1' : '0');
		cout << bit;
	}
	cout << endl << "______" << endl << endl;

}

//Whiax<commit_msg>correction mineure<commit_after>#include "../include/BlueFile.h"

//Prpare pour la lecture
BlueFile::BlueFile(string nom_fichier) : nom_fichier(nom_fichier)
{
	bits.reserve(BLOCK_SIZE);

	determineTaille();

	fichier_lecture = NULL;
	fichier_ecriture = NULL;

	cout << "__________/_\\__________" << endl;
	cout << "CRYPTAGE/DECRYPTAGE DE " << nom_fichier << endl;
	cout << "TAILLE: " << taille << endl << endl;
}

//Dtruit les donnes
BlueFile::~BlueFile()
{
	fichier_lecture->close();
	fichier_ecriture->close();
	delete fichier_lecture;
	delete fichier_ecriture;
}

//Dtermine la taille du fichier
void BlueFile::determineTaille()
{
	ifstream in("" DOSSIER_EFFECTIF "/" + nom_fichier, ifstream::ate | ifstream::binary);
	taille = in.tellg();
}

//Lecture en binaire d'un fichier
void BlueFile::lireBinaire()
{
	if(fichier_lecture == NULL)
		fichier_lecture = new ifstream("" DOSSIER_EFFECTIF "/" + nom_fichier, ios::binary);

	//lecture caractre par caractre
	char c = '_';
	while(bits.size() < BLOCK_SIZE && (int)fichier_lecture->tellg() != taille)
	{
		*fichier_lecture >> noskipws >> c;
		//lecture bit par bit
		for(int i = 7; i >= 0; i--)
			bits.push_back(((c >> i) & 1));

	}

}

//Dtecte si le fichier a fini d'tre crypt
int BlueFile::procedureFinie()
{
	int position = (int)fichier_lecture->tellg();
	bool end = position == taille;

	//message de fin / progression
	if(end)
		cout << "CRYPTAGE/DECRYPTAGE FINI" << endl << "__________\\_/__________" << endl << endl;
	else
		cout << "Progression : " << (float)fichier_lecture->tellg() / (float) taille << "%" << endl;

	if(end)
		return 1;
	else if(position == -1)
		return -1;
	else return 0;
}

//Crypte l'ensemble des bits courants
void BlueFile::crypter()
{
	//cryptage basique: inverser les bits deux  deux
	for(size_t i = 0; i < bits.size() -1; i+=2)
	{
		char c1 = bits[i];
		bits[i] = bits[i + 1];
		bits[i + 1] = c1;
	}
}

//Dcrypte l'ensemble des bits courants
void BlueFile::decrypter()
{
	//dcryptage basique: inverser les bits deux  deux
	for(size_t i = 0; i < bits.size() - 1; i+=2)
	{
		char c1 = bits[i];
		bits[i] = bits[i + 1];
		bits[i + 1] = c1;
	}
}

//Rcris l'ensemble des bits courants dans le fichier en binaire
void BlueFile::ecrireBinaire()
{
	if(!fichier_ecriture)
		fichier_ecriture = new ofstream("" DOSSIER_EFFECTIF "/" + nom_fichier,   ios::binary);

	//lecture des bits
	for(size_t i = 0; i < bits.size(); i+=8)
	{
		//regroupement en caractre
		char c = 0;
		for(int j = 0; j < 8; j++)
			c ^= (-(int)bits[i + j] ^ c) & (1 << (7 - j));

		//si non saut de ligne spcial
		*fichier_ecriture << c;
	}
	bits.clear();
}

//Affiche les bits
void BlueFile::afficherBits()
{
	cout << endl << "AFFICHAGE BITS:" << endl;
	for(size_t i = 0; i < bits.size(); i++)
	{
		if(i % 8 == 0)
			cout << endl;
		char bit = (bits[i] ? '1' : '0');
		cout << bit;
	}
	cout << endl << "______" << endl << endl;

}

//Whiax<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the QtDeclarative module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qmldebugclient_p.h"
#include <private/qobject_p.h>
#include <private/qpacketprotocol_p.h>
#include <QtCore/qdebug.h>
#include <QtCore/qstringlist.h>

QT_BEGIN_NAMESPACE

class QmlDebugConnectionPrivate : public QObject
{
    Q_OBJECT
public:
    QmlDebugConnectionPrivate(QmlDebugConnection *c);
    QmlDebugConnection *q;
    QPacketProtocol *protocol;

    QStringList enabled;
    QHash<QString, QmlDebugClient *> plugins;
public Q_SLOTS:
    void connected();
    void readyRead();
};

QmlDebugConnectionPrivate::QmlDebugConnectionPrivate(QmlDebugConnection *c)
: QObject(c), q(c), protocol(0)
{
    protocol = new QPacketProtocol(q, this);
    QObject::connect(c, SIGNAL(connected()), this, SLOT(connected()));
    QObject::connect(protocol, SIGNAL(readyRead()), this, SLOT(readyRead()));
}

void QmlDebugConnectionPrivate::connected()
{
    QPacket pack;
    pack << QString(QLatin1String("QmlDebugServer")) << enabled;
    protocol->send(pack);
}

void QmlDebugConnectionPrivate::readyRead()
{
    QPacket pack = protocol->read();
    QString name; QByteArray message;
    pack >> name >> message;

    QHash<QString, QmlDebugClient *>::Iterator iter =
        plugins.find(name);
    if (iter == plugins.end()) {
        qWarning() << "QmlDebugConnection: Message received for missing plugin" << name;
    } else {
        (*iter)->messageReceived(message);
    }
}

QmlDebugConnection::QmlDebugConnection(QObject *parent)
: QTcpSocket(parent), d(new QmlDebugConnectionPrivate(this))
{
}

bool QmlDebugConnection::isConnected() const
{
    return state() == ConnectedState;
}

class QmlDebugClientPrivate : public QObjectPrivate
{
    Q_DECLARE_PUBLIC(QmlDebugClient)
public:
    QmlDebugClientPrivate();

    QString name;
    QmlDebugConnection *client;
    bool enabled;
};

QmlDebugClientPrivate::QmlDebugClientPrivate()
: client(0), enabled(false)
{
}

QmlDebugClient::QmlDebugClient(const QString &name,
                                           QmlDebugConnection *parent)
: QObject(*(new QmlDebugClientPrivate), parent)
{
    Q_D(QmlDebugClient);
    d->name = name;
    d->client = parent;

    if (!d->client)
        return;

    if (d->client->d->plugins.contains(name)) {
        qWarning() << "QmlDebugClient: Conflicting plugin name" << name;
        d->client = 0;
    } else {
        d->client->d->plugins.insert(name, this);
    }
}

QString QmlDebugClient::name() const
{
    Q_D(const QmlDebugClient);
    return d->name;
}

bool QmlDebugClient::isEnabled() const
{
    Q_D(const QmlDebugClient);
    return d->enabled;
}

void QmlDebugClient::setEnabled(bool e)
{
    Q_D(QmlDebugClient);
    if (e == d->enabled)
        return;

    d->enabled = e;

    if (d->client) {
        if (e)
            d->client->d->enabled.append(d->name);
        else
            d->client->d->enabled.removeAll(d->name);

        if (d->client->state() == QTcpSocket::ConnectedState) {
            QPacket pack;
            pack << QString(QLatin1String("QmlDebugServer"));
            if (e) pack << (int)1;
            else pack << (int)2;
            pack << d->name;
            d->client->d->protocol->send(pack);
        }
    }
}

bool QmlDebugClient::isConnected() const
{
    Q_D(const QmlDebugClient);

    return d->client->isConnected();
}

void QmlDebugClient::sendMessage(const QByteArray &message)
{
    Q_D(QmlDebugClient);

    if (!d->client || !d->client->isConnected())
        return;

    QPacket pack;
    pack << d->name << message;
    d->client->d->protocol->send(pack);
}

void QmlDebugClient::messageReceived(const QByteArray &)
{
}

QT_END_NAMESPACE

#include "qmldebugclient.moc"
<commit_msg>Fix crash bug where socket is 0.<commit_after>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the QtDeclarative module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qmldebugclient_p.h"
#include <private/qobject_p.h>
#include <private/qpacketprotocol_p.h>
#include <QtCore/qdebug.h>
#include <QtCore/qstringlist.h>

QT_BEGIN_NAMESPACE

class QmlDebugConnectionPrivate : public QObject
{
    Q_OBJECT
public:
    QmlDebugConnectionPrivate(QmlDebugConnection *c);
    QmlDebugConnection *q;
    QPacketProtocol *protocol;

    QStringList enabled;
    QHash<QString, QmlDebugClient *> plugins;
public Q_SLOTS:
    void connected();
    void readyRead();
};

QmlDebugConnectionPrivate::QmlDebugConnectionPrivate(QmlDebugConnection *c)
: QObject(c), q(c), protocol(0)
{
    protocol = new QPacketProtocol(q, this);
    QObject::connect(c, SIGNAL(connected()), this, SLOT(connected()));
    QObject::connect(protocol, SIGNAL(readyRead()), this, SLOT(readyRead()));
}

void QmlDebugConnectionPrivate::connected()
{
    QPacket pack;
    pack << QString(QLatin1String("QmlDebugServer")) << enabled;
    protocol->send(pack);
}

void QmlDebugConnectionPrivate::readyRead()
{
    QPacket pack = protocol->read();
    QString name; QByteArray message;
    pack >> name >> message;

    QHash<QString, QmlDebugClient *>::Iterator iter =
        plugins.find(name);
    if (iter == plugins.end()) {
        qWarning() << "QmlDebugConnection: Message received for missing plugin" << name;
    } else {
        (*iter)->messageReceived(message);
    }
}

QmlDebugConnection::QmlDebugConnection(QObject *parent)
: QTcpSocket(parent), d(new QmlDebugConnectionPrivate(this))
{
}

bool QmlDebugConnection::isConnected() const
{
    return state() == ConnectedState;
}

class QmlDebugClientPrivate : public QObjectPrivate
{
    Q_DECLARE_PUBLIC(QmlDebugClient)
public:
    QmlDebugClientPrivate();

    QString name;
    QmlDebugConnection *client;
    bool enabled;
};

QmlDebugClientPrivate::QmlDebugClientPrivate()
: client(0), enabled(false)
{
}

QmlDebugClient::QmlDebugClient(const QString &name,
                                           QmlDebugConnection *parent)
: QObject(*(new QmlDebugClientPrivate), parent)
{
    Q_D(QmlDebugClient);
    d->name = name;
    d->client = parent;

    if (!d->client)
        return;

    if (d->client->d->plugins.contains(name)) {
        qWarning() << "QmlDebugClient: Conflicting plugin name" << name;
        d->client = 0;
    } else {
        d->client->d->plugins.insert(name, this);
    }
}

QString QmlDebugClient::name() const
{
    Q_D(const QmlDebugClient);
    return d->name;
}

bool QmlDebugClient::isEnabled() const
{
    Q_D(const QmlDebugClient);
    return d->enabled;
}

void QmlDebugClient::setEnabled(bool e)
{
    Q_D(QmlDebugClient);
    if (e == d->enabled)
        return;

    d->enabled = e;

    if (d->client) {
        if (e)
            d->client->d->enabled.append(d->name);
        else
            d->client->d->enabled.removeAll(d->name);

        if (d->client->state() == QTcpSocket::ConnectedState) {
            QPacket pack;
            pack << QString(QLatin1String("QmlDebugServer"));
            if (e) pack << (int)1;
            else pack << (int)2;
            pack << d->name;
            d->client->d->protocol->send(pack);
        }
    }
}

bool QmlDebugClient::isConnected() const
{
    Q_D(const QmlDebugClient);

    if (!d->client)
        return false;
    return d->client->isConnected();
}

void QmlDebugClient::sendMessage(const QByteArray &message)
{
    Q_D(QmlDebugClient);

    if (!d->client || !d->client->isConnected())
        return;

    QPacket pack;
    pack << d->name << message;
    d->client->d->protocol->send(pack);
}

void QmlDebugClient::messageReceived(const QByteArray &)
{
}

QT_END_NAMESPACE

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


<commit_before>/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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 Universite de Sherbrooke 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 HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "CoreWrapper.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Version.h>

int main(int argc, char** argv)
{
	ROS_INFO("Starting node...");

	ULogger::setType(ULogger::kTypeConsole);
	ULogger::setLevel(ULogger::kInfo);

	ros::init(argc, argv, "rtabmap");

	bool deleteDbOnStart = false;
	for(int i=1;i<argc;++i)
	{
		if(strcmp(argv[i], "--delete_db_on_start") == 0)
		{
			deleteDbOnStart = true;
		}
		else if(strcmp(argv[i], "--udebug") == 0)
		{
			ULogger::setLevel(ULogger::kDebug);
		}
		else if(strcmp(argv[i], "--uinfo") == 0)
		{
			ULogger::setLevel(ULogger::kInfo);
		}
		else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
		{
			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
			uInsert(parameters,
					std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(),
					UDirectory::homeDir()+"/.ros")); // change default to ~/.ros

			if(strcmp(argv[i], "--params") == 0)
			{
				// hide specific parameters
				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
				{
					if(iter->first.find("Odom") == 0)
					{
						parameters.erase(iter++);
					}
					else
					{
						++iter;
					}
				}
			}

			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
			{
				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
				std::cout <<
						str <<
						std::setw(60 - str.size()) <<
						" [" <<
						rtabmap::Parameters::getDescription(iter->first).c_str() <<
						"]" <<
						std::endl;
			}
			ROS_WARN("Node will now exit after showing default RTAB-Map parameters because "
					 "argument \"--params\" is detected!");
			exit(0);
		}
		else
		{
			ROS_ERROR("Not recognized argument \"%s\"", argv[i]);
			exit(-1);
		}
	}

	CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart);

	ROS_INFO("rtabmap %s started...", RTABMAP_VERSION);
	ros::spin();

	delete rtabmap;

	return 0;
}
<commit_msg>CoreNode: Log Info->Warning level<commit_after>/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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 Universite de Sherbrooke 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 HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "CoreWrapper.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Version.h>

int main(int argc, char** argv)
{
	ROS_INFO("Starting node...");

	ULogger::setType(ULogger::kTypeConsole);
	ULogger::setLevel(ULogger::kWarning);

	ros::init(argc, argv, "rtabmap");

	bool deleteDbOnStart = false;
	for(int i=1;i<argc;++i)
	{
		if(strcmp(argv[i], "--delete_db_on_start") == 0)
		{
			deleteDbOnStart = true;
		}
		else if(strcmp(argv[i], "--udebug") == 0)
		{
			ULogger::setLevel(ULogger::kDebug);
		}
		else if(strcmp(argv[i], "--uinfo") == 0)
		{
			ULogger::setLevel(ULogger::kInfo);
		}
		else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
		{
			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
			uInsert(parameters,
					std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(),
					UDirectory::homeDir()+"/.ros")); // change default to ~/.ros

			if(strcmp(argv[i], "--params") == 0)
			{
				// hide specific parameters
				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
				{
					if(iter->first.find("Odom") == 0)
					{
						parameters.erase(iter++);
					}
					else
					{
						++iter;
					}
				}
			}

			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
			{
				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
				std::cout <<
						str <<
						std::setw(60 - str.size()) <<
						" [" <<
						rtabmap::Parameters::getDescription(iter->first).c_str() <<
						"]" <<
						std::endl;
			}
			ROS_WARN("Node will now exit after showing default RTAB-Map parameters because "
					 "argument \"--params\" is detected!");
			exit(0);
		}
		else
		{
			ROS_ERROR("Not recognized argument \"%s\"", argv[i]);
			exit(-1);
		}
	}

	CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart);

	ROS_INFO("rtabmap %s started...", RTABMAP_VERSION);
	ros::spin();

	delete rtabmap;

	return 0;
}
<|endoftext|>


<commit_before>/**************************************************************************

 The MIT License (MIT)

 Copyright (c) 2015 Dmitry Sovetov

 https://github.com/dmsovetov

 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 "Physics.h"

DC_BEGIN_DREEMCHEST

namespace Scene {

// ------------------------------------------- Shape2D ------------------------------------------- //

// ** Shape2D::clear
void Shape2D::clear( void )
{
	m_parts.clear();
}

// ** Shape2D::partCount
u32 Shape2D::partCount( void ) const
{
	return ( u32 )m_parts.size();
}

// ** Shape2D::part
const Shape2D::Part& Shape2D::part( u32 index ) const
{
	DC_BREAK_IF( index >= partCount() )
	return m_parts[index];
}

// ** Shape2D::addPart
void Shape2D::addPart( const Part& part )
{
    m_parts.push_back( part );
}

// ** Shape2D::addCircle
void Shape2D::addCircle( f32 radius, f32 x, f32 y, const Material& material )
{
	Part part;
	part.type = Circle;
	part.material = material;
	part.circle.radius = radius;
	part.circle.x = x;
	part.circle.y = y;
    addPart( part );
}

// ** Shape2D::addRect
void Shape2D::addRect( f32 width, f32 height, f32 x, f32 y, const Material& material )
{
	Part part;
	part.type = Rect;
	part.material = material;
	part.rect.width = width;
	part.rect.height = height;
	part.rect.x = x;
	part.rect.y = y;
	addPart( part );
}

// ** Shape2D::addPolygon
void Shape2D::addPolygon( const Vec2* vertices, u32 count, const Material& material )
{
	DC_BREAK_IF( count > Part::MaxVertices );

	Part part;
	part.type = Polygon;
	part.material = material;
	part.polygon.count = count;

	for( u32 i = 0; i < count; i++ ) {
		part.polygon.vertices[i * 2 + 0] = vertices[i].x;
		part.polygon.vertices[i * 2 + 1] = vertices[i].y;
	}

	addPart( part );
}

// ** Shape2D::serialize
void Shape2D::serialize( Ecs::SerializationContext& ctx, Io::KeyValue& ar ) const
{
    switch( m_parts[0].type ) {
    case Polygon:   {
                        Io::KeyValue vertices = Io::KeyValue::array();

                        for( u32 i = 0; i < m_parts[0].polygon.count; i++ ) {
                            vertices << m_parts[0].polygon.vertices[i * 2 + 0] << m_parts[0].polygon.vertices[i * 2 + 1];
                        }

                        ar = Io::KeyValue::object() << "type" << "polygon" << "vertices" << vertices;
                    }
                    break;
    default:        DC_BREAK;
    }
}

// ** Shape2D::deserialize
void Shape2D::deserialize( Ecs::SerializationContext& ctx, const Io::KeyValue& ar )
{
    // Get shape type
    const String& type = ar.get( "type", "" ).asString();

    if( type == "polygon" ) {
        const Io::KeyValue& vertices = ar.get( "vertices" );
        Array<Vec2>         points;

        for( s32 i = 0, n = vertices.size() / 2; i < n; i++ ) {
            points.push_back( Vec2( vertices[i * 2 + 0].asFloat(), vertices[i * 2 + 1].asFloat() ) );
        }

        addPolygon( &points[0], points.size() );
    }
    else {
        DC_BREAK;
    }
}

// ----------------------------------------- RigidBody2D ----------------------------------------- //

// ** RigidBody2D::RigidBody2D
RigidBody2D::RigidBody2D( f32 mass, Type type, u16 category, u16 collisionMask, bool isBullet )
	: m_mass( mass ), m_type( type ), m_linearDamping( 0.0f ), m_angularDamping( 0.0f ), m_torque( 0.0f ), m_category( category ), m_collisionMask( collisionMask ), m_gravityScale( 1.0f )
{
    m_flags.set( IsBullet, isBullet );
}

// ** RigidBody2D::mass
f32 RigidBody2D::mass( void ) const
{
	return m_mass;
}

// ** RigidBody2D::type
RigidBody2D::Type RigidBody2D::type( void ) const
{
	return m_type;
}

// ** RigidBody2D::linearDamping
f32 RigidBody2D::linearDamping( void ) const
{
	return m_linearDamping;
}

// ** RigidBody2D::setLinearDamping
void RigidBody2D::setLinearDamping( f32 value )
{
	m_linearDamping = value;
}

// ** RigidBody2D::angularDamping
f32 RigidBody2D::angularDamping( void ) const
{
	return m_angularDamping;
}

// ** RigidBody2D::setAngularDamping
void RigidBody2D::setAngularDamping( f32 value )
{
	m_angularDamping = value;
}

// ** RigidBody2D::gravityScale
f32 RigidBody2D::gravityScale( void ) const
{
    return m_gravityScale;
}

// ** RigidBody2D::setGravityScale
void RigidBody2D::setGravityScale( f32 value )
{
    m_gravityScale = value;
}

// ** RigidBody2D::moveTo
void RigidBody2D::moveTo( const Vec2& position )
{
    m_movedTo  = position;
    m_flags.on( WasMoved );
}

// ** RigidBody2D::movedTo
const Vec2& RigidBody2D::movedTo( void ) const
{
    return m_movedTo;
}

// ** RigidBody2D::wasMoved
bool RigidBody2D::wasMoved( void ) const
{
    return m_flags.is( WasMoved );
}

// ** RigidBody2D::putToRest
void RigidBody2D::putToRest( void )
{
    m_flags.on( WasPutToRest );
}

// ** RigidBody2D::wasPutToRest
bool RigidBody2D::wasPutToRest( void ) const
{
    return m_flags.is( WasPutToRest );
}

// ** RigidBody2D::isBullet
bool RigidBody2D::isBullet( void ) const
{
    return m_flags.is( IsBullet );
}

// ** RigidBody2D::torque
f32 RigidBody2D::torque( void ) const
{
	return m_torque;
}

// ** RigidBody2D::applyTorque
void RigidBody2D::applyTorque( f32 value )
{
	m_torque += value;
}

// ** RigidBody2D::force
const Vec2& RigidBody2D::force( void ) const
{
	return m_force;
}

// ** RigidBody2D::applyForc
void RigidBody2D::applyForce( const Vec2& value )
{
	m_force += value;
}

// ** RigidBody2D::category
u16 RigidBody2D::category( void ) const
{
    return m_category;
}

// ** RigidBody2D::collisionMask
u16 RigidBody2D::collisionMask( void ) const
{
    return m_collisionMask;
}

// ** RigidBody2D::applyForceToPoint
void RigidBody2D::applyForceToPoint( const Vec2& value, const Vec2& point )
{
	m_forces.push_back( AppliedForce( value, point ) );
}

// ** RigidBody2D::applyImpulse
void RigidBody2D::applyImpulse( const Vec2& value )
{
    m_impulses.push_back( AppliedForce( value, Vec2( 0.0f, 0.0f ) ) );
}

// ** RigidBody2D::applyImpulseToPoint
void RigidBody2D::applyImpulseToPoint( const Vec2& value, const Vec2& point )
{
    m_impulses.push_back( AppliedForce( value, point ) );
}

// ** RigidBody2D::appliedForceCount
u32 RigidBody2D::appliedForceCount( void ) const
{
	return ( u32 )m_forces.size();
}

// ** RigidBody2D::appliedImpulse
const RigidBody2D::AppliedForce& RigidBody2D::appliedImpulse( u32 index ) const
{
	DC_BREAK_IF( index >= appliedImpulseCount() );
	return m_impulses[index];
}

// ** RigidBody2D::appliedImpulseCount
u32 RigidBody2D::appliedImpulseCount( void ) const
{
	return ( u32 )m_impulses.size();
}

// ** RigidBody2D::appliedForce
const RigidBody2D::AppliedForce& RigidBody2D::appliedForce( u32 index ) const
{
	DC_BREAK_IF( index >= appliedForceCount() );
	return m_forces[index];
}

// ** RigidBody2D::collisionEventCount
u32 RigidBody2D::collisionEventCount( void ) const
{
    return ( u32 )m_collisionEvents.size();
}

// ** RigidBody2D::collisionEvent
const RigidBody2D::CollisionEvent& RigidBody2D::collisionEvent( u32 index ) const
{
    DC_BREAK_IF( index < 0 || index >= collisionEventCount() );
    return m_collisionEvents[index];
}

// ** RigidBody2D::queueCollisionEvent
void RigidBody2D::queueCollisionEvent( const CollisionEvent& e )
{
    m_collisionEvents.push_back( e );
}

// ** RigidBody2D::clear
void RigidBody2D::clear( void )
{
	m_torque = 0.0f;
	m_force  = Vec2( 0.0f, 0.0f );
    m_flags.off( WasPutToRest | WasMoved );
	m_forces.clear();
    m_impulses.clear();
}

// ** RigidBody2D::clearEvents
void RigidBody2D::clearEvents( void )
{
    m_collisionEvents.clear();
}

// ** RigidBody2D::linearVelocity
const Vec2& RigidBody2D::linearVelocity( void ) const
{
    return m_linearVelocity;
}

// ** RigidBody2D::setLinearVelocity
void RigidBody2D::setLinearVelocity( const Vec2& value )
{
    m_linearVelocity = value;
}

// ** RigidBody2D::updateMass
void RigidBody2D::updateMass( f32 value )
{
    m_mass = value;
}

} // namespace Scene

DC_END_DREEMCHEST<commit_msg>Improved: Shape2D serialization<commit_after>/**************************************************************************

 The MIT License (MIT)

 Copyright (c) 2015 Dmitry Sovetov

 https://github.com/dmsovetov

 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 "Physics.h"

DC_BEGIN_DREEMCHEST

namespace Scene {

// ------------------------------------------- Shape2D ------------------------------------------- //

// ** Shape2D::clear
void Shape2D::clear( void )
{
	m_parts.clear();
}

// ** Shape2D::partCount
u32 Shape2D::partCount( void ) const
{
	return ( u32 )m_parts.size();
}

// ** Shape2D::part
const Shape2D::Part& Shape2D::part( u32 index ) const
{
	DC_BREAK_IF( index >= partCount() )
	return m_parts[index];
}

// ** Shape2D::addPart
void Shape2D::addPart( const Part& part )
{
    m_parts.push_back( part );
}

// ** Shape2D::addCircle
void Shape2D::addCircle( f32 radius, f32 x, f32 y, const Material& material )
{
	Part part;
	part.type = Circle;
	part.material = material;
	part.circle.radius = radius;
	part.circle.x = x;
	part.circle.y = y;
    addPart( part );
}

// ** Shape2D::addRect
void Shape2D::addRect( f32 width, f32 height, f32 x, f32 y, const Material& material )
{
	Part part;
	part.type = Rect;
	part.material = material;
	part.rect.width = width;
	part.rect.height = height;
	part.rect.x = x;
	part.rect.y = y;
	addPart( part );
}

// ** Shape2D::addPolygon
void Shape2D::addPolygon( const Vec2* vertices, u32 count, const Material& material )
{
	DC_BREAK_IF( count > Part::MaxVertices );

	Part part;
	part.type = Polygon;
	part.material = material;
	part.polygon.count = count;

	for( u32 i = 0; i < count; i++ ) {
		part.polygon.vertices[i * 2 + 0] = vertices[i].x;
		part.polygon.vertices[i * 2 + 1] = vertices[i].y;
	}

	addPart( part );
}

// ** Shape2D::serialize
void Shape2D::serialize( Ecs::SerializationContext& ctx, Io::KeyValue& ar ) const
{
    Io::KeyValue material = Io::KeyValue::object() << "density" << m_parts[0].material.density << "friction" << m_parts[0].material.friction << "restitution" << m_parts[0].material.restitution;

    switch( m_parts[0].type ) {
    case Polygon:   {
                        Io::KeyValue vertices = Io::KeyValue::array();

                        for( u32 i = 0; i < m_parts[0].polygon.count; i++ ) {
                            vertices << m_parts[0].polygon.vertices[i * 2 + 0] << m_parts[0].polygon.vertices[i * 2 + 1];
                        }

                        ar = Io::KeyValue::object() << "type" << "polygon" << "vertices" << vertices << "material" << material;
                    }
                    break;
    case Circle:    {
                        ar = Io::KeyValue::object() << "type" << "circle" << "radius" << m_parts[0].circle.radius << "material" << material;
                    }
                    break;
    default:        DC_BREAK;
    }
}

// ** Shape2D::deserialize
void Shape2D::deserialize( Ecs::SerializationContext& ctx, const Io::KeyValue& ar )
{
    // Get shape type
    const String& type = ar.get( "type", "" ).asString();

    // Get material
    Io::KeyValue material = ar.get( "material" );

    // Parse material
    Material shapeMaterial;

    if( material.isObject() ) {
        shapeMaterial.density       = material.get( "density", 1.0f ).asFloat();
        shapeMaterial.friction      = material.get( "friction", 0.2f ).asFloat();
        shapeMaterial.restitution   = material.get( "restitution", 0.0f ).asFloat();
    }

    if( type == "polygon" ) {
        const Io::KeyValue& vertices = ar.get( "vertices" );
        Array<Vec2>         points;

        for( s32 i = 0, n = vertices.size() / 2; i < n; i++ ) {
            points.push_back( Vec2( vertices[i * 2 + 0].asFloat(), vertices[i * 2 + 1].asFloat() ) );
        }

        addPolygon( &points[0], points.size(), shapeMaterial );
    }
    else if( type == "circle" ) {
        addCircle( ar.get( "radius", 0.0f ).asFloat(), 0.0f, 0.0f, shapeMaterial );
    }
    else {
        DC_BREAK;
    }
}

// ----------------------------------------- RigidBody2D ----------------------------------------- //

// ** RigidBody2D::RigidBody2D
RigidBody2D::RigidBody2D( f32 mass, Type type, u16 category, u16 collisionMask, bool isBullet )
	: m_mass( mass ), m_type( type ), m_linearDamping( 0.0f ), m_angularDamping( 0.0f ), m_torque( 0.0f ), m_category( category ), m_collisionMask( collisionMask ), m_gravityScale( 1.0f )
{
    m_flags.set( IsBullet, isBullet );
}

// ** RigidBody2D::mass
f32 RigidBody2D::mass( void ) const
{
	return m_mass;
}

// ** RigidBody2D::type
RigidBody2D::Type RigidBody2D::type( void ) const
{
	return m_type;
}

// ** RigidBody2D::linearDamping
f32 RigidBody2D::linearDamping( void ) const
{
	return m_linearDamping;
}

// ** RigidBody2D::setLinearDamping
void RigidBody2D::setLinearDamping( f32 value )
{
	m_linearDamping = value;
}

// ** RigidBody2D::angularDamping
f32 RigidBody2D::angularDamping( void ) const
{
	return m_angularDamping;
}

// ** RigidBody2D::setAngularDamping
void RigidBody2D::setAngularDamping( f32 value )
{
	m_angularDamping = value;
}

// ** RigidBody2D::gravityScale
f32 RigidBody2D::gravityScale( void ) const
{
    return m_gravityScale;
}

// ** RigidBody2D::setGravityScale
void RigidBody2D::setGravityScale( f32 value )
{
    m_gravityScale = value;
}

// ** RigidBody2D::moveTo
void RigidBody2D::moveTo( const Vec2& position )
{
    m_movedTo  = position;
    m_flags.on( WasMoved );
}

// ** RigidBody2D::movedTo
const Vec2& RigidBody2D::movedTo( void ) const
{
    return m_movedTo;
}

// ** RigidBody2D::wasMoved
bool RigidBody2D::wasMoved( void ) const
{
    return m_flags.is( WasMoved );
}

// ** RigidBody2D::putToRest
void RigidBody2D::putToRest( void )
{
    m_flags.on( WasPutToRest );
}

// ** RigidBody2D::wasPutToRest
bool RigidBody2D::wasPutToRest( void ) const
{
    return m_flags.is( WasPutToRest );
}

// ** RigidBody2D::isBullet
bool RigidBody2D::isBullet( void ) const
{
    return m_flags.is( IsBullet );
}

// ** RigidBody2D::torque
f32 RigidBody2D::torque( void ) const
{
	return m_torque;
}

// ** RigidBody2D::applyTorque
void RigidBody2D::applyTorque( f32 value )
{
	m_torque += value;
}

// ** RigidBody2D::force
const Vec2& RigidBody2D::force( void ) const
{
	return m_force;
}

// ** RigidBody2D::applyForc
void RigidBody2D::applyForce( const Vec2& value )
{
	m_force += value;
}

// ** RigidBody2D::category
u16 RigidBody2D::category( void ) const
{
    return m_category;
}

// ** RigidBody2D::collisionMask
u16 RigidBody2D::collisionMask( void ) const
{
    return m_collisionMask;
}

// ** RigidBody2D::applyForceToPoint
void RigidBody2D::applyForceToPoint( const Vec2& value, const Vec2& point )
{
	m_forces.push_back( AppliedForce( value, point ) );
}

// ** RigidBody2D::applyImpulse
void RigidBody2D::applyImpulse( const Vec2& value )
{
    m_impulses.push_back( AppliedForce( value, Vec2( 0.0f, 0.0f ) ) );
}

// ** RigidBody2D::applyImpulseToPoint
void RigidBody2D::applyImpulseToPoint( const Vec2& value, const Vec2& point )
{
    m_impulses.push_back( AppliedForce( value, point ) );
}

// ** RigidBody2D::appliedForceCount
u32 RigidBody2D::appliedForceCount( void ) const
{
	return ( u32 )m_forces.size();
}

// ** RigidBody2D::appliedImpulse
const RigidBody2D::AppliedForce& RigidBody2D::appliedImpulse( u32 index ) const
{
	DC_BREAK_IF( index >= appliedImpulseCount() );
	return m_impulses[index];
}

// ** RigidBody2D::appliedImpulseCount
u32 RigidBody2D::appliedImpulseCount( void ) const
{
	return ( u32 )m_impulses.size();
}

// ** RigidBody2D::appliedForce
const RigidBody2D::AppliedForce& RigidBody2D::appliedForce( u32 index ) const
{
	DC_BREAK_IF( index >= appliedForceCount() );
	return m_forces[index];
}

// ** RigidBody2D::collisionEventCount
u32 RigidBody2D::collisionEventCount( void ) const
{
    return ( u32 )m_collisionEvents.size();
}

// ** RigidBody2D::collisionEvent
const RigidBody2D::CollisionEvent& RigidBody2D::collisionEvent( u32 index ) const
{
    DC_BREAK_IF( index < 0 || index >= collisionEventCount() );
    return m_collisionEvents[index];
}

// ** RigidBody2D::queueCollisionEvent
void RigidBody2D::queueCollisionEvent( const CollisionEvent& e )
{
    m_collisionEvents.push_back( e );
}

// ** RigidBody2D::clear
void RigidBody2D::clear( void )
{
	m_torque = 0.0f;
	m_force  = Vec2( 0.0f, 0.0f );
    m_flags.off( WasPutToRest | WasMoved );
	m_forces.clear();
    m_impulses.clear();
}

// ** RigidBody2D::clearEvents
void RigidBody2D::clearEvents( void )
{
    m_collisionEvents.clear();
}

// ** RigidBody2D::linearVelocity
const Vec2& RigidBody2D::linearVelocity( void ) const
{
    return m_linearVelocity;
}

// ** RigidBody2D::setLinearVelocity
void RigidBody2D::setLinearVelocity( const Vec2& value )
{
    m_linearVelocity = value;
}

// ** RigidBody2D::updateMass
void RigidBody2D::updateMass( f32 value )
{
    m_mass = value;
}

} // namespace Scene

DC_END_DREEMCHEST<|endoftext|>


<commit_before>#include "Debugger.h"
#include "Cpu.h"
#include "CpuHelpers.h"
#include "CpuOpCodes.h"
#include "MemoryBus.h"
#include "Platform.h"
#include <array>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <vector>

namespace {
    template <typename T>
    T HexStringToIntegral(const char* s) {
        std::stringstream converter(s);
        T value;
        converter >> std::hex >> value;
        return value;
    }

    std::vector<std::string> Tokenize(const std::string& s) {
        std::vector<std::string> result;
        const char* whitespace = " \t";
        size_t startIndex = 0;
        while ((startIndex = s.find_first_not_of(whitespace, startIndex)) != std::string::npos) {
            size_t endIndex = s.find_first_of(whitespace, startIndex + 1);

            if (endIndex == std::string::npos) {
                result.emplace_back(s.substr(startIndex));
                break;
            } else {
                result.emplace_back(s.substr(startIndex, endIndex - startIndex));
                startIndex = endIndex;
            }
        }
        return result;
    }

    struct Instruction {
        const CpuOp& cpuOp;
        int page;
        std::array<uint8_t, 2> operands;
    };

    Instruction ReadInstruction(uint16_t opAddr, const MemoryBus& memoryBus) {
        int cpuOpPage = 0;
        uint8_t opCodeByte = memoryBus.Read(opAddr++);
        if (IsOpCodePage1(opCodeByte)) {
            cpuOpPage = 1;
            opCodeByte = memoryBus.Read(opAddr++);
        } else if (IsOpCodePage2(opCodeByte)) {
            cpuOpPage = 2;
            opCodeByte = memoryBus.Read(opAddr++);
        }

        const auto& cpuOp = LookupCpuOpRuntime(cpuOpPage, opCodeByte);
        const int numOperands = cpuOp.size - 1 - (cpuOpPage == 0 ? 0 : 1);

        Instruction result = {cpuOp, cpuOpPage};
        for (int i = 0; i < numOperands; ++i) {
            result.operands[i] = memoryBus.Read(opAddr++);
        }
        return result;
    }

    std::string DisassembleOp(const CpuRegisters& reg, const MemoryBus& memoryBus) {
        uint16_t opAddr = reg.PC;
        auto instruction = ReadInstruction(opAddr, memoryBus);
        const auto& cpuOp = instruction.cpuOp;

        std::string hexInstruction;
        // Output instruction in hex
        for (uint16_t i = 0; i < cpuOp.size; ++i)
            hexInstruction += FormattedString<>("%02x", memoryBus.Read(opAddr + i));

        std::string disasmInstruction, comment;

        switch (cpuOp.addrMode) {
        case AddressingMode::Inherent: {
            // Handle specific instructions that take operands (most inherent instructions don't)
            if (cpuOp.opCode == 0x1E /*EXG*/ || cpuOp.opCode == 0x1F /*TFR*/) {
                uint8_t postbyte = instruction.operands[0];
                uint8_t src = (postbyte >> 4) & 0b111;
                uint8_t dst = postbyte & 0b111;
                if (postbyte & BITS(3)) {
                    char* const regName[]{"A", "B", "CC", "DP"};
                    disasmInstruction =
                        FormattedString<>("%s %s,%s", cpuOp.name, regName[src], regName[dst]);
                } else {
                    char* const regName[]{"D", "X", "Y", "U", "S", "PC"};
                    disasmInstruction =
                        FormattedString<>("%s %s,%s", cpuOp.name, regName[src], regName[dst]);
                }
            } else {
                disasmInstruction = cpuOp.name;
            }
        } break;

        case AddressingMode::Immediate: {
            if (cpuOp.size == 2) {
                auto value = instruction.operands[0];
                disasmInstruction = FormattedString<>("%s #$%02x", cpuOp.name, value);
                comment = FormattedString<>("(%d)", value);
            } else {
                auto value = CombineToU16(instruction.operands[0], instruction.operands[1]);
                disasmInstruction = FormattedString<>("%s #$%04x", cpuOp.name, value);
                comment = FormattedString<>("(%d)", value);
            }
        } break;

        case AddressingMode::Extended: {
            auto msb = instruction.operands[0];
            auto lsb = instruction.operands[1];
            uint16_t EA = CombineToU16(msb, lsb);
            uint8_t value = memoryBus.Read(EA);
            disasmInstruction = FormattedString<>("%s $%04x", cpuOp.name, EA);
            comment = FormattedString<>("$%02x (%d)", value, value);
        } break;

        case AddressingMode::Direct: {
            uint16_t EA = CombineToU16(reg.DP, instruction.operands[0]);
            uint8_t value = memoryBus.Read(EA);
            disasmInstruction = FormattedString<>("%s $%02x", cpuOp.name, instruction.operands[0]);
            comment = FormattedString<>("DP:(PC) = $%02x = $%02x (%d)", EA, value, value);
        } break;

        case AddressingMode::Indexed: { //@TODO
            disasmInstruction = cpuOp.name;
        } break;

        case AddressingMode::Relative: {
            // Branch instruction with 8 or 16 bit signed relative offset
            auto nextPC = reg.PC + cpuOp.size;
            if (cpuOp.size == 2) {
                auto offset = static_cast<int8_t>(instruction.operands[0]);
                disasmInstruction = FormattedString<>("%s $%02x", cpuOp.name, U16(offset) & 0x00FF);
                comment = FormattedString<>("(%d), PC + offset = $%04x", offset, nextPC + offset);
            } else {
                assert(cpuOp.size == 3);
                auto offset = static_cast<int16_t>(
                    CombineToU16(instruction.operands[0], instruction.operands[1]));
                disasmInstruction = FormattedString<>("%s $%04x", cpuOp.name, offset);
                comment = FormattedString<>("(%d), PC + offset = $%04x", offset, nextPC + offset);
            }
        } break;

        case AddressingMode::Illegal: {
        case AddressingMode::Variant:
            assert(false);
        } break;
        }

        std::string result =
            FormattedString<>("%-10s %-10s %s", hexInstruction.c_str(), disasmInstruction.c_str(),
                              comment.size() > 0 ? ("# " + comment).c_str() : "");
        return result;
    };

    void PrintOp(const CpuRegisters& reg, const MemoryBus& memoryBus) {
        std::string op = DisassembleOp(reg, memoryBus);
        std::cout << FormattedString<>("[$%x] %s", reg.PC, op.c_str()) << std::endl;
    }

    void PrintRegisters(const CpuRegisters& reg) {
        const auto& cc = reg.CC;

        std::string CC = FormattedString<>(
            "%c%c%c%c%c%c%c%c", cc.Carry ? 'C' : 'c', cc.Overflow ? 'V' : 'v', cc.Zero ? 'Z' : 'z',
            cc.Negative ? 'N' : 'n', cc.InterruptMask ? 'I' : 'i', cc.HalfCarry ? 'H' : 'h',
            cc.FastInterruptMask ? 'F' : 'f', cc.Entire ? 'E' : 'e');

        std::cout << FormattedString<>("A=$%02x (%d) B=$%02x (%d) D=$%04x (%d) X=$%04x (%d) "
                                       "Y=$%04x (%d) U=$%04x S=$%04x DP=$%02x PC=$%04x CC=%s",
                                       reg.A, reg.A, reg.B, reg.B, reg.D, reg.D, reg.X, reg.X,
                                       reg.Y, reg.Y, reg.U, reg.S, reg.DP, reg.PC, CC.c_str())
                  << std::endl;
    }

    void PrintHelp() {
        std::cout << "s[tep]                step instruction\n"
                     "c[continue]           continue running\n"
                     "info reg[isters]      display register values\n"
                     "p[rint] <address>     display value add address\n"
                     "q[uit]                quit\n"
                     "h[help]               display this help text\n"
                  << std::flush;
    }

} // namespace

void Debugger::Init(MemoryBus& memoryBus, Cpu& cpu) {
    m_memoryBus = &memoryBus;
    m_cpu = &cpu;

    Platform::SetConsoleCtrlHandler([this] {
        m_breakIntoDebugger = true;
        return true;
    });
}

void Debugger::Run() {
    m_lastCommand = "step"; // Reasonable default

    // Break on start
    m_breakIntoDebugger = true;

    // Enable trace when running normally
    m_traceEnabled = true;

    while (true) {
        if (m_breakIntoDebugger) {
            std::cout << FormattedString<>("$%04x (%s)>", m_cpu->Registers().PC,
                                           m_lastCommand.c_str())
                      << std::flush;

            std::string input;
            const auto& stream = std::getline(std::cin, input);

            if (!stream) {
                // getline will fail under certain conditions, like when Ctrl+C is pressed, in which
                // case we just clear the stream status and restart the loop.
                std::cin.clear();
                std::cout << std::endl;
                continue;
            }

            auto tokens = Tokenize(input);

            // If no input, repeat last command
            if (tokens.size() == 0) {
                input = m_lastCommand;
                tokens = Tokenize(m_lastCommand);
            }

            bool validCommand = true;

            if (tokens.size() == 0) {
                // Don't do anything (no command entered yet)
            } else if (tokens[0] == "quit" || tokens[0] == "q") {
                return;

            } else if (tokens[0] == "help" || tokens[0] == "h") {
                PrintHelp();

            } else if (tokens[0] == "continue" || tokens[0] == "c") {
                m_breakIntoDebugger = false;

            } else if (tokens[0] == "step" || tokens[0] == "s") {
                // "Step into"
                PrintOp(m_cpu->Registers(), *m_memoryBus);
                m_cpu->ExecuteInstruction();

            } else if (tokens[0] == "info") {
                if (tokens.size() > 1 && (tokens[1] == "registers" || tokens[1] == "reg")) {
                    PrintRegisters(m_cpu->Registers());
                } else {
                    validCommand = false;
                }

            } else if (tokens[0] == "print" || tokens[0] == "p") {
                if (tokens.size() > 1 && tokens[1][0] == '$') {
                    uint16_t address = HexStringToIntegral<uint16_t>(tokens[1].substr(1).c_str());
                    uint8_t value = m_memoryBus->Read(address);
                    std::cout << FormattedString<>("$%04x = $%02x (%d)", address, value, value)
                              << std::endl;
                } else {
                    validCommand = false;
                }

            } else {
                validCommand = false;
            }

            if (validCommand) {
                m_lastCommand = input;
            } else {
                std::cout << "Invalid command: " << input << std::endl;
            }

        } else {
            if (m_traceEnabled)
                PrintOp(m_cpu->Registers(), *m_memoryBus);

            m_cpu->ExecuteInstruction();
        }
    }
}
<commit_msg>Disassemble indexed addressing mode instructions<commit_after>#include "Debugger.h"
#include "Cpu.h"
#include "CpuHelpers.h"
#include "CpuOpCodes.h"
#include "MemoryBus.h"
#include "Platform.h"
#include <array>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <vector>

namespace {
    template <typename T>
    T HexStringToIntegral(const char* s) {
        std::stringstream converter(s);
        T value;
        converter >> std::hex >> value;
        return value;
    }

    std::vector<std::string> Tokenize(const std::string& s) {
        std::vector<std::string> result;
        const char* whitespace = " \t";
        size_t startIndex = 0;
        while ((startIndex = s.find_first_not_of(whitespace, startIndex)) != std::string::npos) {
            size_t endIndex = s.find_first_of(whitespace, startIndex + 1);

            if (endIndex == std::string::npos) {
                result.emplace_back(s.substr(startIndex));
                break;
            } else {
                result.emplace_back(s.substr(startIndex, endIndex - startIndex));
                startIndex = endIndex;
            }
        }
        return result;
    }

    const char* GetRegisterName(const CpuRegisters& cpuRegisters, const uint8_t& r) {
        ptrdiff_t offset =
            reinterpret_cast<const uint8_t*>(&r) - reinterpret_cast<const uint8_t*>(&cpuRegisters);
        switch (offset) {
        case offsetof(CpuRegisters, A):
            return "A";
        case offsetof(CpuRegisters, B):
            return "B";
        case offsetof(CpuRegisters, DP):
            return "DP";
        case offsetof(CpuRegisters, CC):
            return "CC";
        default:
            assert(false);
            return "INVALID";
        }
    };

    const char* GetRegisterName(const CpuRegisters& cpuRegisters, const uint16_t& r) {
        ptrdiff_t offset =
            reinterpret_cast<const uint8_t*>(&r) - reinterpret_cast<const uint8_t*>(&cpuRegisters);
        switch (offset) {
        case offsetof(CpuRegisters, X):
            return "X";
        case offsetof(CpuRegisters, Y):
            return "Y";
        case offsetof(CpuRegisters, U):
            return "U";
        case offsetof(CpuRegisters, S):
            return "S";
        case offsetof(CpuRegisters, PC):
            return "PC";
        case offsetof(CpuRegisters, D):
            return "D";
        default:
            assert(false);
            return "INVALID";
        }
    };

    struct Instruction {
        const CpuOp& cpuOp;
        int page;
        std::array<uint8_t, 3> operands;
    };

    Instruction ReadInstruction(uint16_t opAddr, const MemoryBus& memoryBus) {
        int cpuOpPage = 0;
        uint8_t opCodeByte = memoryBus.Read(opAddr++);
        if (IsOpCodePage1(opCodeByte)) {
            cpuOpPage = 1;
            opCodeByte = memoryBus.Read(opAddr++);
        } else if (IsOpCodePage2(opCodeByte)) {
            cpuOpPage = 2;
            opCodeByte = memoryBus.Read(opAddr++);
        }

        const auto& cpuOp = LookupCpuOpRuntime(cpuOpPage, opCodeByte);
        Instruction result = {cpuOp, cpuOpPage};

        // Always read max operand bytes, even if they're not used. We would want to only read as
        // many operands as stipulated by the opcode's size - 1 (or 2 if not on page 0), but
        // unfortunately, indexed instructions sometimes read an extra operand byte, determined
        // dynamically. So for disassembling purposes, we just read 3 bytes and call it a day.
        for (auto& operand : result.operands) {
            operand = memoryBus.Read(opAddr++);
        }
        return result;
    }

    void DisassembleIndexedInstruction(const Instruction& instruction,
                                       const CpuRegisters& cpuRegisters, const MemoryBus& memoryBus,
                                       std::string& disasmInstruction, std::string& comment) {
        auto RegisterSelect = [&cpuRegisters](uint8_t postbyte) -> const uint16_t& {
            switch ((postbyte >> 5) & 0b11) {
            case 0b00:
                return cpuRegisters.X;
            case 0b01:
                return cpuRegisters.Y;
            case 0b10:
                return cpuRegisters.U;
            default: // 0b11:
                return cpuRegisters.S;
            }
        };

        uint16_t EA = 0;
        uint8_t postbyte = instruction.operands[0];
        bool supportsIndirect = true;
        std::string operands;

        if ((postbyte & BITS(7)) == 0) // (+/- 4 bit offset),R
        {
            // postbyte is a 5 bit two's complement number we convert to 8 bit.
            // So if bit 4 is set (sign bit), we extend the sign bit by turning on bits 6,7,8;
            uint8_t offset = postbyte & 0b0000'1111;
            if (postbyte & BITS(4))
                offset |= 0b1110'0000;
            auto& reg = RegisterSelect(postbyte);
            EA = reg + S16(offset);
            supportsIndirect = false;

            operands = FormattedString<>("%d,%s", offset, GetRegisterName(cpuRegisters, reg));
            comment = FormattedString<>("%d,$%04x", offset, reg);
        } else {
            switch (postbyte & 0b1111) {
            case 0b0000: { // ,R+
                auto reg = RegisterSelect(postbyte);
                EA = reg;
                reg += 1;
                supportsIndirect = false;

                operands = FormattedString<>(",%s+", GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>(",$%04x", reg);
            } break;
            case 0b0001: { // ,R++
                auto reg = RegisterSelect(postbyte);
                EA = reg;
                reg += 2;

                operands = FormattedString<>(",%s++", GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>(",$%04x", reg);
            } break;
            case 0b0010: { // ,-R
                auto reg = RegisterSelect(postbyte);
                reg -= 1;
                EA = reg;
                supportsIndirect = false;

                operands = FormattedString<>(",-%s", GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>(",$%04x", reg);
            } break;
            case 0b0011: { // ,--R
                auto reg = RegisterSelect(postbyte);
                reg -= 2;
                EA = reg;

                operands = FormattedString<>(",--%s", GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>(",$%04x", reg);
            } break;
            case 0b0100: { // ,R
                auto& reg = RegisterSelect(postbyte);
                EA = reg;

                operands = FormattedString<>(",%s", GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>(",$%04x", reg);
            } break;
            case 0b0101: { // (+/- B),R
                auto& reg = RegisterSelect(postbyte);
                auto offset = S16(cpuRegisters.B);
                EA = reg + offset;

                operands = FormattedString<>("B,%s", GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>("%d,$%04x", offset, reg);
            } break;
            case 0b0110: { // (+/- A),R
                auto& reg = RegisterSelect(postbyte);
                auto offset = S16(cpuRegisters.A);
                EA = reg + offset;

                operands = FormattedString<>("A,%s", GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>("%d,$%04x", offset, reg);
            } break;
            case 0b0111:
                FAIL("Illegal");
                break;
            case 0b1000: { // (+/- 7 bit offset),R
                auto& reg = RegisterSelect(postbyte);
                uint8_t postbyte2 = instruction.operands[2];
                auto offset = S16(postbyte2);
                EA = reg + offset;

                operands = FormattedString<>("%d,%s", offset, GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>("%d,$%04x", offset, reg);
            } break;
            case 0b1001: { // (+/- 15 bit offset),R
                uint8_t postbyte2 = instruction.operands[2];
                uint8_t postbyte3 = instruction.operands[3];
                auto& reg = RegisterSelect(postbyte);
                auto offset = CombineToS16(postbyte2, postbyte3);
                EA = reg + offset;

                operands = FormattedString<>("%d,%s", offset, GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>("%d,$%04x", offset, reg);
            } break;
            case 0b1010:
                FAIL("Illegal");
                break;
            case 0b1011: { // (+/- D),R
                auto& reg = RegisterSelect(postbyte);
                auto offset = S16(cpuRegisters.D);
                EA = reg + offset;

                operands = FormattedString<>("D,%s", GetRegisterName(cpuRegisters, reg));
                comment = FormattedString<>("%d,$%04x", offset, reg);
            } break;
            case 0b1100: { // (+/- 7 bit offset),PC
                uint8_t postbyte2 = instruction.operands[2];
                auto offset = S16(postbyte2);
                EA = cpuRegisters.PC + offset;

                operands = FormattedString<>("%d,PC", offset);
                comment = FormattedString<>("%d,$%04x", offset, cpuRegisters.PC);
            } break;
            case 0b1101: { // (+/- 15 bit offset),PC
                uint8_t postbyte2 = instruction.operands[2];
                uint8_t postbyte3 = instruction.operands[3];
                auto offset = CombineToS16(postbyte2, postbyte3);
                EA = cpuRegisters.PC + offset;

                operands = FormattedString<>("%d,PC", offset);
                comment = FormattedString<>("%d,$%04x", offset, cpuRegisters.PC);
            } break;
            case 0b1110:
                FAIL("Illegal");
                break;
            case 0b1111: { // [address] (Indirect-only)
                uint8_t postbyte2 = instruction.operands[2];
                uint8_t postbyte3 = instruction.operands[3];
                EA = CombineToS16(postbyte2, postbyte3);
            } break;
            default:
                FAIL("Illegal");
                break;
            }
        }

        if (supportsIndirect && (postbyte & BITS(4))) {
            uint8_t msb = memoryBus.Read(EA);
            uint8_t lsb = memoryBus.Read(EA + 1);
            EA = CombineToU16(lsb, msb);
            operands = "[" + operands + "]";
        }

        disasmInstruction = std::string(instruction.cpuOp.name) + " " + operands;
        uint8_t value = memoryBus.Read(EA);
        comment += FormattedString<>(" = $%04x = $%02x (%d)", EA, value, value);
    }

    std::string DisassembleOp(const CpuRegisters& cpuRegisters, const MemoryBus& memoryBus) {
        uint16_t opAddr = cpuRegisters.PC;
        auto instruction = ReadInstruction(opAddr, memoryBus);
        const auto& cpuOp = instruction.cpuOp;

        std::string hexInstruction;
        // Output instruction in hex
        for (uint16_t i = 0; i < cpuOp.size; ++i)
            hexInstruction += FormattedString<>("%02x", memoryBus.Read(opAddr + i));

        std::string disasmInstruction, comment;

        switch (cpuOp.addrMode) {
        case AddressingMode::Inherent: {
            // Handle specific instructions that take operands (most inherent instructions don't)
            if (cpuOp.opCode == 0x1E /*EXG*/ || cpuOp.opCode == 0x1F /*TFR*/) {
                uint8_t postbyte = instruction.operands[0];
                uint8_t src = (postbyte >> 4) & 0b111;
                uint8_t dst = postbyte & 0b111;
                if (postbyte & BITS(3)) {
                    char* const regName[]{"A", "B", "CC", "DP"};
                    disasmInstruction =
                        FormattedString<>("%s %s,%s", cpuOp.name, regName[src], regName[dst]);
                } else {
                    char* const regName[]{"D", "X", "Y", "U", "S", "PC"};
                    disasmInstruction =
                        FormattedString<>("%s %s,%s", cpuOp.name, regName[src], regName[dst]);
                }
            } else {
                disasmInstruction = cpuOp.name;
            }
        } break;

        case AddressingMode::Immediate: {
            if (cpuOp.size == 2) {
                auto value = instruction.operands[0];
                disasmInstruction = FormattedString<>("%s #$%02x", cpuOp.name, value);
                comment = FormattedString<>("(%d)", value);
            } else {
                auto value = CombineToU16(instruction.operands[0], instruction.operands[1]);
                disasmInstruction = FormattedString<>("%s #$%04x", cpuOp.name, value);
                comment = FormattedString<>("(%d)", value);
            }
        } break;

        case AddressingMode::Extended: {
            auto msb = instruction.operands[0];
            auto lsb = instruction.operands[1];
            uint16_t EA = CombineToU16(msb, lsb);
            uint8_t value = memoryBus.Read(EA);
            disasmInstruction = FormattedString<>("%s $%04x", cpuOp.name, EA);
            comment = FormattedString<>("$%02x (%d)", value, value);
        } break;

        case AddressingMode::Direct: {
            uint16_t EA = CombineToU16(cpuRegisters.DP, instruction.operands[0]);
            uint8_t value = memoryBus.Read(EA);
            disasmInstruction = FormattedString<>("%s $%02x", cpuOp.name, instruction.operands[0]);
            comment = FormattedString<>("DP:(PC) = $%02x = $%02x (%d)", EA, value, value);
        } break;

        case AddressingMode::Indexed: {
            DisassembleIndexedInstruction(instruction, cpuRegisters, memoryBus, disasmInstruction,
                                          comment);
        } break;

        case AddressingMode::Relative: {
            // Branch instruction with 8 or 16 bit signed relative offset
            auto nextPC = cpuRegisters.PC + cpuOp.size;
            if (cpuOp.size == 2) {
                auto offset = static_cast<int8_t>(instruction.operands[0]);
                disasmInstruction = FormattedString<>("%s $%02x", cpuOp.name, U16(offset) & 0x00FF);
                comment = FormattedString<>("(%d), PC + offset = $%04x", offset, nextPC + offset);
            } else {
                assert(cpuOp.size == 3);
                auto offset = static_cast<int16_t>(
                    CombineToU16(instruction.operands[0], instruction.operands[1]));
                disasmInstruction = FormattedString<>("%s $%04x", cpuOp.name, offset);
                comment = FormattedString<>("(%d), PC + offset = $%04x", offset, nextPC + offset);
            }
        } break;

        case AddressingMode::Illegal: {
        case AddressingMode::Variant:
            assert(false);
        } break;
        }

        std::string result =
            FormattedString<>("%-10s %-10s %s", hexInstruction.c_str(), disasmInstruction.c_str(),
                              comment.size() > 0 ? ("# " + comment).c_str() : "");
        return result;
    };

    void PrintOp(const CpuRegisters& cpuRegisters, const MemoryBus& memoryBus) {
        std::string op = DisassembleOp(cpuRegisters, memoryBus);
        std::cout << FormattedString<>("[$%x] %s", cpuRegisters.PC, op.c_str()) << std::endl;
    }

    void PrintRegisters(const CpuRegisters& cpuRegisters) {
        const auto& cc = cpuRegisters.CC;

        std::string CC = FormattedString<>(
            "%c%c%c%c%c%c%c%c", cc.Carry ? 'C' : 'c', cc.Overflow ? 'V' : 'v', cc.Zero ? 'Z' : 'z',
            cc.Negative ? 'N' : 'n', cc.InterruptMask ? 'I' : 'i', cc.HalfCarry ? 'H' : 'h',
            cc.FastInterruptMask ? 'F' : 'f', cc.Entire ? 'E' : 'e');

        const auto& r = cpuRegisters;
        std::cout << FormattedString<>("A=$%02x (%d) B=$%02x (%d) D=$%04x (%d) X=$%04x (%d) "
                                       "Y=$%04x (%d) U=$%04x S=$%04x DP=$%02x PC=$%04x CC=%s",
                                       r.A, r.A, r.B, r.B, r.D, r.D, r.X, r.X, r.Y, r.Y, r.U, r.S,
                                       r.DP, r.PC, CC.c_str())
                  << std::endl;
    }

    void PrintHelp() {
        std::cout << "s[tep]                step instruction\n"
                     "c[continue]           continue running\n"
                     "info reg[isters]      display register values\n"
                     "p[rint] <address>     display value add address\n"
                     "q[uit]                quit\n"
                     "h[help]               display this help text\n"
                  << std::flush;
    }

} // namespace

void Debugger::Init(MemoryBus& memoryBus, Cpu& cpu) {
    m_memoryBus = &memoryBus;
    m_cpu = &cpu;

    Platform::SetConsoleCtrlHandler([this] {
        m_breakIntoDebugger = true;
        return true;
    });
}

void Debugger::Run() {
    m_lastCommand = "step"; // Reasonable default

    // Break on start
    m_breakIntoDebugger = true;

    // Enable trace when running normally
    m_traceEnabled = true;

    while (true) {
        if (m_breakIntoDebugger) {
            std::cout << FormattedString<>("$%04x (%s)>", m_cpu->Registers().PC,
                                           m_lastCommand.c_str())
                      << std::flush;

            std::string input;
            const auto& stream = std::getline(std::cin, input);

            if (!stream) {
                // getline will fail under certain conditions, like when Ctrl+C is pressed, in which
                // case we just clear the stream status and restart the loop.
                std::cin.clear();
                std::cout << std::endl;
                continue;
            }

            auto tokens = Tokenize(input);

            // If no input, repeat last command
            if (tokens.size() == 0) {
                input = m_lastCommand;
                tokens = Tokenize(m_lastCommand);
            }

            bool validCommand = true;

            if (tokens.size() == 0) {
                // Don't do anything (no command entered yet)
            } else if (tokens[0] == "quit" || tokens[0] == "q") {
                return;

            } else if (tokens[0] == "help" || tokens[0] == "h") {
                PrintHelp();

            } else if (tokens[0] == "continue" || tokens[0] == "c") {
                m_breakIntoDebugger = false;

            } else if (tokens[0] == "step" || tokens[0] == "s") {
                // "Step into"
                PrintOp(m_cpu->Registers(), *m_memoryBus);
                m_cpu->ExecuteInstruction();

            } else if (tokens[0] == "info") {
                if (tokens.size() > 1 && (tokens[1] == "registers" || tokens[1] == "reg")) {
                    PrintRegisters(m_cpu->Registers());
                } else {
                    validCommand = false;
                }

            } else if (tokens[0] == "print" || tokens[0] == "p") {
                if (tokens.size() > 1 && tokens[1][0] == '$') {
                    uint16_t address = HexStringToIntegral<uint16_t>(tokens[1].substr(1).c_str());
                    uint8_t value = m_memoryBus->Read(address);
                    std::cout << FormattedString<>("$%04x = $%02x (%d)", address, value, value)
                              << std::endl;
                } else {
                    validCommand = false;
                }

            } else {
                validCommand = false;
            }

            if (validCommand) {
                m_lastCommand = input;
            } else {
                std::cout << "Invalid command: " << input << std::endl;
            }

        } else {
            if (m_traceEnabled)
                PrintOp(m_cpu->Registers(), *m_memoryBus);

            m_cpu->ExecuteInstruction();
        }
    }
}
<|endoftext|>