<commit_before>/* bzflag
* Copyright (c) 1993 - 2005 Tim Riker
*
* This package is free software;  you can redistribute it and/or
* modify it under the terms of the license found in the file
* named COPYING that should have accompanied this file.
*
* THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
* WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*/

// bzflag global common header
#include "common.h"
#include "global.h"

// interface header
#include "effectsRenderer.h"

// system headers
#include <string>
#include <vector>

// common impl headers
#include "bzfgl.h"
#include "OpenGLGState.h"
#include "OpenGLMaterial.h"
#include "TextureManager.h"
#include "StateDatabase.h"
#include "BZDBCache.h"
#include "TimeKeeper.h"
#include "TextUtils.h"
#include "ParseColor.h"

// local impl headers
#include "SceneRenderer.h"
#include "Intersect.h"
#include "RoofTops.h"

EffectsRenderer::EffectsRenderer()
{
}

EffectsRenderer::~EffectsRenderer()
{
	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		delete(effectsList[i]);

	effectsList.clear();
}

void EffectsRenderer::init(void)
{
	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		delete(effectsList[i]);

	effectsList.clear();
}

void EffectsRenderer::update(void)
{
	tvEffectsList::iterator itr = effectsList.begin();

	float time = (float)TimeKeeper::getCurrent().getSeconds();

	while ( itr != effectsList.end() )
	{
		if ( (*itr)->update(time) )
		{
			delete((*itr));
			itr = effectsList.erase(itr);
		}
		else
			itr++;
	}
}

void EffectsRenderer::draw(const SceneRenderer& sr)
{
	// really should check here for only the things that are VISIBILE!!!

	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		effectsList[i]->draw(sr);
}

void EffectsRenderer::freeContext(void)
{
	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		effectsList[i]->freeContext();
}

void EffectsRenderer::rebuildContext(void)
{
	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		effectsList[i]->rebuildContext();
}

void EffectsRenderer::addSpawnFlash ( int team, float* pos )
{
	SpawnFlashEffect	*flash = new SpawnFlashEffect;
	flash->setPos(pos,NULL);
	flash->setStartTime((float)TimeKeeper::getCurrent().getSeconds());
	flash->setTeam(team);

	effectsList.push_back(flash);
}

//****************** effects base class*******************************
BasicEffect::BasicEffect()
{
	position[0] = position[1] = position[2] = 0.0f;
	rotation[0] = rotation[1] = rotation[2] = 0.0f;
	teamColor = -1;
	startTime = (float)TimeKeeper::getCurrent().getSeconds();

	lifetime = 0;
	lastTime = startTime;
	deltaTime = 0;
}

void BasicEffect::setPos ( float *pos, float *rot )
{
	if (pos)
	{
		position[0] = pos[0];
		position[1] = pos[1];
		position[2] = pos[2];
	}

	if (rot)
	{
		rotation[0] = rot[0];
		rotation[1] = rot[1];
		rotation[2] = rot[2];
	}
}

void BasicEffect::setTeam ( int team )
{
	teamColor = team;
}

void BasicEffect::setStartTime ( float time )
{
	startTime = time;
	lastTime = time;
	deltaTime = 0;
}

bool BasicEffect::update( float time )
{
	age = time - startTime;

	if ( age >= lifetime)
		return true;

	deltaTime = time - lastTime;
	lastTime = time;
	return false;
}

SpawnFlashEffect::SpawnFlashEffect() : BasicEffect()
{
	texture = TextureManager::instance().getTextureID("blend_flash",false);
	lifetime = 2.5f;
	radius = 1.75f;

	OpenGLGStateBuilder gstate;
	gstate.reset();
	gstate.setShading();
	gstate.setBlending((GLenum) GL_SRC_ALPHA,(GLenum) GL_ONE_MINUS_SRC_ALPHA);
	gstate.setAlphaFunc();

	if (texture >-1)
		gstate.setTexture(texture);

	ringState = gstate.getState();
}

SpawnFlashEffect::~SpawnFlashEffect()
{
}

bool SpawnFlashEffect::update ( float time )
{
	// see if it's time to die
	// if not update all those fun times
	if ( BasicEffect::update(time))
		return true;

	// nope it's not.
	// we live another day
	// do stuff that maybe need to be done every time to animage

	radius += deltaTime*4;
	return false;
}

void SpawnFlashEffect::draw ( const SceneRenderer& sr )
{
	glPushMatrix();

	glTranslatef(position[0],position[1],position[2]+0.1f);

	ringState.setState();

	glColor4f(1,1,1,1.0f-(age/lifetime));
	glDepthMask(0);

	drawRing(radius*0.1f,1.5f+(age*2));
	drawRing(radius*0.5f,1.5f,0.5f,0.5f);
	drawRing(radius,2);

	glColor4f(1,1,1,1);
	glDepthMask(1);
	glPopMatrix();
}

#define deg2Rad 0.017453292519943295769236907684886f

void RadialToCartesian ( float angle, float rad, float *pos )
{
	pos[0] = sin(angle*deg2Rad)*rad;
	pos[1] = cos(angle*deg2Rad)*rad;
}

void SpawnFlashEffect::drawRing ( float rad, float z, float topsideOffset, float bottomUV )
{
	int segements = 32;

	for ( int i = 0; i < segements; i ++)
	{
		float thisAng = 360.0f/segements * i;
		float nextAng = 360.0f/segements * (i+1);
		if ( i+1 >= segements )
			nextAng = 0;

		float thispos[2];
		float nextPos[2];

		float thispos2[2];
		float nextPos2[2];

		float thisNormal[3] = {0};
		float nextNormal[3] = {0};

		RadialToCartesian(thisAng,rad,thispos);
		RadialToCartesian(thisAng,1,thisNormal);
		RadialToCartesian(nextAng,rad,nextPos);
		RadialToCartesian(nextAng,1,nextNormal);

		RadialToCartesian(thisAng,rad+topsideOffset,thispos2);
		RadialToCartesian(nextAng,rad+topsideOffset,nextPos2);

		glBegin(GL_QUADS);

		// the "inside"
			glNormal3f(-thisNormal[0],-thisNormal[1],-thisNormal[2]);
			glTexCoord2f(0,bottomUV);
			glVertex3f(thispos[0],thispos[1],0);

			glNormal3f(-nextNormal[0],-nextNormal[1],-nextNormal[2]);
			glTexCoord2f(1,bottomUV);
			glVertex3f(nextPos[0],nextPos[1],0);

			glNormal3f(-nextNormal[0],-nextNormal[1],-nextNormal[2]);
			glTexCoord2f(1,1);
			glVertex3f(nextPos2[0],nextPos2[1],z);

			glNormal3f(-thisNormal[0],-thisNormal[1],-thisNormal[2]);
			glTexCoord2f(0,1);
			glVertex3f(thispos2[0],thispos2[1],z);

		// the "outside"

			glNormal3f(thisNormal[0],thisNormal[1],thisNormal[2]);
			glTexCoord2f(0,1);
			glVertex3f(thispos2[0],thispos2[1],z);

			glNormal3f(nextNormal[0],nextNormal[1],nextNormal[2]);
			glTexCoord2f(1,1);
			glVertex3f(nextPos2[0],nextPos2[1],z);

			glNormal3f(nextNormal[0],nextNormal[1],nextNormal[2]);
			glTexCoord2f(1,bottomUV);
			glVertex3f(nextPos[0],nextPos[1],0);

			glNormal3f(thisNormal[0],thisNormal[1],thisNormal[2]);
			glTexCoord2f(0,bottomUV);
			glVertex3f(thispos[0],thispos[1],0);

		glEnd();

	}
}

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8

<commit_msg>make spawn effect use team color<commit_after>/* bzflag
* Copyright (c) 1993 - 2005 Tim Riker
*
* This package is free software;  you can redistribute it and/or
* modify it under the terms of the license found in the file
* named COPYING that should have accompanied this file.
*
* THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
* WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*/

// bzflag global common header
#include "common.h"
#include "global.h"

// interface header
#include "effectsRenderer.h"

// system headers
#include <string>
#include <vector>

// common impl headers
#include "bzfgl.h"
#include "OpenGLGState.h"
#include "OpenGLMaterial.h"
#include "TextureManager.h"
#include "StateDatabase.h"
#include "BZDBCache.h"
#include "TimeKeeper.h"
#include "TextUtils.h"
#include "ParseColor.h"

// local impl headers
#include "SceneRenderer.h"
#include "Intersect.h"
#include "RoofTops.h"

EffectsRenderer::EffectsRenderer()
{
}

EffectsRenderer::~EffectsRenderer()
{
	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		delete(effectsList[i]);

	effectsList.clear();
}

void EffectsRenderer::init(void)
{
	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		delete(effectsList[i]);

	effectsList.clear();
}

void EffectsRenderer::update(void)
{
	tvEffectsList::iterator itr = effectsList.begin();

	float time = (float)TimeKeeper::getCurrent().getSeconds();

	while ( itr != effectsList.end() )
	{
		if ( (*itr)->update(time) )
		{
			delete((*itr));
			itr = effectsList.erase(itr);
		}
		else
			itr++;
	}
}

void EffectsRenderer::draw(const SceneRenderer& sr)
{
	// really should check here for only the things that are VISIBILE!!!

	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		effectsList[i]->draw(sr);
}

void EffectsRenderer::freeContext(void)
{
	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		effectsList[i]->freeContext();
}

void EffectsRenderer::rebuildContext(void)
{
	for ( unsigned int i = 0; i < effectsList.size(); i++ )
		effectsList[i]->rebuildContext();
}

void EffectsRenderer::addSpawnFlash ( int team, float* pos )
{
	SpawnFlashEffect	*flash = new SpawnFlashEffect;
	flash->setPos(pos,NULL);
	flash->setStartTime((float)TimeKeeper::getCurrent().getSeconds());
	flash->setTeam(team);

	effectsList.push_back(flash);
}

//****************** effects base class*******************************
BasicEffect::BasicEffect()
{
	position[0] = position[1] = position[2] = 0.0f;
	rotation[0] = rotation[1] = rotation[2] = 0.0f;
	teamColor = -1;
	startTime = (float)TimeKeeper::getCurrent().getSeconds();

	lifetime = 0;
	lastTime = startTime;
	deltaTime = 0;
}

void BasicEffect::setPos ( float *pos, float *rot )
{
	if (pos)
	{
		position[0] = pos[0];
		position[1] = pos[1];
		position[2] = pos[2];
	}

	if (rot)
	{
		rotation[0] = rot[0];
		rotation[1] = rot[1];
		rotation[2] = rot[2];
	}
}

void BasicEffect::setTeam ( int team )
{
	teamColor = team;
}

void BasicEffect::setStartTime ( float time )
{
	startTime = time;
	lastTime = time;
	deltaTime = 0;
}

bool BasicEffect::update( float time )
{
	age = time - startTime;

	if ( age >= lifetime)
		return true;

	deltaTime = time - lastTime;
	lastTime = time;
	return false;
}

SpawnFlashEffect::SpawnFlashEffect() : BasicEffect()
{
	texture = TextureManager::instance().getTextureID("blend_flash",false);
	lifetime = 2.5f;
	radius = 1.75f;

	OpenGLGStateBuilder gstate;
	gstate.reset();
	gstate.setShading();
	gstate.setBlending((GLenum) GL_SRC_ALPHA,(GLenum) GL_ONE_MINUS_SRC_ALPHA);
	gstate.setAlphaFunc();

	if (texture >-1)
		gstate.setTexture(texture);

	ringState = gstate.getState();
}

SpawnFlashEffect::~SpawnFlashEffect()
{
}

bool SpawnFlashEffect::update ( float time )
{
	// see if it's time to die
	// if not update all those fun times
	if ( BasicEffect::update(time))
		return true;

	// nope it's not.
	// we live another day
	// do stuff that maybe need to be done every time to animage

	radius += deltaTime*4;
	return false;
}

void SpawnFlashEffect::draw ( const SceneRenderer& sr )
{
	glPushMatrix();

	glTranslatef(position[0],position[1],position[2]+0.1f);

	ringState.setState();

	float color[3] = {0};
	switch(teamColor)
	{
	default:
		color[0] = color[1] = color[2] = 1;
		break;

	case BlueTeam:
		color[0] = 0.25f;
		color[1] = 0.25f;
		color[2] = 1;
		break;

	case GreenTeam:
		color[0] = 0.25f;
		color[1] = 1;
		color[2] = 0.25f;
		break;

	case RedTeam:
		color[0] = 1;
		color[1] = 0.25f;
		color[2] = 0.25f;
		break;

	case PurpleTeam:
		color[0] = 1;
		color[1] = 0.25f;
		color[2] = 1.0f;
		break;

	case RogueTeam:
		color[0] = 0.25;
		color[1] = 0.25f;
		color[2] = 0.25f;
		break;
	}

	glColor4f(color[0],color[1],color[2],1.0f-(age/lifetime));
	glDepthMask(0);

	drawRing(radius*0.1f,1.5f+(age*2));
	drawRing(radius*0.5f,1.5f,0.5f,0.5f);
	drawRing(radius,2);

	glColor4f(1,1,1,1);
	glDepthMask(1);
	glPopMatrix();
}

#define deg2Rad 0.017453292519943295769236907684886f

void RadialToCartesian ( float angle, float rad, float *pos )
{
	pos[0] = sin(angle*deg2Rad)*rad;
	pos[1] = cos(angle*deg2Rad)*rad;
}

void SpawnFlashEffect::drawRing ( float rad, float z, float topsideOffset, float bottomUV )
{
	int segements = 32;

	for ( int i = 0; i < segements; i ++)
	{
		float thisAng = 360.0f/segements * i;
		float nextAng = 360.0f/segements * (i+1);
		if ( i+1 >= segements )
			nextAng = 0;

		float thispos[2];
		float nextPos[2];

		float thispos2[2];
		float nextPos2[2];

		float thisNormal[3] = {0};
		float nextNormal[3] = {0};

		RadialToCartesian(thisAng,rad,thispos);
		RadialToCartesian(thisAng,1,thisNormal);
		RadialToCartesian(nextAng,rad,nextPos);
		RadialToCartesian(nextAng,1,nextNormal);

		RadialToCartesian(thisAng,rad+topsideOffset,thispos2);
		RadialToCartesian(nextAng,rad+topsideOffset,nextPos2);

		glBegin(GL_QUADS);

		// the "inside"
			glNormal3f(-thisNormal[0],-thisNormal[1],-thisNormal[2]);
			glTexCoord2f(0,bottomUV);
			glVertex3f(thispos[0],thispos[1],0);

			glNormal3f(-nextNormal[0],-nextNormal[1],-nextNormal[2]);
			glTexCoord2f(1,bottomUV);
			glVertex3f(nextPos[0],nextPos[1],0);

			glNormal3f(-nextNormal[0],-nextNormal[1],-nextNormal[2]);
			glTexCoord2f(1,1);
			glVertex3f(nextPos2[0],nextPos2[1],z);

			glNormal3f(-thisNormal[0],-thisNormal[1],-thisNormal[2]);
			glTexCoord2f(0,1);
			glVertex3f(thispos2[0],thispos2[1],z);

		// the "outside"

			glNormal3f(thisNormal[0],thisNormal[1],thisNormal[2]);
			glTexCoord2f(0,1);
			glVertex3f(thispos2[0],thispos2[1],z);

			glNormal3f(nextNormal[0],nextNormal[1],nextNormal[2]);
			glTexCoord2f(1,1);
			glVertex3f(nextPos2[0],nextPos2[1],z);

			glNormal3f(nextNormal[0],nextNormal[1],nextNormal[2]);
			glTexCoord2f(1,bottomUV);
			glVertex3f(nextPos[0],nextPos[1],0);

			glNormal3f(thisNormal[0],thisNormal[1],thisNormal[2]);
			glTexCoord2f(0,bottomUV);
			glVertex3f(thispos[0],thispos[1],0);

		glEnd();

	}
}

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8

<|endoftext|>


<commit_before>// Copyright 2016 Yahoo Inc. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.
#include <util/timer.h>
#include <httpclient/httpclient.h>
#include <util/filereader.h>
#include "client.h"

Client::Client(ClientArguments *args)
    : _args(args),
      _status(new ClientStatus()),
      _reqTimer(new Timer()),
      _cycleTimer(new Timer()),
      _masterTimer(new Timer()),
      _http(new HTTPClient(_args->_hostname, _args->_port,
                           _args->_keepAlive, _args->_headerBenchmarkdataCoverage,
                           _args->_extraHeaders, _args->_authority)),
      _reader(new FileReader()),
      _output(),
      _linebufsize(args->_maxLineSize),
      _stop(false),
      _done(false),
      _thread()
{
    assert(args != NULL);
    _cycleTimer->SetMax(_args->_cycle);
}

Client::~Client()
{
}

void Client::runMe(Client * me) {
    me->run();
}

class UrlReader {
    FileReader &_reader;
    const ClientArguments &_args;
    int _restarts;
    int _linebufsize;
    int _contentbufsize;
    int _leftOversLen;
    char *_linebuf;
    char *_contentbuf;
    char *_leftOvers;
public:
    UrlReader(FileReader& reader, const ClientArguments &args)
        : _reader(reader), _args(args), _restarts(0),
          _linebufsize(args._maxLineSize),
          _contentbufsize(0), _leftOversLen(0),
          _linebuf(new char[_linebufsize]),
          _contentbuf(0), _leftOvers(0)
    {
        if (_args._usePostMode) {
            _contentbufsize = 16 * _linebufsize;
            _contentbuf = new char[_contentbufsize];
        }
    }
    int nextUrl();
    int getContent();
    char *url() const { return _linebuf; }
    char *contents() const { return _contentbuf; }
    ~UrlReader() {}
};

int UrlReader::nextUrl()
{
    char *buf = _linebuf;
    int buflen = _linebufsize;
    if (_leftOvers) {
        if (_leftOversLen < buflen) {
            strncpy(buf, _leftOvers, _leftOversLen);
            buf[_leftOversLen] = '\0';
        } else {
            strncpy(buf, _leftOvers, buflen);
            buf[buflen-1] = '\0';
        }
        _leftOvers = NULL;
        return _leftOversLen;
    }
    // Read maximum to _queryfileOffsetEnd
    if ( _args._singleQueryFile && _reader.GetFilePos() >= _args._queryfileEndOffset ) {
        _reader.SetFilePos(_args._queryfileOffset);
        if (_restarts == _args._restartLimit) {
            return 0;
        } else if (_args._restartLimit > 0) {
            _restarts++;
        }
    }
    int ll = _reader.ReadLine(buf, buflen);
    while (ll > 0 && _args._usePostMode && buf[0] != '/') {
        ll = _reader.ReadLine(buf, buflen);
    }
    if (ll > 0 && (buf[0] == '/' || !_args._usePostMode)) {
        return ll;
    }
    if (_restarts == _args._restartLimit) {
        return 0;
    } else if (_args._restartLimit > 0) {
        _restarts++;
    }
    if (ll < 0) {
        _reader.Reset();
        // Start reading from offset
        if (_args._singleQueryFile) {
            _reader.SetFilePos(_args._queryfileOffset);
        }
    }
    ll = _reader.ReadLine(buf, buflen);
    while (ll > 0 && _args._usePostMode && buf[0] != '/') {
        ll = _reader.ReadLine(buf, buflen);
    }
    if (ll > 0 && (buf[0] == '/' || !_args._usePostMode)) {
        return ll;
    }
    return 0;
}

int UrlReader::getContent()
{
    char *buf = _contentbuf;
    int bufLen = _contentbufsize;
    int totLen = 0;
    while (totLen < bufLen) {
       int len = _reader.ReadLine(buf, bufLen);
       if (len > 0) {
           if (buf[0] == '/') {
               _leftOvers = buf;
               _leftOversLen = len;
               return totLen;
           }
           buf += len;
           bufLen -= len;
           totLen += len;
       } else {
           return totLen;
       }
    }
    return totLen;
}

void
Client::run()
{
    char inputFilename[1024];
    char outputFilename[1024];
    char timestr[64];
    int  linelen;
    ///   int  reslen;

    std::this_thread::sleep_for(std::chrono::milliseconds(_args->_delay));

    // open query file
    snprintf(inputFilename, 1024, _args->_filenamePattern, _args->_myNum);
    if (!_reader->Open(inputFilename)) {
        printf("Client %d: ERROR: could not open file '%s' [read mode]\n",
               _args->_myNum, inputFilename);
        _status->SetError("Could not open query file.");
        return;
    }
    if (_args->_outputPattern != NULL) {
        snprintf(outputFilename, 1024, _args->_outputPattern, _args->_myNum);
        _output = std::make_unique<std::ofstream>(outputFilename, std::ofstream::out | std::ofstream::binary);
        if (_output->fail()) {
            printf("Client %d: ERROR: could not open file '%s' [write mode]\n",
                   _args->_myNum, outputFilename);
            _status->SetError("Could not open output file.");
            return;
        }
    }
    if (_output)
        _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);

    if (_args->_ignoreCount == 0)
        _masterTimer->Start();

    // Start reading from offset
    if ( _args->_singleQueryFile )
        _reader->SetFilePos(_args->_queryfileOffset);

    UrlReader urlSource(*_reader, *_args);
    size_t urlNumber = 0;

    // run queries
    while (!_stop) {

        _cycleTimer->Start();

        linelen = urlSource.nextUrl();
        if (linelen > 0) {
            ++urlNumber;
        } else {
            if (urlNumber == 0) {
                fprintf(stderr, "Client %d: ERROR: could not read any lines from '%s'\n",
                        _args->_myNum, inputFilename);
                _status->SetError("Could not read any lines from query file.");
            }
            break;
        }
        if (linelen < _linebufsize) {
            char *linebuf = urlSource.url();
            if (_output) {
                _output->write("URL: ", strlen("URL: "));
                _output->write(linebuf, linelen);
                _output->write("\n\n", 2);
            }
            if (linelen + (int)_args->_queryStringToAppend.length() < _linebufsize) {
                strcat(linebuf, _args->_queryStringToAppend.c_str());
            }
            int cLen = _args->_usePostMode ? urlSource.getContent() : 0;
            _reqTimer->Start();
            auto fetch_status = _http->Fetch(linebuf, _output.get(), _args->_usePostMode, urlSource.contents(), cLen);
            _reqTimer->Stop();
            _status->AddRequestStatus(fetch_status.RequestStatus());
            if (fetch_status.Ok() && fetch_status.TotalHitCount() == 0)
                ++_status->_zeroHitQueries;
            if (_output) {
                if (!fetch_status.Ok()) {
                    _output->write("\nFBENCH: URL FETCH FAILED!\n",
                                          strlen("\nFBENCH: URL FETCH FAILED!\n"));
                    _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);
                } else {
                    sprintf(timestr, "\nTIME USED: %0.4f s\n",
                            _reqTimer->GetTimespan() / 1000.0);
                    _output->write(timestr, strlen(timestr));
                    _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);
                }
            }
            if (fetch_status.ResultSize() >= _args->_byteLimit) {
                if (_args->_ignoreCount == 0)
                    _status->ResponseTime(_reqTimer->GetTimespan());
            } else {
                if (_args->_ignoreCount == 0)
                    _status->RequestFailed();
            }
        } else {
            if (_args->_ignoreCount == 0)
                _status->SkippedRequest();
        }
        _cycleTimer->Stop();
        if (_args->_cycle < 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds(int(_reqTimer->GetTimespan())));
        } else {
            if (_cycleTimer->GetRemaining() > 0) {
                std::this_thread::sleep_for(std::chrono::milliseconds(int(_cycleTimer->GetRemaining())));
            } else {
                if (_args->_ignoreCount == 0)
                    _status->OverTime();
            }
        }
        if (_args->_ignoreCount > 0) {
            _args->_ignoreCount--;
            if (_args->_ignoreCount == 0)
                _masterTimer->Start();
        }
        // Update current time span to calculate Q/s
        _status->SetRealTime(_masterTimer->GetCurrent());
    }
    _masterTimer->Stop();
    _status->SetRealTime(_masterTimer->GetTimespan());
    _status->SetReuseCount(_http->GetReuseCount());
    printf(".");
    fflush(stdout);
    _done = true;
}

void Client::stop() {
    _stop = true;
}

bool Client::done() {
    return _done;
}

void Client::start() {
    _thread = std::thread(Client::runMe, this);
}

void Client::join() {
    _thread.join();
}
<commit_msg>refactor loop for url reading<commit_after>// Copyright 2016 Yahoo Inc. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.
#include <util/timer.h>
#include <httpclient/httpclient.h>
#include <util/filereader.h>
#include "client.h"

Client::Client(ClientArguments *args)
    : _args(args),
      _status(new ClientStatus()),
      _reqTimer(new Timer()),
      _cycleTimer(new Timer()),
      _masterTimer(new Timer()),
      _http(new HTTPClient(_args->_hostname, _args->_port,
                           _args->_keepAlive, _args->_headerBenchmarkdataCoverage,
                           _args->_extraHeaders, _args->_authority)),
      _reader(new FileReader()),
      _output(),
      _linebufsize(args->_maxLineSize),
      _stop(false),
      _done(false),
      _thread()
{
    assert(args != NULL);
    _cycleTimer->SetMax(_args->_cycle);
}

Client::~Client()
{
}

void Client::runMe(Client * me) {
    me->run();
}

class UrlReader {
    FileReader &_reader;
    const ClientArguments &_args;
    int _restarts;
    int _linebufsize;
    int _contentbufsize;
    int _leftOversLen;
    char *_linebuf;
    char *_contentbuf;
    char *_leftOvers;
public:
    UrlReader(FileReader& reader, const ClientArguments &args)
        : _reader(reader), _args(args), _restarts(0),
          _linebufsize(args._maxLineSize),
          _contentbufsize(0), _leftOversLen(0),
          _linebuf(new char[_linebufsize]),
          _contentbuf(0), _leftOvers(0)
    {
        if (_args._usePostMode) {
            _contentbufsize = 16 * _linebufsize;
            _contentbuf = new char[_contentbufsize];
        }
    }
    bool reset();
    int nextUrl();
    int getContent();
    char *url() const { return _linebuf; }
    char *contents() const { return _contentbuf; }
    ~UrlReader() {}
};

bool UrlReader::reset()
{
    _reader.Reset();
    // Start reading from offset
    if (_args._singleQueryFile) {
        _reader.SetFilePos(_args._queryfileOffset);
    }
    if (_restarts == _args._restartLimit) {
        return false;
    } else if (_args._restartLimit > 0) {
        _restarts++;
    }
    return true;
}

int UrlReader::nextUrl()
{
    char *buf = _linebuf;
    int buflen = _linebufsize;
    if (_leftOvers) {
        if (_leftOversLen < buflen) {
            strncpy(buf, _leftOvers, _leftOversLen);
            buf[_leftOversLen] = '\0';
        } else {
            strncpy(buf, _leftOvers, buflen);
            buf[buflen-1] = '\0';
        }
        _leftOvers = NULL;
        return _leftOversLen;
    }
    bool again = true;
    for (int retry = 0; again && retry < 100; ++retry) {
        // Read maximum to _queryfileEndOffset
        if ( _args._singleQueryFile && _reader.GetFilePos() >= _args._queryfileEndOffset ) {
            again = reset();
        }
        int ll = _reader.ReadLine(buf, buflen);
        if (ll > 0) {
            if (buf[0] == '/' || !_args._usePostMode) {
                return ll;
            }
        }
        if (ll < 0) {
            // reached EOF
            again = reset();
        }
    }
    return 0;
}

int UrlReader::getContent()
{
    char *buf = _contentbuf;
    int bufLen = _contentbufsize;
    int totLen = 0;
    while (totLen < bufLen) {
       int len = _reader.ReadLine(buf, bufLen);
       if (len > 0) {
           if (buf[0] == '/') {
               _leftOvers = buf;
               _leftOversLen = len;
               return totLen;
           }
           buf += len;
           bufLen -= len;
           totLen += len;
       } else {
           return totLen;
       }
    }
    return totLen;
}

void
Client::run()
{
    char inputFilename[1024];
    char outputFilename[1024];
    char timestr[64];
    int  linelen;
    ///   int  reslen;

    std::this_thread::sleep_for(std::chrono::milliseconds(_args->_delay));

    // open query file
    snprintf(inputFilename, 1024, _args->_filenamePattern, _args->_myNum);
    if (!_reader->Open(inputFilename)) {
        printf("Client %d: ERROR: could not open file '%s' [read mode]\n",
               _args->_myNum, inputFilename);
        _status->SetError("Could not open query file.");
        return;
    }
    if (_args->_outputPattern != NULL) {
        snprintf(outputFilename, 1024, _args->_outputPattern, _args->_myNum);
        _output = std::make_unique<std::ofstream>(outputFilename, std::ofstream::out | std::ofstream::binary);
        if (_output->fail()) {
            printf("Client %d: ERROR: could not open file '%s' [write mode]\n",
                   _args->_myNum, outputFilename);
            _status->SetError("Could not open output file.");
            return;
        }
    }
    if (_output)
        _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);

    if (_args->_ignoreCount == 0)
        _masterTimer->Start();

    // Start reading from offset
    if ( _args->_singleQueryFile )
        _reader->SetFilePos(_args->_queryfileOffset);

    UrlReader urlSource(*_reader, *_args);
    size_t urlNumber = 0;

    // run queries
    while (!_stop) {

        _cycleTimer->Start();

        linelen = urlSource.nextUrl();
        if (linelen > 0) {
            ++urlNumber;
        } else {
            if (urlNumber == 0) {
                fprintf(stderr, "Client %d: ERROR: could not read any lines from '%s'\n",
                        _args->_myNum, inputFilename);
                _status->SetError("Could not read any lines from query file.");
            }
            break;
        }
        if (linelen < _linebufsize) {
            char *linebuf = urlSource.url();
            if (_output) {
                _output->write("URL: ", strlen("URL: "));
                _output->write(linebuf, linelen);
                _output->write("\n\n", 2);
            }
            if (linelen + (int)_args->_queryStringToAppend.length() < _linebufsize) {
                strcat(linebuf, _args->_queryStringToAppend.c_str());
            }
            int cLen = _args->_usePostMode ? urlSource.getContent() : 0;
            _reqTimer->Start();
            auto fetch_status = _http->Fetch(linebuf, _output.get(), _args->_usePostMode, urlSource.contents(), cLen);
            _reqTimer->Stop();
            _status->AddRequestStatus(fetch_status.RequestStatus());
            if (fetch_status.Ok() && fetch_status.TotalHitCount() == 0)
                ++_status->_zeroHitQueries;
            if (_output) {
                if (!fetch_status.Ok()) {
                    _output->write("\nFBENCH: URL FETCH FAILED!\n",
                                          strlen("\nFBENCH: URL FETCH FAILED!\n"));
                    _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);
                } else {
                    sprintf(timestr, "\nTIME USED: %0.4f s\n",
                            _reqTimer->GetTimespan() / 1000.0);
                    _output->write(timestr, strlen(timestr));
                    _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);
                }
            }
            if (fetch_status.ResultSize() >= _args->_byteLimit) {
                if (_args->_ignoreCount == 0)
                    _status->ResponseTime(_reqTimer->GetTimespan());
            } else {
                if (_args->_ignoreCount == 0)
                    _status->RequestFailed();
            }
        } else {
            if (_args->_ignoreCount == 0)
                _status->SkippedRequest();
        }
        _cycleTimer->Stop();
        if (_args->_cycle < 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds(int(_reqTimer->GetTimespan())));
        } else {
            if (_cycleTimer->GetRemaining() > 0) {
                std::this_thread::sleep_for(std::chrono::milliseconds(int(_cycleTimer->GetRemaining())));
            } else {
                if (_args->_ignoreCount == 0)
                    _status->OverTime();
            }
        }
        if (_args->_ignoreCount > 0) {
            _args->_ignoreCount--;
            if (_args->_ignoreCount == 0)
                _masterTimer->Start();
        }
        // Update current time span to calculate Q/s
        _status->SetRealTime(_masterTimer->GetCurrent());
    }
    _masterTimer->Stop();
    _status->SetRealTime(_masterTimer->GetTimespan());
    _status->SetReuseCount(_http->GetReuseCount());
    printf(".");
    fflush(stdout);
    _done = true;
}

void Client::stop() {
    _stop = true;
}

bool Client::done() {
    return _done;
}

void Client::start() {
    _thread = std::thread(Client::runMe, this);
}

void Client::join() {
    _thread.join();
}
<|endoftext|>


<commit_before>#include <Servo.h>
#include "Msg.h"
#include "Device.h"
#include "MrlServo.h"

MrlServo::MrlServo(int deviceId) : Device(deviceId, DEVICE_TYPE_SERVO) {
  isMoving = false;
  isSweeping = false;
  // create the servo
  servo = new Servo();
  lastUpdate = 0;
  currentPosUs = 0.0;
  targetPosUs = 0;
  velocity = -1;
  acceleration = -1;
  moveStart = 0;
}

MrlServo::~MrlServo() {
  if (servo){
    servo->detach();
    delete servo;
  }
}

// this method "may" be called with a pin or pin & pos depending on
// config size
bool MrlServo::attach(byte pin, int initPosUs, int initVelocity){
  // msg->publishDebug("MrlServo.deviceAttach !");
  servo->writeMicroseconds(initPosUs);
  currentPosUs = initPosUs;
  targetPosUs = initPosUs;
  velocity = initVelocity;
  this->pin = pin;
  servo->attach(pin);
  //publishServoEvent(SERVO_EVENT_STOPPED);
  return true;
}

// This method is equivalent to Arduino's Servo.attach(pin) - (no pos)
void MrlServo::attachPin(int pin){
  attach(pin, currentPosUs, velocity);
}

void MrlServo::detachPin(){
  servo->detach();
}

// FIXME - what happened to events ?
void MrlServo::update() {
  //it may have an imprecision of +- 1 due to the conversion of currentPosUs to int
  if (isMoving) {
    if ((int)currentPosUs != targetPosUs) {
      long deltaTime = millis() - lastUpdate;
      lastUpdate = millis();
      float _velocity = velocity;
      if (acceleration != -1) {
        _velocity *= acceleration * pow(((float)(millis()- moveStart)) / 1000,2) / 10;
        //msg->publishDebug(String(_velocity));
        if (_velocity > velocity) {
          _velocity = velocity;
        }
      }
      if(targetPosUs > 500) {
        _velocity = map(_velocity, 0, 180, 544, 2400) - 544;
      }
      float step = _velocity * deltaTime;
      step /= 1000; //for deg/ms;
      if (isSweeping) {
        step = sweepStep;
      }
      if (velocity < 0) { // when velocity < 0, it mean full speed ahead
        step = targetPosUs - (int)currentPosUs;
      }
      else if (currentPosUs > targetPosUs) {
        step *=-1;
      }
      int previousCurrentPosUs = (int)currentPosUs;
      currentPosUs += step;
      if ((step > 0.0 && (int)currentPosUs > targetPosUs) || (step < 0.0 && (int)currentPosUs < targetPosUs)) {
        currentPosUs = targetPosUs;
      }
      if (!(previousCurrentPosUs == (int)currentPosUs)) {
        servo->writeMicroseconds((int)currentPosUs);
        if ((int)currentPosUs == targetPosUs) {
          publishServoEvent(SERVO_EVENT_STOPPED);
        }
        else {
          publishServoEvent(SERVO_EVENT_POSITION_UPDATE);
        }
      }
    }
    else {
      if (isSweeping) {
        if (targetPosUs == minUs) {
          targetPosUs = maxUs;
        }
        else {
          targetPosUs = minUs;
        }
        sweepStep *= -1;
      }
      else {
        isMoving = false;
        publishServoEvent(SERVO_EVENT_STOPPED);
      }
    }
  }
}

void MrlServo::moveToMicroseconds(int posUs) {
  if (servo == NULL){
    return;
  }
  targetPosUs = posUs;
  isMoving = true;
  lastUpdate = millis();
  moveStart = lastUpdate;
  publishServoEvent(SERVO_EVENT_POSITION_UPDATE);
}

void MrlServo::startSweep(int minUs, int maxUs, int step) {
  this->minUs = minUs;
  this->maxUs = maxUs;
  sweepStep = step;
  targetPosUs = maxUs;
  isMoving = true;
  isSweeping = true;
}

void MrlServo::stopSweep() {
  isMoving = false;
  isSweeping = false;
}

void MrlServo::setVelocity(int velocity) {
  this->velocity = velocity;
}

void MrlServo::setAcceleration(int acceleration) {
  this->acceleration = acceleration;
}

void MrlServo::publishServoEvent(int type) {
  msg->publishServoEvent(id, type, (int)currentPosUs, targetPosUs);
}

<commit_msg>forgot one<commit_after>#include <Servo.h>
#include "Msg.h"
#include "Device.h"
#include "MrlServo.h"

MrlServo::MrlServo(int deviceId) : Device(deviceId, DEVICE_TYPE_SERVO) {
  isMoving = false;
  isSweeping = false;
  // create the servo
  servo = new Servo();
  lastUpdate = 0;
  currentPosUs = 0.0;
  targetPosUs = 0;
  velocity = -1;
  acceleration = -1;
  moveStart = 0;
}

MrlServo::~MrlServo() {
  if (servo){
    servo->detach();
    delete servo;
  }
}

// this method "may" be called with a pin or pin & pos depending on
// config size
bool MrlServo::attach(byte pin, int initPosUs, int initVelocity){
  // msg->publishDebug("MrlServo.deviceAttach !");
  servo->writeMicroseconds(initPosUs);
  currentPosUs = initPosUs;
  targetPosUs = initPosUs;
  velocity = initVelocity;
  this->pin = pin;
  servo->attach(pin);
  //publishServoEvent(SERVO_EVENT_STOPPED);
  return true;
}

// This method is equivalent to Arduino's Servo.attach(pin) - (no pos)
void MrlServo::attachPin(int pin){
  attach(pin, currentPosUs, velocity);
}

void MrlServo::detachPin(){
  servo->detach();
}

// FIXME - what happened to events ?
void MrlServo::update() {
  //it may have an imprecision of +- 1 due to the conversion of currentPosUs to int
  if (isMoving) {
    if ((int)currentPosUs != targetPosUs) {
      long deltaTime = millis() - lastUpdate;
      lastUpdate = millis();
      float _velocity = velocity;
      if (acceleration != -1) {
        _velocity *= acceleration * pow(((float)(millis()- moveStart)) / 1000,2) / 10;
        //msg->publishDebug(String(_velocity));
        if (_velocity > velocity) {
          _velocity = velocity;
        }
      }
      if(targetPosUs > 500) {
        _velocity = map(_velocity, 0, 180, 544, 2400) - 544;
      }
      float step = _velocity * deltaTime;
      step /= 1000; //for deg/ms;
      if (isSweeping) {
        step = sweepStep;
      }
      if (velocity < 0) { // when velocity < 0, it mean full speed ahead
        step = targetPosUs - (int)currentPosUs;
      }
      else if (currentPosUs > targetPosUs) {
        step *=-1;
      }
      int previousCurrentPosUs = (int)currentPosUs;
      currentPosUs += step;
      if ((step > 0.0 && (int)currentPosUs > targetPosUs) || (step < 0.0 && (int)currentPosUs < targetPosUs)) {
        currentPosUs = targetPosUs;
      }
      if (!(previousCurrentPosUs == (int)currentPosUs)) {
        servo->writeMicroseconds((int)currentPosUs);
        if ((int)currentPosUs == targetPosUs) {
          publishServoEvent(SERVO_EVENT_STOPPED);
        }
        else {
          publishServoEvent(SERVO_EVENT_POSITION_UPDATE);
        }
      }
    }
    else {
      if (isSweeping) {
        if (targetPosUs == minUs) {
          targetPosUs = maxUs;
        }
        else {
          targetPosUs = minUs;
        }
        sweepStep *= -1;
      }
      else {
        isMoving = false;
        publishServoEvent(SERVO_EVENT_STOPPED);
      }
    }
  }
}

void MrlServo::moveToMicroseconds(int posUs) {
  if (servo == NULL){
    return;
  }
  targetPosUs = posUs;
  isMoving = true;
  lastUpdate = millis();
  moveStart = lastUpdate;
  publishServoEvent(SERVO_EVENT_POSITION_UPDATE);
}

void MrlServo::startSweep(int minUs, int maxUs, int step) {
  this->minUs = minUs;
  this->maxUs = maxUs;
  sweepStep = step;
  targetPosUs = maxUs;
  isMoving = true;
  isSweeping = true;
}

void MrlServo::stop() {
  isMoving = false;
  isSweeping = false;
}

void MrlServo::stopSweep() {
  isMoving = false;
  isSweeping = false;
}

void MrlServo::setVelocity(int velocity) {
  this->velocity = velocity;
}

void MrlServo::setAcceleration(int acceleration) {
  this->acceleration = acceleration;
}

void MrlServo::publishServoEvent(int type) {
  msg->publishServoEvent(id, type, (int)currentPosUs, targetPosUs);
}

<|endoftext|>


<commit_before>/*
 Copyright (c) 2020 ANON authors, see AUTHORS file.

 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 "sync_teflon_app.h"
#include "log.h"
#include "sproc_mgr.h"
#include <sys/stat.h>
#include <aws/core/Aws.h>
#include <aws/core/utils/Outcome.h>
#include <aws/dynamodb/DynamoDBClient.h>
#include <aws/dynamodb/model/GetItemRequest.h>

namespace
{

struct tef_app
{
  tef_app(const Aws::String& id,
    const Aws::Vector<Aws::String>& filesv,
    Aws::Map<Aws::String, const std::shared_ptr<Aws::DynamoDB::Model::AttributeValue>>& fids)
    : id(id)
  {
    for (auto &f : filesv) {
      files[fids[f]->GetS()] = f;
    }
  }

  Aws::String id;
  std::map<Aws::String, Aws::String> files;
};

bool exe_cmd(const std::string &str)
{
  auto f = popen(str.c_str(), "r");
  if (f)
  {
    auto exit_code = pclose(f);
    if (exit_code != 0)
    {
      anon_log_error("command: " << str << " exited non-zero: " << error_string(exit_code));
      return false;
    }
  }
  else
  {
    anon_log_error("popen failed: " << errno_string());
    return false;
  }
  return true;
}

bool create_empty_directory(const ec2_info &ec2i, const Aws::String &id)
{
  auto dir = ec2i.root_dir;
  if (id.size() != 0)
  {
    dir += "/";
    dir += id.c_str();
  }
  std::ostringstream str;
  str << "rm -rf " << id << " && mkdir " << id;
  return exe_cmd(str.str());
}

void remove_directory(const ec2_info &ec2i, const Aws::String &id)
{
  auto dir = ec2i.root_dir;
  if (id.size() != 0)
  {
    dir += "/";
    dir += id.c_str();
  }
  std::ostringstream str;
  str << "rm -rf " << id;
  if (!exe_cmd(str.str()))
    anon_log_error("failed to remove " << id << " directory");
}

std::shared_ptr<tef_app> curr_app;

} // namespace

teflon_state sync_teflon_app(const ec2_info &ec2i)
{
  if (ec2i.user_data_js.find("current_server_primary_key_value") == ec2i.user_data_js.end() || ec2i.user_data_js.find("current_server_primary_key_name") == ec2i.user_data_js.end())
  {
    anon_log_error("no current server info specified in user data - cannot start teflon app");
    return teflon_server_failed;
  }

  if (!curr_app)
  {
    if (!create_empty_directory(ec2i, ""))
      return teflon_server_failed;
  }

  Aws::Client::ClientConfiguration ddb_config;
  if (ec2i.user_data_js.find("current_server_region") != ec2i.user_data_js.end())
    ddb_config.region = ec2i.user_data_js["current_server_region"];
  else
    ddb_config.region = ec2i.default_region;
  Aws::DynamoDB::DynamoDBClient ddbc(ddb_config);

  Aws::DynamoDB::Model::AttributeValue primary_key;
  std::string cs_primary_key = ec2i.user_data_js["current_server_primary_key_value"];
  primary_key.SetS(cs_primary_key.c_str());
  Aws::DynamoDB::Model::GetItemRequest req;
  std::string cs_table_name = ec2i.user_data_js["current_server_table_name"];
  std::string cs_key_name = ec2i.user_data_js["current_server_primary_key_name"];
  req.WithTableName(cs_table_name.c_str())
      .AddKey(cs_key_name.c_str(), primary_key);
  auto outcome = ddbc.GetItem(req);
  if (!outcome.IsSuccess())
  {
    anon_log_error("GetItem failed: " << outcome.GetError());
    return teflon_server_failed;
  }

  auto &map = outcome.GetResult().GetItem();
  auto state_it = map.find("state");
  if (state_it == map.end())
  {
    anon_log_error("current_server record missing required \"state\" field");
    return teflon_server_failed;
  }
  auto state = state_it->second.GetS();
  if (state == "stop")
  {
    anon_log("stopping server");
    return teflon_shut_down;
  }

  if (state != "run")
  {
    anon_log_error("unknown teflon server state: " << state);
    return teflon_server_failed;
  }

  auto id_it = map.find("current_server_id");

  auto end = map.end();
  if (id_it == end)
  {
    anon_log_error("current_server record missing required \"current_server_id\" field");
    return teflon_server_failed;
  }

  auto current_server_id = id_it->second.GetS();
  if (curr_app && current_server_id == curr_app->id)
  {
    anon_log("current server definition matches running server, no-op");
    return teflon_server_running;
  }

  auto files_it = map.find("files");
  auto exe_it = map.find("entry");
  auto ids_it = map.find("fids");
  if (files_it == end || exe_it == end || ids_it == end)
  {
    anon_log_error("current_server record missing required \"files\", \"entry\", and/or \"fids\" field(s)");
    return teflon_server_failed;
  }

  auto files_needed = files_it->second.GetSS();
  auto file_to_execute = exe_it->second.GetS();
  auto ids = ids_it->second.GetM();

  if (ec2i.user_data_js.find("current_server_artifacts_bucket") == ec2i.user_data_js.end() || ec2i.user_data_js.find("current_server_artifacts_key") == ec2i.user_data_js.end())
  {
    anon_log_error("user data missing required fields \"current_server_artifacts_bucket\" and/or \"current_server_artifacts_key\"");
    return teflon_server_failed;
  }
  std::string bucket = ec2i.user_data_js["current_server_artifacts_bucket"];
  std::string key = ec2i.user_data_js["current_server_artifacts_key"];

  std::ostringstream files_cmd;
  files_cmd << "FAIL=0\n";
  for (const auto &f : files_needed)
  {
    if (ids.find(f) == ids.end())
    {
      anon_log_error("file: \"" << f << "\" missing in fids");
      return teflon_server_failed;
    }
    if (curr_app && curr_app->files.find(f) != curr_app->files.end())
    {
      // f matches a file we have already downloaded, so just hard link it
      // to the new directory
      auto existing_file = curr_app->files[f];
      files_cmd << "ln " << ec2i.root_dir << "/" << curr_app->id << "/" << curr_app->files[f]
               << " " << ec2i.root_dir << "/" << current_server_id << "/" << f << " &\n";
    }
    else
    {
      // does not match an existing file, so download it from s3
      files_cmd << "aws s3 --region " << ec2i.default_region << " cp s3://" << bucket << "/" << key << "/" << f
               << " " << ec2i.root_dir << "/" << current_server_id << "/" << f << " --quiet &\n";
    }
  }
  if (!create_empty_directory(ec2i, current_server_id))
    return teflon_server_failed;
  files_cmd << "for job in `jobs -p`\n"
            << "do\n"
            << " wait $job || let \"FAIL+=1\"\n"
            << "done\n"
            << "f [ \"$FAIL\" == \"0\" ];\n"
            << "then\n"
            << " exit 0\n"
            << "else\n"
            << " exit 1";
  if (!exe_cmd(files_cmd.str()))
    return teflon_server_failed;

  std::ostringstream ef;
  ef << ec2i.root_dir << "/" << current_server_id << "/" << file_to_execute;
  chmod(ef.str().c_str(), ACCESSPERMS);

  try {
    auto new_app = std::make_shared<tef_app>(current_server_id, files_needed, ids);
    start_server(file_to_execute.c_str(), false/*do_tls*/, std::vector<std::string>());
    if (curr_app)
      remove_directory(ec2i, curr_app->id);
    curr_app = new_app;
  }
  catch(const std::exception& exc)
  {
    anon_log_error("start_server failed: " << exc.what());
    remove_directory(ec2i, current_server_id);
    return teflon_server_failed;
  }
  catch(...)
  {
    anon_log_error("start_server");
    remove_directory(ec2i, current_server_id);
    return teflon_server_failed;
  }

  return teflon_server_running;
}<commit_msg>debugging<commit_after>/*
 Copyright (c) 2020 ANON authors, see AUTHORS file.

 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 "sync_teflon_app.h"
#include "log.h"
#include "sproc_mgr.h"
#include <sys/stat.h>
#include <aws/core/Aws.h>
#include <aws/core/utils/Outcome.h>
#include <aws/dynamodb/DynamoDBClient.h>
#include <aws/dynamodb/model/GetItemRequest.h>

namespace
{

struct tef_app
{
  tef_app(const Aws::String& id,
    const Aws::Vector<Aws::String>& filesv,
    Aws::Map<Aws::String, const std::shared_ptr<Aws::DynamoDB::Model::AttributeValue>>& fids)
    : id(id)
  {
    for (auto &f : filesv) {
      files[fids[f]->GetS()] = f;
    }
  }

  Aws::String id;
  std::map<Aws::String, Aws::String> files;
};

bool exe_cmd(const std::string &str)
{
  auto f = popen(str.c_str(), "r");
  if (f)
  {
    auto exit_code = pclose(f);
    if (exit_code != 0)
    {
      anon_log_error("command: " << str << " exited non-zero: " << error_string(exit_code));
      return false;
    }
  }
  else
  {
    anon_log_error("popen failed: " << errno_string());
    return false;
  }
  return true;
}

bool create_empty_directory(const ec2_info &ec2i, const Aws::String &id)
{
  auto dir = ec2i.root_dir;
  if (id.size() != 0)
  {
    dir += "/";
    dir += id.c_str();
  }
  std::ostringstream str;
  str << "rm -rf " << dir << " && mkdir " << dir;
  return exe_cmd(str.str());
}

void remove_directory(const ec2_info &ec2i, const Aws::String &id)
{
  auto dir = ec2i.root_dir;
  if (id.size() != 0)
  {
    dir += "/";
    dir += id.c_str();
  }
  std::ostringstream str;
  str << "rm -rf " << id;
  if (!exe_cmd(str.str()))
    anon_log_error("failed to remove " << id << " directory");
}

std::shared_ptr<tef_app> curr_app;

} // namespace

teflon_state sync_teflon_app(const ec2_info &ec2i)
{
  if (ec2i.user_data_js.find("current_server_primary_key_value") == ec2i.user_data_js.end() || ec2i.user_data_js.find("current_server_primary_key_name") == ec2i.user_data_js.end())
  {
    anon_log_error("no current server info specified in user data - cannot start teflon app");
    return teflon_server_failed;
  }

  if (!curr_app)
  {
    if (!create_empty_directory(ec2i, ""))
      return teflon_server_failed;
  }

  Aws::Client::ClientConfiguration ddb_config;
  if (ec2i.user_data_js.find("current_server_region") != ec2i.user_data_js.end())
    ddb_config.region = ec2i.user_data_js["current_server_region"];
  else
    ddb_config.region = ec2i.default_region;
  Aws::DynamoDB::DynamoDBClient ddbc(ddb_config);

  Aws::DynamoDB::Model::AttributeValue primary_key;
  std::string cs_primary_key = ec2i.user_data_js["current_server_primary_key_value"];
  primary_key.SetS(cs_primary_key.c_str());
  Aws::DynamoDB::Model::GetItemRequest req;
  std::string cs_table_name = ec2i.user_data_js["current_server_table_name"];
  std::string cs_key_name = ec2i.user_data_js["current_server_primary_key_name"];
  req.WithTableName(cs_table_name.c_str())
      .AddKey(cs_key_name.c_str(), primary_key);
  auto outcome = ddbc.GetItem(req);
  if (!outcome.IsSuccess())
  {
    anon_log_error("GetItem failed: " << outcome.GetError());
    return teflon_server_failed;
  }

  auto &map = outcome.GetResult().GetItem();
  auto state_it = map.find("state");
  if (state_it == map.end())
  {
    anon_log_error("current_server record missing required \"state\" field");
    return teflon_server_failed;
  }
  auto state = state_it->second.GetS();
  if (state == "stop")
  {
    anon_log("stopping server");
    return teflon_shut_down;
  }

  if (state != "run")
  {
    anon_log_error("unknown teflon server state: " << state);
    return teflon_server_failed;
  }

  auto id_it = map.find("current_server_id");

  auto end = map.end();
  if (id_it == end)
  {
    anon_log_error("current_server record missing required \"current_server_id\" field");
    return teflon_server_failed;
  }

  auto current_server_id = id_it->second.GetS();
  if (curr_app && current_server_id == curr_app->id)
  {
    anon_log("current server definition matches running server, no-op");
    return teflon_server_running;
  }

  auto files_it = map.find("files");
  auto exe_it = map.find("entry");
  auto ids_it = map.find("fids");
  if (files_it == end || exe_it == end || ids_it == end)
  {
    anon_log_error("current_server record missing required \"files\", \"entry\", and/or \"fids\" field(s)");
    return teflon_server_failed;
  }

  auto files_needed = files_it->second.GetSS();
  auto file_to_execute = exe_it->second.GetS();
  auto ids = ids_it->second.GetM();

  if (ec2i.user_data_js.find("current_server_artifacts_bucket") == ec2i.user_data_js.end() || ec2i.user_data_js.find("current_server_artifacts_key") == ec2i.user_data_js.end())
  {
    anon_log_error("user data missing required fields \"current_server_artifacts_bucket\" and/or \"current_server_artifacts_key\"");
    return teflon_server_failed;
  }
  std::string bucket = ec2i.user_data_js["current_server_artifacts_bucket"];
  std::string key = ec2i.user_data_js["current_server_artifacts_key"];

  std::ostringstream files_cmd;
  files_cmd << "FAIL=0\n";
  for (const auto &f : files_needed)
  {
    if (ids.find(f) == ids.end())
    {
      anon_log_error("file: \"" << f << "\" missing in fids");
      return teflon_server_failed;
    }
    if (curr_app && curr_app->files.find(f) != curr_app->files.end())
    {
      // f matches a file we have already downloaded, so just hard link it
      // to the new directory
      auto existing_file = curr_app->files[f];
      files_cmd << "ln " << ec2i.root_dir << "/" << curr_app->id << "/" << curr_app->files[f]
               << " " << ec2i.root_dir << "/" << current_server_id << "/" << f << " &\n";
    }
    else
    {
      // does not match an existing file, so download it from s3
      files_cmd << "aws s3 --region " << ec2i.default_region << " cp s3://" << bucket << "/" << key << "/" << f
               << " " << ec2i.root_dir << "/" << current_server_id << "/" << f << " --quiet &\n";
    }
  }
  if (!create_empty_directory(ec2i, current_server_id))
    return teflon_server_failed;
  files_cmd << "for job in `jobs -p`\n"
            << "do\n"
            << " wait $job || let \"FAIL+=1\"\n"
            << "done\n"
            << "if [ \"$FAIL\" == \"0\" ];\n"
            << "then\n"
            << " exit 0\n"
            << "else\n"
            << " exit 1";
  anon_log("executing script:\n" << files_cmd.str());
  if (!exe_cmd(files_cmd.str()))
    return teflon_server_failed;

  std::ostringstream ef;
  ef << ec2i.root_dir << "/" << current_server_id << "/" << file_to_execute;
  chmod(ef.str().c_str(), ACCESSPERMS);

  try {
    auto new_app = std::make_shared<tef_app>(current_server_id, files_needed, ids);
    start_server(file_to_execute.c_str(), false/*do_tls*/, std::vector<std::string>());
    if (curr_app)
      remove_directory(ec2i, curr_app->id);
    curr_app = new_app;
  }
  catch(const std::exception& exc)
  {
    anon_log_error("start_server failed: " << exc.what());
    remove_directory(ec2i, current_server_id);
    return teflon_server_failed;
  }
  catch(...)
  {
    anon_log_error("start_server");
    remove_directory(ec2i, current_server_id);
    return teflon_server_failed;
  }

  return teflon_server_running;
}
<|endoftext|>


<commit_before>#include <ros/ros.h>

//ROS libraries
#include <tf/transform_datatypes.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>

//Package include
#include <usbSerial.h>

using namespace std;

//aBridge functions
void cmdHandler(const geometry_msgs::Twist::ConstPtr& message);
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void serialActivityTimer(const ros::TimerEvent& e);
void publishRosTopics();
void parseData(string data);

//Globals
geometry_msgs::QuaternionStamped fingerAngle;
geometry_msgs::QuaternionStamped wristAngle;
sensor_msgs::Imu imu;
nav_msgs::Odometry odom;
sensor_msgs::Range sonarLeft;
sensor_msgs::Range sonarCenter;
sensor_msgs::Range sonarRight;
USBSerial usb;
const int baud = 115200;
char dataCmd[] = "d\n";
char moveCmd[16];
char host[128];
float linearSpeed = 0.;
float turnSpeed = 0.;
const float deltaTime = 0.1;

//Publishers
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher imuPublish;
ros::Publisher odomPublish;
ros::Publisher sonarLeftPublish;
ros::Publisher sonarCenterPublish;
ros::Publisher sonarRightPublish;

//Subscribers
ros::Subscriber velocitySubscriber;
ros::Subscriber fingerAngleSubscriber;
ros::Subscriber wristAngleSubscriber;

//Timers
ros::Timer publishTimer;

int main(int argc, char **argv) {

    gethostname(host, sizeof (host));
    string hostname(host);
    string publishedName;
    ros::init(argc, argv, (hostname + "_ABRIDGE"));

    ros::NodeHandle param("~");
    string devicePath;
    param.param("device", devicePath, string("/dev/ttyUSB0"));
    usb.openUSBPort(devicePath, baud);

    sleep(5);

    ros::NodeHandle aNH;

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "!  ABridge module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No Name Selected. Default is: " << publishedName << endl;
    }

    fingerAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/fingerAngle/prev_cmd"), 10);
    wristAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/wristAngle/prev_cmd"), 10);
    imuPublish = aNH.advertise<sensor_msgs::Imu>((publishedName + "/imu"), 10);
    odomPublish = aNH.advertise<nav_msgs::Odometry>((publishedName + "/odom"), 10);
    sonarLeftPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarLeft"), 10);
    sonarCenterPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarCenter"), 10);
    sonarRightPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarRight"), 10);

    velocitySubscriber = aNH.subscribe((publishedName + "/velocity"), 10, cmdHandler);
    fingerAngleSubscriber = aNH.subscribe((publishedName + "/fingerAngle/cmd"), 1, fingerAngleHandler);
    wristAngleSubscriber = aNH.subscribe((publishedName + "/wristAngle/cmd"), 1, wristAngleHandler);

    publishTimer = aNH.createTimer(ros::Duration(deltaTime), serialActivityTimer);

    imu.header.frame_id = publishedName+"/base_link";

    odom.header.frame_id = publishedName+"/odom";
    odom.child_frame_id = publishedName+"/base_link";

    ros::spin();

    return EXIT_SUCCESS;
}

void cmdHandler(const geometry_msgs::Twist::ConstPtr& message) {
    // remove artificial factor that was multiplied for simulation. this scales it back down to -1.0 to +1.0
  linearSpeed = (message->linear.x); // / 1.5;
  turnSpeed = (message->angular.z); // / 8;

    if (linearSpeed != 0.) {
        sprintf(moveCmd, "m,%d\n", (int) (linearSpeed * 255));
        usb.sendData(moveCmd);
    } else if (turnSpeed != 0.) {
        sprintf(moveCmd, "t,%d\n", (int) (turnSpeed * 255));
        usb.sendData(moveCmd);
    } else {
        sprintf(moveCmd, "s\n");
        usb.sendData(moveCmd);
    }

    memset(&moveCmd, '\0', sizeof (moveCmd));
}

// The finger and wrist handlers receive gripper angle commands in floating point
// radians, write them to a string and send that to the arduino
// for processing.
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
  char cmd[16]={'\0'};

  // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'f' indicates this is a finger command to the arduino
    sprintf(cmd, "f,0\n");
  } else {
    sprintf(cmd, "f,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
    char cmd[16]={'\0'};

    // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'w' indicates this is a wrist command to the arduino
    sprintf(cmd, "w,0\n");
  } else {
    sprintf(cmd, "w,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void serialActivityTimer(const ros::TimerEvent& e) {
    usb.sendData(dataCmd);
    parseData(usb.readData());
    publishRosTopics();
}

void publishRosTopics() {
    fingerAnglePublish.publish(fingerAngle);
    wristAnglePublish.publish(wristAngle);
    imuPublish.publish(imu);
    odomPublish.publish(odom);
    sonarLeftPublish.publish(sonarLeft);
    sonarCenterPublish.publish(sonarCenter);
    sonarRightPublish.publish(sonarRight);
}

void parseData(string str) {
    istringstream oss(str);
    string sentence;

    while (getline(oss, sentence, '\n')) {
		istringstream wss(sentence);
		string word;

		vector<string> dataSet;
		while (getline(wss, word, ',')) {
			dataSet.push_back(word);
		}

		if (dataSet.size() >= 3 && dataSet.at(1) == "1") {

			if (dataSet.at(0) == "GRF") {
				fingerAngle.header.stamp = ros::Time::now();
				fingerAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "GRW") {
				wristAngle.header.stamp = ros::Time::now();
				wristAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "IMU") {
				imu.header.stamp = ros::Time::now();
				imu.linear_acceleration.x = atof(dataSet.at(2).c_str());
				imu.linear_acceleration.y = atof(dataSet.at(3).c_str());
				imu.linear_acceleration.z = atof(dataSet.at(4).c_str());
				imu.angular_velocity.x = atof(dataSet.at(5).c_str());
				imu.angular_velocity.y = atof(dataSet.at(6).c_str());
				imu.angular_velocity.z = atof(dataSet.at(7).c_str());
				imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(8).c_str()), atof(dataSet.at(9).c_str()), atof(dataSet.at(10).c_str()));
			}
			else if (dataSet.at(0) == "ODOM") {
				odom.header.stamp = ros::Time::now();
				odom.pose.pose.position.x += atof(dataSet.at(2).c_str()) / 100.0;
				odom.pose.pose.position.y += atof(dataSet.at(3).c_str()) / 100.0;
				odom.pose.pose.position.z = 0.0;
				odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(atof(dataSet.at(4).c_str()));
				odom.twist.twist.linear.x = atof(dataSet.at(5).c_str()) / 100.0;
				odom.twist.twist.linear.y = atof(dataSet.at(6).c_str()) / 100.0;
				odom.twist.twist.angular.z = atof(dataSet.at(7).c_str());
			}
			else if (dataSet.at(0) == "USL") {
				sonarLeft.header.stamp = ros::Time::now();
				sonarLeft.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USC") {
				sonarCenter.header.stamp = ros::Time::now();
				sonarCenter.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USR") {
				sonarRight.header.stamp = ros::Time::now();
				sonarRight.range = atof(dataSet.at(2).c_str()) / 100.0;
			}

		}
	}
}
<commit_msg>Added PID for driving Added a PID in abridge to control all aspects of movment. It attempts to maintain a target velocity and make the yaw error that is passed in 0. Currently no operable due to mobility and arduino code notbeing setup to work with the PID This also means the pid is not tuned or run time testead.<commit_after>#include <ros/ros.h>

//ROS libraries
#include <tf/transform_datatypes.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>

//Package include
#include <usbSerial.h>

using namespace std;

//aBridge functions
void cmdHandler(const geometry_msgs::Twist::ConstPtr& message);
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void serialActivityTimer(const ros::TimerEvent& e);
void publishRosTopics();
void parseData(string data);

//Globals
geometry_msgs::QuaternionStamped fingerAngle;
geometry_msgs::QuaternionStamped wristAngle;
sensor_msgs::Imu imu;
nav_msgs::Odometry odom;
sensor_msgs::Range sonarLeft;
sensor_msgs::Range sonarCenter;
sensor_msgs::Range sonarRight;
USBSerial usb;
const int baud = 115200;
char dataCmd[] = "d\n";
char moveCmd[16];
char host[128];
float linearSpeed = 0.;
float yawError = 0.;
const float deltaTime = 0.1;

//PID constants and arrays
float Kpv = 0; //Proportinal Velocity
float Kiv = 0; //Integral Velocity
float Kdv = 0; //Derivative Velocity
int stepV = 0; //keeps track of the point in the array for adding new error each update cycle.
float evArray[1000]; //array of previous error for (arraySize/hz) seconds (error Velocity Array)
float pvError = 0; //previouse velocity error

float Kpy = 0; //Proportinal Yaw
float Kiy = 0; //Inegral Yaw
float Kdy = 0; //Derivative Yaw
int stepY = 0; //keeps track of the point in the array for adding new error each update cycle.
float eyArray[1000]; //array of previous error for (arraySize/hz) seconds (error Yaw Array)
float pyError = 0; //previouse yaw error

//Publishers
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher imuPublish;
ros::Publisher odomPublish;
ros::Publisher sonarLeftPublish;
ros::Publisher sonarCenterPublish;
ros::Publisher sonarRightPublish;

//Subscribers
ros::Subscriber velocitySubscriber;
ros::Subscriber fingerAngleSubscriber;
ros::Subscriber wristAngleSubscriber;

//Timers
ros::Timer publishTimer;

int main(int argc, char **argv) {

    gethostname(host, sizeof (host));
    string hostname(host);
    string publishedName;
    ros::init(argc, argv, (hostname + "_ABRIDGE"));

    ros::NodeHandle param("~");
    string devicePath;
    param.param("device", devicePath, string("/dev/ttyUSB0"));
    usb.openUSBPort(devicePath, baud);

    sleep(5);

    ros::NodeHandle aNH;

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "!  ABridge module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No Name Selected. Default is: " << publishedName << endl;
    }

    fingerAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/fingerAngle/prev_cmd"), 10);
    wristAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/wristAngle/prev_cmd"), 10);
    imuPublish = aNH.advertise<sensor_msgs::Imu>((publishedName + "/imu"), 10);
    odomPublish = aNH.advertise<nav_msgs::Odometry>((publishedName + "/odom"), 10);
    sonarLeftPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarLeft"), 10);
    sonarCenterPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarCenter"), 10);
    sonarRightPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarRight"), 10);

    velocitySubscriber = aNH.subscribe((publishedName + "/velocity"), 10, cmdHandler);
    fingerAngleSubscriber = aNH.subscribe((publishedName + "/fingerAngle/cmd"), 1, fingerAngleHandler);
    wristAngleSubscriber = aNH.subscribe((publishedName + "/wristAngle/cmd"), 1, wristAngleHandler);

    publishTimer = aNH.createTimer(ros::Duration(deltaTime), serialActivityTimer);

    imu.header.frame_id = publishedName+"/base_link";

    odom.header.frame_id = publishedName+"/odom";
    odom.child_frame_id = publishedName+"/base_link";

    ros::spin();

    for (int i = 0; i < 1000; i++)
    {
    evArray[i] = 0;
    eyArray[i] = 0;
    }

    return EXIT_SUCCESS;
}

void cmdHandler(const geometry_msgs::Twist::ConstPtr& message) {
    // remove artificial factor that was multiplied for simulation. this scales it back down to -1.0 to +1.0
  linearSpeed = (message->linear.x); // / 1.5;
  yawError = (message->angular.z); // / 8;

  //PID Code {
  float sat = 255; //Saturation point

  //Velocity--------------------------
  float xVel = odom.twist.twist.linear.x;
  float yVel = odom.twist.twist.linear.y;
  float vel = sqrt(xVel*xVel + yVel*yVel);
  float velError = linearSpeed - vel; //calculate the error
  float IV = 0;

  //Propotinal
  float PV = Kpv * velError;
  if (PV > sat) //limit the max and minimum output of proportinal
  PV = sat;
  if (PV < -sat)
  PV= -sat;

  //Integral
  if (velError > 1 || velError < -1) //only use integral when error is larger than presumed noise.
  {
    evArray[stepV] = velError; //add error into the error Array.
    stepV++;

    if (stepV >= 1000)
    stepV = 0;

    float sumV= 0;
    for (int i= 0; i < 1000; i++) //sum the array to get the error over time from t = 0 to present.
    {
        sumV += evArray[i];
    }

    IV = Kiv * sumV;
   }

    //anti windup                             //if PV is already commanding greater than half power dont use the integrel
    if (fabs(IV) > sat/2 || fabs(PV) > sat/2) //reset the integral to 0 if it hits its cap of half max power
    {
       for (int i= 0; i < 1000; i++)
        {
           evArray[i] = 0;
        }
        IV = 0;
    }

    float DV = 0;
    if (!(fabs(PV) > sat/2))
    {
    //Derivative
       DV = Kdv * (velError - pvError) * 10; //10 being the frequency of the system giving us a one second prediction base.
    }
    pvError = velError; //set previouse error to current error

    float velOut = PV + IV + DV;
    if (velOut > sat) //cap vel command
    {
        velOut = sat;
    }
    else if (velOut < -sat)
    {
        velOut = -sat;
    }

  //Yaw-----------------------------
  float IY = 0;

  //Propotinal
  float PY = Kpv * yawError;
  if (PY > sat) //limit the max and minimum output of proportinal
  PY = sat;
  if (PY < -sat)
  PY= -sat;

  //Integral
  if (yawError > 0.1 || yawError < -0.1) //only use integral when error is larger than presumed noise.
  {
    eyArray[stepY] = yawError; //add error into the error Array.
    stepY++;

    if (stepY >= 1000)
    stepY = 0;

    float sumY= 0;
    for (int i= 0; i < 1000; i++) //sum the array to get the error over time from t = 0 to present.
    {
        sumY += eyArray[i];
    }

    IY = Kiy * sumY;
   }

    //anti windup                                     //if PV is already commanding greater than half power dont use the integrel;
    if (fabs(IY) > sat/2 || fabs(PY) > sat/2) //reset the integral to 0 if it hits its cap
    {
       for (int i= 0; i < 1000; i++)
        {
           eyArray[i] = 0;
        }
        IY = 0;
    }

    float DY = 0;
    if (!(fabs(PY) > sat/2))
    {
    //Derivative
       DY = Kdy * (yawError - pyError) * 10; //10 being the frequency of the system giving us a one second prediction base.
    }
    pyError = yawError;

    float yawOut = PY + IY + DY;
    if (yawOut > sat) //cap yaw command
    {
        yawOut = sat;
    }
    else if (yawOut < -sat)
    {
        yawOut = -sat;
    }
    // } end PID

   int left = velOut + yawOut;
   int right = velOut - yawOut;

   if (left > sat)  {left = sat;}
   if (left < -sat) {left = -sat;}
   if (right > sat) {right = sat;}
   if (right < -sat){right = -sat;}

    sprintf(moveCmd, "v,%d%d\n", left, right);
    usb.sendData(moveCmd);
    memset(&moveCmd, '\0', sizeof (moveCmd));
}

// The finger and wrist handlers receive gripper angle commands in floating point
// radians, write them to a string and send that to the arduino
// for processing.
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
  char cmd[16]={'\0'};

  // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'f' indicates this is a finger command to the arduino
    sprintf(cmd, "f,0\n");
  } else {
    sprintf(cmd, "f,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
    char cmd[16]={'\0'};

    // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'w' indicates this is a wrist command to the arduino
    sprintf(cmd, "w,0\n");
  } else {
    sprintf(cmd, "w,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void serialActivityTimer(const ros::TimerEvent& e) {
    usb.sendData(dataCmd);
    parseData(usb.readData());
    publishRosTopics();
}

void publishRosTopics() {
    fingerAnglePublish.publish(fingerAngle);
    wristAnglePublish.publish(wristAngle);
    imuPublish.publish(imu);
    odomPublish.publish(odom);
    sonarLeftPublish.publish(sonarLeft);
    sonarCenterPublish.publish(sonarCenter);
    sonarRightPublish.publish(sonarRight);
}

void parseData(string str) {
    istringstream oss(str);
    string sentence;

    while (getline(oss, sentence, '\n')) {
		istringstream wss(sentence);
		string word;

		vector<string> dataSet;
		while (getline(wss, word, ',')) {
			dataSet.push_back(word);
		}

		if (dataSet.size() >= 3 && dataSet.at(1) == "1") {

			if (dataSet.at(0) == "GRF") {
				fingerAngle.header.stamp = ros::Time::now();
				fingerAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "GRW") {
				wristAngle.header.stamp = ros::Time::now();
				wristAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "IMU") {
				imu.header.stamp = ros::Time::now();
				imu.linear_acceleration.x = atof(dataSet.at(2).c_str());
				imu.linear_acceleration.y = atof(dataSet.at(3).c_str());
				imu.linear_acceleration.z = atof(dataSet.at(4).c_str());
				imu.angular_velocity.x = atof(dataSet.at(5).c_str());
				imu.angular_velocity.y = atof(dataSet.at(6).c_str());
				imu.angular_velocity.z = atof(dataSet.at(7).c_str());
				imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(8).c_str()), atof(dataSet.at(9).c_str()), atof(dataSet.at(10).c_str()));
			}
			else if (dataSet.at(0) == "ODOM") {
				odom.header.stamp = ros::Time::now();
				odom.pose.pose.position.x += atof(dataSet.at(2).c_str()) / 100.0;
				odom.pose.pose.position.y += atof(dataSet.at(3).c_str()) / 100.0;
				odom.pose.pose.position.z = 0.0;
				odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(atof(dataSet.at(4).c_str()));
				odom.twist.twist.linear.x = atof(dataSet.at(5).c_str()) / 100.0;
				odom.twist.twist.linear.y = atof(dataSet.at(6).c_str()) / 100.0;
				odom.twist.twist.angular.z = atof(dataSet.at(7).c_str());
			}
			else if (dataSet.at(0) == "USL") {
				sonarLeft.header.stamp = ros::Time::now();
				sonarLeft.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USC") {
				sonarCenter.header.stamp = ros::Time::now();
				sonarCenter.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USR") {
				sonarRight.header.stamp = ros::Time::now();
				sonarRight.range = atof(dataSet.at(2).c_str()) / 100.0;
			}

		}
	}
}
<|endoftext|>


<commit_before>#include "pacman.hh"

#include <fnmatch.h>
#include <glob.h>

#include <algorithm>
#include <fstream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>

namespace std {

template <>
struct default_delete<glob_t> {
  void operator()(glob_t* globbuf) {
    globfree(globbuf);
    delete globbuf;
  }
};

}  // namespace std

namespace {

std::string* ltrim(std::string* s) {
  s->erase(s->begin(),
           std::find_if(s->begin(), s->end(),
                        std::not1(std::ptr_fun<int, int>(std::isspace))));
  return s;
}

std::string* rtrim(std::string* s) {
  s->erase(std::find_if(s->rbegin(), s->rend(),
                        std::not1(std::ptr_fun<int, int>(std::isspace)))
               .base(),
           s->end());
  return s;
}

std::string* trim(std::string* s) { return ltrim(rtrim(s)); }

bool IsSection(const std::string& s) {
  return s.size() > 2 && s[0] == '[' && s[s.size() - 1] == ']';
}

}  // namespace

namespace auracle {

Pacman::Pacman(alpm_handle_t* alpm, std::vector<std::string> ignored_packages)
    : alpm_(alpm),
      local_db_(alpm_get_localdb(alpm_)),
      ignored_packages_(std::move(ignored_packages)) {}

Pacman::~Pacman() { alpm_release(alpm_); }

struct ParseState {
  alpm_handle_t* alpm;
  std::string dbpath = "/var/lib/pacman";
  std::string rootdir = "/";

  std::string section;
  std::vector<std::string> ignorepkgs;
  std::vector<std::string> repos;
};

bool ParseOneFile(const std::string& path, ParseState* state) {
  std::ifstream file(path);

  std::string line;
  while (std::getline(file, line)) {
    trim(&line);

    if (line.empty() || line[0] == '#') {
      continue;
    }

    if (IsSection(line)) {
      state->section = line.substr(1, line.size() - 2);
      continue;
    }

    auto equals = line.find('=');
    if (equals == std::string::npos) {
      // There aren't any directives we care about which are valueless.
      continue;
    }

    auto key = line.substr(0, equals);
    trim(&key);

    auto value = line.substr(equals + 1);
    trim(&value);

    if (state->section == "options") {
      if (key == "IgnorePkg") {
        std::istringstream iss(value);
        std::copy(std::istream_iterator<std::string>(iss),
                  std::istream_iterator<std::string>(),
                  std::back_inserter(state->ignorepkgs));
      } else if (key == "DBPath") {
        state->dbpath = value;
      } else if (key == "RootDir") {
        state->rootdir = value;
      }
    } else {
      state->repos.emplace_back(state->section);
    }

    if (key == "Include") {
      auto globbuf = std::make_unique<glob_t>();

      if (glob(value.c_str(), GLOB_NOCHECK, nullptr, globbuf.get()) != 0) {
        return false;
      }

      for (size_t i = 0; i < globbuf->gl_pathc; ++i) {
        if (!ParseOneFile(globbuf->gl_pathv[i], state)) {
          return false;
        }
      }
    }
  }

  file.close();

  return true;
}

// static
std::unique_ptr<Pacman> Pacman::NewFromConfig(const std::string& config_file) {
  ParseState state;

  if (!ParseOneFile(config_file, &state)) {
    return nullptr;
  }

  alpm_errno_t err;
  state.alpm = alpm_initialize("/", state.dbpath.c_str(), &err);
  if (state.alpm == nullptr) {
    return nullptr;
  }

  for (const auto& repo : state.repos) {
    alpm_register_syncdb(state.alpm, repo.c_str(),
                         static_cast<alpm_siglevel_t>(0));
  }

  return std::unique_ptr<Pacman>(
      new Pacman(state.alpm, std::move(state.ignorepkgs)));
}

bool Pacman::ShouldIgnorePackage(const std::string& package) const {
  return std::find_if(ignored_packages_.cbegin(), ignored_packages_.cend(),
                      [&package](const std::string& p) {
                        return fnmatch(p.c_str(), package.c_str(), 0) == 0;
                      }) != ignored_packages_.cend();
}

std::string Pacman::RepoForPackage(const std::string& package) const {
  for (auto i = alpm_get_syncdbs(alpm_); i != nullptr; i = i->next) {
    auto db = static_cast<alpm_db_t*>(i->data);
    auto pkgcache = alpm_db_get_pkgcache(db);

    if (alpm_find_satisfier(pkgcache, package.c_str()) != nullptr) {
      return alpm_db_get_name(db);
    }
  }

  return std::string();
}

bool Pacman::DependencyIsSatisfied(const std::string& package) const {
  auto* cache = alpm_db_get_pkgcache(local_db_);
  return alpm_find_satisfier(cache, package.c_str()) != nullptr;
}

std::optional<Pacman::Package> Pacman::GetLocalPackage(
    const std::string& name) const {
  auto* pkg = alpm_db_get_pkg(local_db_, name.c_str());
  if (pkg == nullptr) {
    return std::nullopt;
  }

  return Package{alpm_pkg_get_name(pkg), alpm_pkg_get_version(pkg)};
}

std::vector<Pacman::Package> Pacman::ForeignPackages() const {
  std::vector<Package> packages;

  for (auto i = alpm_db_get_pkgcache(local_db_); i != nullptr; i = i->next) {
    const auto pkg = static_cast<alpm_pkg_t*>(i->data);
    std::string pkgname(alpm_pkg_get_name(pkg));

    if (RepoForPackage(pkgname).empty()) {
      packages.emplace_back(std::move(pkgname), alpm_pkg_get_version(pkg));
    }
  }

  return packages;
}

// static
int Pacman::Vercmp(const std::string& a, const std::string& b) {
  return alpm_pkg_vercmp(a.c_str(), b.c_str());
}

}  // namespace auracle
<commit_msg>Avoid use of deprecated/removed functions<commit_after>#include "pacman.hh"

#include <fnmatch.h>
#include <glob.h>

#include <algorithm>
#include <fstream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>

namespace std {

template <>
struct default_delete<glob_t> {
  void operator()(glob_t* globbuf) {
    globfree(globbuf);
    delete globbuf;
  }
};

}  // namespace std

namespace {

bool notspace(char c) { return !std::isspace(c); }

std::string* ltrim(std::string* s) {
  s->erase(s->begin(), std::find_if(s->begin(), s->end(), &notspace));
  return s;
}

std::string* rtrim(std::string* s) {
  s->erase(std::find_if(s->rbegin(), s->rend(), &notspace).base(), s->end());
  return s;
}

std::string* trim(std::string* s) { return ltrim(rtrim(s)); }

bool IsSection(const std::string& s) {
  return s.size() > 2 && s[0] == '[' && s[s.size() - 1] == ']';
}

}  // namespace

namespace auracle {

Pacman::Pacman(alpm_handle_t* alpm, std::vector<std::string> ignored_packages)
    : alpm_(alpm),
      local_db_(alpm_get_localdb(alpm_)),
      ignored_packages_(std::move(ignored_packages)) {}

Pacman::~Pacman() { alpm_release(alpm_); }

struct ParseState {
  alpm_handle_t* alpm;
  std::string dbpath = "/var/lib/pacman";
  std::string rootdir = "/";

  std::string section;
  std::vector<std::string> ignorepkgs;
  std::vector<std::string> repos;
};

bool ParseOneFile(const std::string& path, ParseState* state) {
  std::ifstream file(path);

  std::string line;
  while (std::getline(file, line)) {
    trim(&line);

    if (line.empty() || line[0] == '#') {
      continue;
    }

    if (IsSection(line)) {
      state->section = line.substr(1, line.size() - 2);
      continue;
    }

    auto equals = line.find('=');
    if (equals == std::string::npos) {
      // There aren't any directives we care about which are valueless.
      continue;
    }

    auto key = line.substr(0, equals);
    trim(&key);

    auto value = line.substr(equals + 1);
    trim(&value);

    if (state->section == "options") {
      if (key == "IgnorePkg") {
        std::istringstream iss(value);
        std::copy(std::istream_iterator<std::string>(iss),
                  std::istream_iterator<std::string>(),
                  std::back_inserter(state->ignorepkgs));
      } else if (key == "DBPath") {
        state->dbpath = value;
      } else if (key == "RootDir") {
        state->rootdir = value;
      }
    } else {
      state->repos.emplace_back(state->section);
    }

    if (key == "Include") {
      auto globbuf = std::make_unique<glob_t>();

      if (glob(value.c_str(), GLOB_NOCHECK, nullptr, globbuf.get()) != 0) {
        return false;
      }

      for (size_t i = 0; i < globbuf->gl_pathc; ++i) {
        if (!ParseOneFile(globbuf->gl_pathv[i], state)) {
          return false;
        }
      }
    }
  }

  file.close();

  return true;
}

// static
std::unique_ptr<Pacman> Pacman::NewFromConfig(const std::string& config_file) {
  ParseState state;

  if (!ParseOneFile(config_file, &state)) {
    return nullptr;
  }

  alpm_errno_t err;
  state.alpm = alpm_initialize("/", state.dbpath.c_str(), &err);
  if (state.alpm == nullptr) {
    return nullptr;
  }

  for (const auto& repo : state.repos) {
    alpm_register_syncdb(state.alpm, repo.c_str(),
                         static_cast<alpm_siglevel_t>(0));
  }

  return std::unique_ptr<Pacman>(
      new Pacman(state.alpm, std::move(state.ignorepkgs)));
}

bool Pacman::ShouldIgnorePackage(const std::string& package) const {
  return std::find_if(ignored_packages_.cbegin(), ignored_packages_.cend(),
                      [&package](const std::string& p) {
                        return fnmatch(p.c_str(), package.c_str(), 0) == 0;
                      }) != ignored_packages_.cend();
}

std::string Pacman::RepoForPackage(const std::string& package) const {
  for (auto i = alpm_get_syncdbs(alpm_); i != nullptr; i = i->next) {
    auto db = static_cast<alpm_db_t*>(i->data);
    auto pkgcache = alpm_db_get_pkgcache(db);

    if (alpm_find_satisfier(pkgcache, package.c_str()) != nullptr) {
      return alpm_db_get_name(db);
    }
  }

  return std::string();
}

bool Pacman::DependencyIsSatisfied(const std::string& package) const {
  auto* cache = alpm_db_get_pkgcache(local_db_);
  return alpm_find_satisfier(cache, package.c_str()) != nullptr;
}

std::optional<Pacman::Package> Pacman::GetLocalPackage(
    const std::string& name) const {
  auto* pkg = alpm_db_get_pkg(local_db_, name.c_str());
  if (pkg == nullptr) {
    return std::nullopt;
  }

  return Package{alpm_pkg_get_name(pkg), alpm_pkg_get_version(pkg)};
}

std::vector<Pacman::Package> Pacman::ForeignPackages() const {
  std::vector<Package> packages;

  for (auto i = alpm_db_get_pkgcache(local_db_); i != nullptr; i = i->next) {
    const auto pkg = static_cast<alpm_pkg_t*>(i->data);
    std::string pkgname(alpm_pkg_get_name(pkg));

    if (RepoForPackage(pkgname).empty()) {
      packages.emplace_back(std::move(pkgname), alpm_pkg_get_version(pkg));
    }
  }

  return packages;
}

// static
int Pacman::Vercmp(const std::string& a, const std::string& b) {
  return alpm_pkg_vercmp(a.c_str(), b.c_str());
}

}  // namespace auracle
<|endoftext|>


<commit_before>#include "playsong.h"
//#include "mary.mid.h"

//byte dir = HIGH;

void playSong(const uint32_t *const *song)
{
  if (!song) return;

  WaitTime wt[4] = { };

  uint32_t indx[4] = { 0 };

  uint32_t freq[] = {
    song[0] ? getElement(song[0], 0) : DONE,
    song[1] ? getElement(song[1], 0) : DONE,
    song[2] ? getElement(song[2], 0) : DONE,
    song[3] ? getElement(song[3], 0) : DONE,
  };

  uint32_t len[] = {
    song[0] ? getElement(song[0], 1) : DONE,
    song[1] ? getElement(song[1], 1) : DONE,
    song[2] ? getElement(song[2], 1) : DONE,
    song[3] ? getElement(song[3], 1) : DONE,
  };

  uint64_t currentTime = 0;

  while (!(isDone(freq[0], len[0]) && isDone(freq[1], len[1]) &&
           isDone(freq[2], len[2]) && isDone(freq[3], len[3]))) {
    for (uint8_t d = 0; d < 4; ++d) {
      currentTime = (uint64_t)micros();

      if (!isDone(freq[d], len[d])) {
          if (wt[d].nextStep < currentTime) {

            if (freq[d] != REST) pulse(d);
            wt[d].nextStep = currentTime + (freq[d] = (uint64_t)(getElement(song[d], indx[d])));
          }

        if (!wt[d].stopTime) {
          wt[d].stopTime = currentTime + (len[d] = (uint64_t)(getElement(song[d], indx[d] + 1)));
        }

        if (wt[d].stopTime < currentTime) {
          indx[d] += 2;
          wt[d].nextStep = currentTime + (freq[d] = (uint64_t)(getElement(song[d], indx[d])));
          wt[d].stopTime = currentTime + (len[d]  = (uint64_t)(getElement(song[d], indx[d] + 1)));
        }
      }
    }
  }
}
<commit_msg>i have no idea<commit_after>#include "playsong.h"

void playSong(const uint32_t *const *song)
{
  if (!song) return;

  WaitTime wt[4] = { };

  uint32_t indx[4] = { 0 };

  uint32_t freq[] = {
    song[0] ? getElement(song[0], 0) : DONE,
    song[1] ? getElement(song[1], 0) : DONE,
    song[2] ? getElement(song[2], 0) : DONE,
    song[3] ? getElement(song[3], 0) : DONE,
  };

  uint32_t len[] = {
    song[0] ? getElement(song[0], 1) : DONE,
    song[1] ? getElement(song[1], 1) : DONE,
    song[2] ? getElement(song[2], 1) : DONE,
    song[3] ? getElement(song[3], 1) : DONE,
  };

  uint64_t currentTime = 0;

  while (!(isDone(freq[0], len[0]) && isDone(freq[1], len[1]) &&
           isDone(freq[2], len[2]) && isDone(freq[3], len[3]))) {
    for (uint8_t d = 0; d < 4; ++d) {
      currentTime = (uint64_t)micros();

      if (!isDone(freq[d], len[d])) {
          if (wt[d].nextStep < currentTime) {

            if (freq[d] != REST) pulse(d);
            wt[d].nextStep = currentTime + (freq[d] = (uint64_t)(getElement(song[d], indx[d])));
          }

        if (!wt[d].stopTime) {
          wt[d].stopTime = currentTime + (len[d] = (uint64_t)(getElement(song[d], indx[d] + 1)));
        }

        if (wt[d].stopTime < currentTime) {
          indx[d] += 2;
          wt[d].nextStep = currentTime + (freq[d] = (uint64_t)(getElement(song[d], indx[d])));
          wt[d].stopTime = currentTime + (len[d]  = (uint64_t)(getElement(song[d], indx[d] + 1)));
        }
      }
    }
  }
}
<|endoftext|>


<commit_before>#include "guard.h"

#include "core/level.h"
#include "core/environment.h"
#include "core/keyboardevent.h"

#include <iostream>

using namespace std;

Guard::Guard(Object *parent, ObjectID id, double x, double y, int mass, bool walkable, string t, int dir)
    : Object(parent, id, x, y), m_animation (new Animation("res/sprites/idle.png",
    	0, 0, 70, 70, 2, 1000, true)), m_direction((Direction) dir), m_last(0), type(t)
{
    this->set_w(70);
    this->set_h(70);
    this->set_walkable(walkable);
    update_vision();
}

Guard::~Guard()
{
}

Guard::Direction
Guard::direction()
{
    return m_direction;
}

void
Guard::update_vision()
{
    const list<Object *> filhos = this->children();

    for (auto filho : filhos)
    {
        if(filho->id() == "visao")
        {
            remove_child(filho);
        }
    }

    if(direction() == Guard::RIGHT)
    {
        Sight *visao = new Sight(this, "visao", this->x()+40, this->y(), 200, 80);
        add_child(visao);
    }
    else if(direction() == Guard::LEFT)
    {
        Sight *visao = new Sight(this, "visao", this->x() - 200, this->y(), 240, 80);
        add_child(visao);
    }
    else if(direction() == Guard::UP)
    {
        Sight *visao = new Sight(this, "visao", this->x(), this->y() - 200, 80, 240);
        add_child(visao);
    }
    else if(direction() == Guard::DOWN)
    {
        Sight *visao = new Sight(this, "visao", this->x(), this->y() + 40, 80, 200);
        add_child(visao);
    }

}

void
Guard::set_direction(Direction direction)
{
    m_direction = direction;
}

void
Guard::draw_self()
{
    m_animation->draw(x(), y());
}

void
Guard::walk(unsigned long elapsed)
{
    double speed = 0.3;
    if(type == "easy")
        return;
    else if(type == "normal")
    {

        if(elapsed - m_last > 3000)
        {
            if(direction() == Guard::RIGHT || direction() == Guard::LEFT)
            {
                set_x(x() - speed + (speed * direction()));
            }
            if(direction() == Guard::UP || direction() == Guard::DOWN)
            {
                set_y(y() - 2 * speed + (speed * direction()));
            }
        }
    }
    else if(type == "hard")
    {

        if(player_posx < this->x())
            set_x(x() - speed);
        else
            set_x(x() + speed);

        if(player_posy < this->y())
            set_y(y() - speed);
        else
            set_y(y() + speed);

    }
        return;
}

void
Guard::update_direction(unsigned long elapsed)
{

    if(type == "easy")
    {
        if(elapsed - m_last > 1000)
        {
            int random = rand()%100;

            if(random < 25)
                set_direction(Guard::LEFT);
            else if(random < 50)
                set_direction(Guard::UP);
            else if(random < 75)
                set_direction(Guard::RIGHT);
            else
                set_direction(Guard::DOWN);

            m_last = elapsed;
        }
    }
    else if (type == "normal")
    {
        if(elapsed - m_last > 5000)
        {
            int test = ((int)direction() + 2) % 4;
            Direction new_direction = (Direction)test;
            set_direction(new_direction);

            m_last = elapsed;
        }
    }
    else if(type == "hard")
    {
        if(elapsed - m_last > 5000)
        {
            int random = rand()%100;

            if(random < 25)
                set_direction(Guard::LEFT);
            else if(random < 50)
                set_direction(Guard::UP);
            else if(random < 75)
                set_direction(Guard::RIGHT);
            else
                set_direction(Guard::DOWN);

            m_last = elapsed;
        }
    }
    m_animation->set_row(this->direction());
}

void
Guard::get_playerx(int pos_x)
{
    player_posx = pos_x;
}

void
Guard::get_playery(int pos_y)
{
    player_posy = pos_y;
}

void
Guard::update_self(unsigned long elapsed)
{

    set_x(this->x());
    set_y(this->y());

    update_direction(elapsed);
    m_animation->update(elapsed);
    walk(elapsed);
    update_vision();
}

    <commit_msg>Melhorando a IA do guarda no modo hard.<commit_after>#include "guard.h"

#include "core/level.h"
#include "core/environment.h"
#include "core/keyboardevent.h"

#include <iostream>

using namespace std;

Guard::Guard(Object *parent, ObjectID id, double x, double y, int mass, bool walkable, string t, int dir)
    : Object(parent, id, x, y), m_animation (new Animation("res/sprites/idle.png",
    	0, 0, 70, 70, 2, 1000, true)), m_direction((Direction) dir), m_last(0), type(t)
{
    this->set_w(70);
    this->set_h(70);
    this->set_walkable(walkable);
    update_vision();
}

Guard::~Guard()
{
}

Guard::Direction
Guard::direction()
{
    return m_direction;
}

void
Guard::update_vision()
{
    const list<Object *> filhos = this->children();

    for (auto filho : filhos)
    {
        if(filho->id() == "visao")
        {
            remove_child(filho);
        }
    }

    if(direction() == Guard::RIGHT)
    {
        Sight *visao = new Sight(this, "visao", this->x()+40, this->y(), 200, 80);
        add_child(visao);
    }
    else if(direction() == Guard::LEFT)
    {
        Sight *visao = new Sight(this, "visao", this->x() - 200, this->y(), 240, 80);
        add_child(visao);
    }
    else if(direction() == Guard::UP)
    {
        Sight *visao = new Sight(this, "visao", this->x(), this->y() - 200, 80, 240);
        add_child(visao);
    }
    else if(direction() == Guard::DOWN)
    {
        Sight *visao = new Sight(this, "visao", this->x(), this->y() + 40, 80, 200);
        add_child(visao);
    }

}

void
Guard::set_direction(Direction direction)
{
    m_direction = direction;
}

void
Guard::draw_self()
{
    m_animation->draw(x(), y());
}

void
Guard::walk(unsigned long elapsed)
{
    double speed = 0.3;
    if(type == "easy")
        return;
    else if(type == "normal")
    {

        if(elapsed - m_last > 3000)
        {
            if(direction() == Guard::RIGHT || direction() == Guard::LEFT)
            {
                set_x(x() - speed + (speed * direction()));
            }
            if(direction() == Guard::UP || direction() == Guard::DOWN)
            {
                set_y(y() - 2 * speed + (speed * direction()));
            }
        }
    }
    else if(type == "hard")
    {

        if(player_posx < this->x())
            set_x(x() - speed);
        else
            set_x(x() + speed);

        if(player_posy < this->y())
            set_y(y() - speed);
        else
            set_y(y() + speed);

        if(player_posx > this->x() - 100 && player_posx < this->x() + 100 && player_posy < this->y())
            set_direction(Guard::UP);
        else if(player_posx > this->x() - 100 && player_posx < this->x() + 100 && player_posy > this->y())
            set_direction(Guard::DOWN);
        else if(player_posx < this->x())
            set_direction(Guard::LEFT);
        else
            set_direction(Guard::RIGHT);

    }
        return;
}

void
Guard::update_direction(unsigned long elapsed)
{

    if(type == "easy")
    {
        if(elapsed - m_last > 1000)
        {
            int random = rand()%100;

            if(random < 25)
                set_direction(Guard::LEFT);
            else if(random < 50)
                set_direction(Guard::UP);
            else if(random < 75)
                set_direction(Guard::RIGHT);
            else
                set_direction(Guard::DOWN);

            m_last = elapsed;
        }
    }
    else if (type == "normal")
    {
        if(elapsed - m_last > 5000)
        {
            int test = ((int)direction() + 2) % 4;
            Direction new_direction = (Direction)test;
            set_direction(new_direction);

            m_last = elapsed;
        }
    }
    else if(type == "hard")
    {
        if(elapsed - m_last > 5000)
        {
            int random = rand()%100;

            if(random < 25)
                set_direction(Guard::LEFT);
            else if(random < 50)
                set_direction(Guard::UP);
            else if(random < 75)
                set_direction(Guard::RIGHT);
            else
                set_direction(Guard::DOWN);

            m_last = elapsed;
        }
    }
    m_animation->set_row(this->direction());
}

void
Guard::get_playerx(int pos_x)
{
    player_posx = pos_x;
}

void
Guard::get_playery(int pos_y)
{
    player_posy = pos_y;
}

void
Guard::update_self(unsigned long elapsed)
{

    set_x(this->x());
    set_y(this->y());

    update_direction(elapsed);
    m_animation->update(elapsed);
    walk(elapsed);
    update_vision();
}

    <|endoftext|>