<commit_before>////////////////////////////////////////////////////////////////////////////////
/// @brief Aql, query context
///
/// @file
///
/// DISCLAIMER
///
/// Copyright 2014 ArangoDB GmbH, Cologne, Germany
/// Copyright 2004-2014 triAGENS GmbH, Cologne, Germany
///
/// Licensed under the Apache License, Version 2.0 (the "License");
/// you may not use this file except in compliance with the License.
/// You may obtain a copy of the License at
///
///     http://www.apache.org/licenses/LICENSE-2.0
///
/// Unless required by applicable law or agreed to in writing, software
/// distributed under the License is distributed on an "AS IS" BASIS,
/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
/// See the License for the specific language governing permissions and
/// limitations under the License.
///
/// Copyright holder is ArangoDB GmbH, Cologne, Germany
///
/// @author Jan Steemann
/// @author Copyright 2014, ArangoDB GmbH, Cologne, Germany
/// @author Copyright 2012-2013, triAGENS GmbH, Cologne, Germany
////////////////////////////////////////////////////////////////////////////////

#include "Aql/Query.h"
#include "Aql/ExecutionBlock.h"
#include "Aql/ExecutionEngine.h"
#include "Aql/ExecutionPlan.h"
#include "Aql/Parser.h"
#include "Aql/V8Executor.h"
#include "Aql/ExecutionEngine.h"
#include "Basics/JsonHelper.h"
#include "BasicsC/json.h"
#include "BasicsC/tri-strings.h"
#include "Utils/AqlTransaction.h"
#include "Utils/Exception.h"
#include "Utils/V8TransactionContext.h"
#include "VocBase/vocbase.h"

using namespace triagens::aql;

// -----------------------------------------------------------------------------
// --SECTION--                                        constructors / destructors
// -----------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
/// @brief creates a query
////////////////////////////////////////////////////////////////////////////////

Query::Query (TRI_vocbase_t* vocbase,
              char const* queryString,
              size_t queryLength,
              TRI_json_t* bindParameters)
  : _vocbase(vocbase),
    _executor(nullptr),
    _queryString(queryString),
    _queryLength(queryLength),
    _type(AQL_QUERY_READ),
    _bindParameters(bindParameters),
    _collections(vocbase),
    _strings() {

  TRI_ASSERT(_vocbase != nullptr);

  _strings.reserve(32);
}

////////////////////////////////////////////////////////////////////////////////
/// @brief destroys a query
////////////////////////////////////////////////////////////////////////////////

Query::~Query () {
  if (_executor != nullptr) {
    delete _executor;
    _executor = nullptr;
  }

  // free strings
  for (auto it = _strings.begin(); it != _strings.end(); ++it) {
    TRI_FreeString(TRI_UNKNOWN_MEM_ZONE, const_cast<char*>(*it));
  }
}

// -----------------------------------------------------------------------------
// --SECTION--                                                    public methods
// -----------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
/// @brief extract a region from the query
////////////////////////////////////////////////////////////////////////////////

std::string Query::extractRegion (int line,
                                  int column) const {
  // note: line numbers reported by bison/flex start at 1, columns start at 0
  int currentLine   = 1;
  int currentColumn = 0;

  char c;
  char const* p = _queryString;

  while ((c = *p)) {
    if (currentLine > line ||
        (currentLine >= line && currentColumn >= column)) {
      break;
    }

    if (c == '\n') {
      ++p;
      ++currentLine;
      currentColumn = 0;
    }
    else if (c == '\r') {
      ++p;
      ++currentLine;
      currentColumn = 0;

      // eat a following newline
      if (*p == '\n') {
        ++p;
      }
    }
    else {
      ++currentColumn;
      ++p;
    }
  }

  // p is pointing at the position in the query the parse error occurred at
  TRI_ASSERT(p >= _queryString);

  size_t offset = static_cast<size_t>(p - _queryString);

  static int const   SNIPPET_LENGTH = 32;
  static char const* SNIPPET_SUFFIX = "...";

  if (_queryLength < offset + SNIPPET_LENGTH) {
    // return a copy of the region
    return std::string(_queryString + offset, _queryLength - offset);
  }

  // copy query part
  std::string result(_queryString + offset, SNIPPET_LENGTH);
  result.append(SNIPPET_SUFFIX);

  return result;
}

////////////////////////////////////////////////////////////////////////////////
/// @brief register an error
////////////////////////////////////////////////////////////////////////////////

void Query::registerError (int code,
                           char const* details) {

  TRI_ASSERT(code != TRI_ERROR_NO_ERROR);

  if (details == nullptr) {
    THROW_ARANGO_EXCEPTION(code);
  }
  else {
    THROW_ARANGO_EXCEPTION_STRING(code, details);
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief execute an AQL query
////////////////////////////////////////////////////////////////////////////////

QueryResult Query::execute () {
  try {
    Parser parser(this);
    parser.parse();

    // put in bind parameters
    parser.ast()->injectBindParameters(_bindParameters);
    // optimize the ast
    parser.ast()->optimize();

    triagens::arango::AqlTransaction<triagens::arango::V8TransactionContext<true>> trx(_vocbase, _collections.collections());

    int res = trx.begin();

    if (res != TRI_ERROR_NO_ERROR) {
      return QueryResult(res, TRI_errno_string(res));
    }

    auto plan = ExecutionPlan::instanciateFromAst(parser.ast());

    triagens::basics::Json json(triagens::basics::Json::List);

    try {
      auto engine = ExecutionEngine::instanciateFromPlan(&trx, plan->root());

      try {
        auto root = engine->root();
        root->execute();

        AqlItemBlock* value;

        while (nullptr != (value = root->getOne())) {
          AqlValue val = value->getValue(0, 0);
          TRI_ASSERT(! val.isEmpty());
          auto doc = value->getDocumentCollection(0);
          json.add(val.toJson(doc));
          // TODO: remove debug output
          //std::cout << val.toString(value->getDocumentCollections()[0]) << std::endl;
          delete value;
        }

        delete engine;
      }
      catch (...) {
        delete engine;
        delete plan;
        // TODO: convert exception code
        return QueryResult(TRI_ERROR_INTERNAL);
      }
    }
    catch (triagens::arango::Exception const& ex) {
      delete plan;
      return QueryResult(ex.code(), ex.message());
    }
    catch (...) {
      delete plan;
      // TODO: convert exception code
      return QueryResult(TRI_ERROR_INTERNAL);
    }

    delete plan;
    trx.commit();

    QueryResult result(TRI_ERROR_NO_ERROR);
    result.json = parser.ast()->toJson(TRI_UNKNOWN_MEM_ZONE);

    return result;
  }
  catch (triagens::arango::Exception const& ex) {
    return QueryResult(ex.code(), ex.message());
  }
  catch (...) {
    return QueryResult(TRI_ERROR_OUT_OF_MEMORY, TRI_errno_string(TRI_ERROR_OUT_OF_MEMORY));
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief parse an AQL query
////////////////////////////////////////////////////////////////////////////////

QueryResult Query::parse () {
  try {
    Parser parser(this);
    return parser.parse();
  }
  catch (triagens::arango::Exception const& ex) {
    return QueryResult(ex.code(), ex.message());
  }
  catch (...) {
    return QueryResult(TRI_ERROR_OUT_OF_MEMORY, TRI_errno_string(TRI_ERROR_OUT_OF_MEMORY));
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief explain an AQL query - TODO: implement and determine return type
////////////////////////////////////////////////////////////////////////////////

void Query::explain () {
}

////////////////////////////////////////////////////////////////////////////////
/// @brief get v8 executor
////////////////////////////////////////////////////////////////////////////////

V8Executor* Query::executor () {
  if (_executor == nullptr) {
    // the executor is a singleton per query
    _executor = new V8Executor;
  }

  TRI_ASSERT(_executor != nullptr);
  return _executor;
}

////////////////////////////////////////////////////////////////////////////////
/// @brief register a string
/// the string is freed when the query is destroyed
////////////////////////////////////////////////////////////////////////////////

char* Query::registerString (char const* p,
                             size_t length,
                             bool mustUnescape) {

  if (p == nullptr) {
    THROW_ARANGO_EXCEPTION(TRI_ERROR_OUT_OF_MEMORY);
  }

  if (length == 0) {
    static char const* empty = "";
    // optimisation for the empty string
    return const_cast<char*>(empty);
  }

  char* copy = nullptr;
  if (mustUnescape) {
    size_t outLength;
    copy = TRI_UnescapeUtf8StringZ(TRI_UNKNOWN_MEM_ZONE, p, length, &outLength);
  }
  else {
    copy = TRI_DuplicateString2Z(TRI_UNKNOWN_MEM_ZONE, p, length);
  }

  if (copy == nullptr) {
    THROW_ARANGO_EXCEPTION(TRI_ERROR_OUT_OF_MEMORY);
  }

  try {
    _strings.push_back(copy);
  }
  catch (...) {
    THROW_ARANGO_EXCEPTION(TRI_ERROR_OUT_OF_MEMORY);
  }

  return copy;
}

// -----------------------------------------------------------------------------
// --SECTION--                                                       END-OF-FILE
// -----------------------------------------------------------------------------

// Local Variables:
// mode: outline-minor
// outline-regexp: "/// @brief\\|/// {@inheritDoc}\\|/// @page\\|// --SECTION--\\|/// @\\}"
// End:
<commit_msg>removing old code.<commit_after>////////////////////////////////////////////////////////////////////////////////
/// @brief Aql, query context
///
/// @file
///
/// DISCLAIMER
///
/// Copyright 2014 ArangoDB GmbH, Cologne, Germany
/// Copyright 2004-2014 triAGENS GmbH, Cologne, Germany
///
/// Licensed under the Apache License, Version 2.0 (the "License");
/// you may not use this file except in compliance with the License.
/// You may obtain a copy of the License at
///
///     http://www.apache.org/licenses/LICENSE-2.0
///
/// Unless required by applicable law or agreed to in writing, software
/// distributed under the License is distributed on an "AS IS" BASIS,
/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
/// See the License for the specific language governing permissions and
/// limitations under the License.
///
/// Copyright holder is ArangoDB GmbH, Cologne, Germany
///
/// @author Jan Steemann
/// @author Copyright 2014, ArangoDB GmbH, Cologne, Germany
/// @author Copyright 2012-2013, triAGENS GmbH, Cologne, Germany
////////////////////////////////////////////////////////////////////////////////

#include "Aql/Query.h"
#include "Aql/ExecutionBlock.h"
#include "Aql/ExecutionEngine.h"
#include "Aql/ExecutionPlan.h"
#include "Aql/Parser.h"
#include "Aql/V8Executor.h"
#include "Aql/ExecutionEngine.h"
#include "Basics/JsonHelper.h"
#include "BasicsC/json.h"
#include "BasicsC/tri-strings.h"
#include "Utils/AqlTransaction.h"
#include "Utils/Exception.h"
#include "Utils/V8TransactionContext.h"
#include "VocBase/vocbase.h"

using namespace triagens::aql;

// -----------------------------------------------------------------------------
// --SECTION--                                        constructors / destructors
// -----------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
/// @brief creates a query
////////////////////////////////////////////////////////////////////////////////

Query::Query (TRI_vocbase_t* vocbase,
              char const* queryString,
              size_t queryLength,
              TRI_json_t* bindParameters)
  : _vocbase(vocbase),
    _executor(nullptr),
    _queryString(queryString),
    _queryLength(queryLength),
    _type(AQL_QUERY_READ),
    _bindParameters(bindParameters),
    _collections(vocbase),
    _strings() {

  TRI_ASSERT(_vocbase != nullptr);

  _strings.reserve(32);
}

////////////////////////////////////////////////////////////////////////////////
/// @brief destroys a query
////////////////////////////////////////////////////////////////////////////////

Query::~Query () {
  if (_executor != nullptr) {
    delete _executor;
    _executor = nullptr;
  }

  // free strings
  for (auto it = _strings.begin(); it != _strings.end(); ++it) {
    TRI_FreeString(TRI_UNKNOWN_MEM_ZONE, const_cast<char*>(*it));
  }
}

// -----------------------------------------------------------------------------
// --SECTION--                                                    public methods
// -----------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
/// @brief extract a region from the query
////////////////////////////////////////////////////////////////////////////////

std::string Query::extractRegion (int line,
                                  int column) const {
  // note: line numbers reported by bison/flex start at 1, columns start at 0
  int currentLine   = 1;
  int currentColumn = 0;

  char c;
  char const* p = _queryString;

  while ((c = *p)) {
    if (currentLine > line ||
        (currentLine >= line && currentColumn >= column)) {
      break;
    }

    if (c == '\n') {
      ++p;
      ++currentLine;
      currentColumn = 0;
    }
    else if (c == '\r') {
      ++p;
      ++currentLine;
      currentColumn = 0;

      // eat a following newline
      if (*p == '\n') {
        ++p;
      }
    }
    else {
      ++currentColumn;
      ++p;
    }
  }

  // p is pointing at the position in the query the parse error occurred at
  TRI_ASSERT(p >= _queryString);

  size_t offset = static_cast<size_t>(p - _queryString);

  static int const   SNIPPET_LENGTH = 32;
  static char const* SNIPPET_SUFFIX = "...";

  if (_queryLength < offset + SNIPPET_LENGTH) {
    // return a copy of the region
    return std::string(_queryString + offset, _queryLength - offset);
  }

  // copy query part
  std::string result(_queryString + offset, SNIPPET_LENGTH);
  result.append(SNIPPET_SUFFIX);

  return result;
}

////////////////////////////////////////////////////////////////////////////////
/// @brief register an error
////////////////////////////////////////////////////////////////////////////////

void Query::registerError (int code,
                           char const* details) {

  TRI_ASSERT(code != TRI_ERROR_NO_ERROR);

  if (details == nullptr) {
    THROW_ARANGO_EXCEPTION(code);
  }
  else {
    THROW_ARANGO_EXCEPTION_STRING(code, details);
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief execute an AQL query
////////////////////////////////////////////////////////////////////////////////

QueryResult Query::execute () {
  try {
    Parser parser(this);
    parser.parse();

    // put in bind parameters
    parser.ast()->injectBindParameters(_bindParameters);
    // optimize the ast
    parser.ast()->optimize();

    triagens::arango::AqlTransaction<triagens::arango::V8TransactionContext<true>> trx(_vocbase, _collections.collections());

    int res = trx.begin();

    if (res != TRI_ERROR_NO_ERROR) {
      return QueryResult(res, TRI_errno_string(res));
    }

    auto plan = ExecutionPlan::instanciateFromAst(parser.ast());

    triagens::basics::Json json(triagens::basics::Json::List);

    try {
      auto engine = ExecutionEngine::instanciateFromPlan(&trx, plan->root());

      try {
        auto root = engine->root();
        root->execute();

        AqlItemBlock* value;

        while (nullptr != (value = root->getOne())) {
          AqlValue val = value->getValue(0, 0);
          TRI_ASSERT(! val.isEmpty());
          auto doc = value->getDocumentCollection(0);
          json.add(val.toJson(doc));
          delete value;
        }

        delete engine;
      }
      catch (...) {
        delete engine;
        delete plan;
        // TODO: convert exception code
        return QueryResult(TRI_ERROR_INTERNAL);
      }
    }
    catch (triagens::arango::Exception const& ex) {
      delete plan;
      return QueryResult(ex.code(), ex.message());
    }
    catch (...) {
      delete plan;
      // TODO: convert exception code
      return QueryResult(TRI_ERROR_INTERNAL);
    }

    delete plan;
    trx.commit();

    QueryResult result(TRI_ERROR_NO_ERROR);
    //result.json = parser.ast()->toJson(TRI_UNKNOWN_MEM_ZONE);
    result.json = json;
    return result;
  }
  catch (triagens::arango::Exception const& ex) {
    return QueryResult(ex.code(), ex.message());
  }
  catch (...) {
    return QueryResult(TRI_ERROR_OUT_OF_MEMORY, TRI_errno_string(TRI_ERROR_OUT_OF_MEMORY));
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief parse an AQL query
////////////////////////////////////////////////////////////////////////////////

QueryResult Query::parse () {
  try {
    Parser parser(this);
    return parser.parse();
  }
  catch (triagens::arango::Exception const& ex) {
    return QueryResult(ex.code(), ex.message());
  }
  catch (...) {
    return QueryResult(TRI_ERROR_OUT_OF_MEMORY, TRI_errno_string(TRI_ERROR_OUT_OF_MEMORY));
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief explain an AQL query - TODO: implement and determine return type
////////////////////////////////////////////////////////////////////////////////

void Query::explain () {
}

////////////////////////////////////////////////////////////////////////////////
/// @brief get v8 executor
////////////////////////////////////////////////////////////////////////////////

V8Executor* Query::executor () {
  if (_executor == nullptr) {
    // the executor is a singleton per query
    _executor = new V8Executor;
  }

  TRI_ASSERT(_executor != nullptr);
  return _executor;
}

////////////////////////////////////////////////////////////////////////////////
/// @brief register a string
/// the string is freed when the query is destroyed
////////////////////////////////////////////////////////////////////////////////

char* Query::registerString (char const* p,
                             size_t length,
                             bool mustUnescape) {

  if (p == nullptr) {
    THROW_ARANGO_EXCEPTION(TRI_ERROR_OUT_OF_MEMORY);
  }

  if (length == 0) {
    static char const* empty = "";
    // optimisation for the empty string
    return const_cast<char*>(empty);
  }

  char* copy = nullptr;
  if (mustUnescape) {
    size_t outLength;
    copy = TRI_UnescapeUtf8StringZ(TRI_UNKNOWN_MEM_ZONE, p, length, &outLength);
  }
  else {
    copy = TRI_DuplicateString2Z(TRI_UNKNOWN_MEM_ZONE, p, length);
  }

  if (copy == nullptr) {
    THROW_ARANGO_EXCEPTION(TRI_ERROR_OUT_OF_MEMORY);
  }

  try {
    _strings.push_back(copy);
  }
  catch (...) {
    THROW_ARANGO_EXCEPTION(TRI_ERROR_OUT_OF_MEMORY);
  }

  return copy;
}

// -----------------------------------------------------------------------------
// --SECTION--                                                       END-OF-FILE
// -----------------------------------------------------------------------------

// Local Variables:
// mode: outline-minor
// outline-regexp: "/// @brief\\|/// {@inheritDoc}\\|/// @page\\|// --SECTION--\\|/// @\\}"
// End:
<|endoftext|>


<commit_before>// ======================
//    Wesley Miravete
//         2016
// ======================

// Notes:
// Try to use as small of types as you can for optimization purposes
// Avoid operations and conversions when possible
// (Frive == floppy drive)
// Toggling the pin is neccessary because the writing from 0 to 1 to 0 again sometimes overlooks the 1, especially on a crappy frive.

// Todo:

#include <stdint.h>
#include <cstdio>
#include <iostream>
#include <wiringPi.h>
#include <wiringSerial.h>
#include "notes.h"
#include <thread>
#include <signal.h>

// #define STEPFRIVEF stepFrive_sliding // stepFrive_oscillating or stepFrive_sliding

typedef uint8_t byte;

// =========
//  Globals
// =========
// ------------------------------
//  Individual frive information
// ------------------------------
// The array positions are numbered based on position in the Frive array
//byte pins[4][3] = {
	// Dir, step, led
//	{17, 18, 19},
//	{13, 26, 6},
//	{23, 24, 25},
//	{20, 21, 22}
//};
byte pins[4][3] = {
	// Dir, step, led
	{20, 21, 22},
	{23, 24, 25},
	{13, 26, 6},
	{17, 18, 19},
};
// Keep track of amount of frives for loop purposes
byte friveCount = 4;

// 3.5" frives have 80 tracks and 5.25" have 50
// Subtract 8 to add a bit of padding
byte MAX_POSITIONS[]    = {72, 72, 72, 72};
byte currentPositions[] = {0, 0, 0, 0};
byte currentDirection[] = {0, 0, 0, 0};
bool currentVoltage[]   = {0, 0, 0, 0};

unsigned int currentPeriod[] = {0, 0, 0, 0}; // Current period is how long until another step
unsigned int currentTick[]   = {0, 0, 0, 0}; // Counts how long has passed since the frive has been stepped

// Initialize the stepmethod variable (0 = Sliding, 1 = Oscillating)
static bool stepmethod;

// =============
//   Functions
// =============
int setup()
{
	wiringPiSetupGpio();
	for (byte i = 0; i < friveCount; i++)
	{
		pinMode(pins[i][0], OUTPUT); // Direction pin
		pinMode(pins[i][1], OUTPUT); // Step pin
		pinMode(pins[i][2], OUTPUT); // LED pin
	}

	// Starting the serial device and returning file descriptor
	int fd;
	if ((fd = serialOpen("/dev/ttyAMA0", 115200)) < 0)
	{
		std::cout << "Could not open serial device." << std::endl;
		return 1;
	}
	return fd;
}

// NOTE: I've noticed that the sliding method seems to produce notes that are an octave higher than they should be.
// This is probably due to the fact that an octave is just double/half the frequency of the previous/next octave, respectably, which
// could mean that for whatever reason the sliding method produces notes of double frequency

void stepFrive_oscillating(byte frive)
{
	// Switch the direction every time the step pin is at a high voltage
	currentDirection[frive] = currentVoltage[frive] ? !currentDirection[frive] : currentDirection[frive];
	digitalWrite(pins[frive][0], currentDirection[frive]);

	// Toggle the step pin
	currentVoltage[frive] = !currentVoltage[frive];
	digitalWrite(pins[frive][1], currentVoltage[frive]);
}

void stepFrive_sliding(byte frive)
{
	// Switch directions when reaching either end
	if (currentPositions[frive] >= MAX_POSITIONS[frive])
	{
		currentDirection[frive] = 1;
		digitalWrite(pins[frive][0], 1);
	}
	else if (currentPositions[frive] <= 0)
	{
		currentDirection[frive] = 0;
		digitalWrite(pins[frive][0], 0);
	}

	// Toggle the step pin
	currentVoltage[frive] = !currentVoltage[frive];
	digitalWrite(pins[frive][1], currentVoltage[frive]);

	// Keep track of current motor arm position
	if (currentVoltage[frive])
	{
		if (currentDirection[frive] == 1)
		{
			currentPositions[frive] -= 1;
		}
		else
		{
			currentPositions[frive] += 1;
		}
	}
}

void STEPFRIVEF(byte frive)
{
	// Just a standin function in case I ever want to go back to the preprocessor definition
	if (stepmethod) stepFrive_oscillating(frive); else stepFrive_sliding(frive);
}

void tick()
{
	// Get delta time
	static unsigned int time = 0;
	static unsigned int lasttime = 0;
	time = micros();
	unsigned int delta = time-lasttime;
	lasttime = time;

	// Go through each frive and check if it's time to call a stepping function
	// (I don't use a for loop to iterate over each one, because I'm afraid it'll slow it down)
	if (currentPeriod[0]>0)
	{
		currentTick[0] += delta;
		if (currentTick[0] >= currentPeriod[0])
		{
			STEPFRIVEF(0);
			currentTick[0]=0;
		}
	}
	if (currentPeriod[1]>0)
	{
		currentTick[1] += delta;
		if (currentTick[1] >= currentPeriod[1])
		{
			STEPFRIVEF(1);
			currentTick[1]=0;
		}
	}
	if (currentPeriod[2]>0)
	{
		currentTick[2] += delta;
		if (currentTick[2] >= currentPeriod[2])
		{
			STEPFRIVEF(2);
			currentTick[2]=0;
		}
	}
	if (currentPeriod[3]>0)
	{
		currentTick[3] += delta;
		if (currentTick[3] >= currentPeriod[3])
		{
			STEPFRIVEF(3);
			currentTick[3]=0;
		}
	}
	delayMicroseconds(5); // Prevent the loop from going too fast and giving itself a bad time
}

void resetAll(bool method)
{
	for (byte i = 0; i < friveCount; i++)
	{
		currentPeriod[i] = 0; // Stop playing any notes

		digitalWrite(pins[i][0], 0);
		for (int pos=0; pos<MAX_POSITIONS[i]; pos++) // MAX_POSITIONS[i] is divided by two because this isn't toggling
		{
			digitalWrite(pins[i][1], 1);
			delay(1);
			digitalWrite(pins[i][1], 0);
			delay(1);
		}

		digitalWrite(pins[i][0], 1);
		for (int pos=0; pos<(method ? MAX_POSITIONS[i]/2 : MAX_POSITIONS[i]); pos++) // Stop halfway if using the oscillating method
		{
			digitalWrite(pins[i][1], 1);
			delay(1);
			digitalWrite(pins[i][1], 0);
			delay(1);
		}
		digitalWrite(pins[i][0], 0);
	}
}

void serialLoop(int fd, unsigned int currentPeriod[])
{
	signed char data;
	byte note;
	byte frive;
	while(1)
	{
		// Read the serial pins hooked up to the other pi to set the notes
		data = serialGetchar(fd);
		if (data==-1) continue; // timeout catcher

		// Get bits out of received char
		note = (data >> 3) & 0b00011111;
		frive = data & 0b00000111;
		if (note < 32 && frive < 8) // Make sure recieved values are in range
			currentPeriod[frive] = notes[note];

		digitalWrite(pins[frive][2], int(note!=0));

		//std::cout << "Note: " << +byte((data >> 3) & 0b00011111) << std::endl;
		//std::cout << "Frive: " << +byte(data & 0b00000111) << std::endl;
	}
}

void onExit(int s)
{
	std::cout << "Resetting pins to 0... ";
	for (byte i = 0; i < friveCount; i++)
	{
		digitalWrite(pins[i][0], 0);
		digitalWrite(pins[i][1], 0);
		digitalWrite(pins[i][2], 0);
	}
	std::cout << "Done." << std::endl;
	exit(1);
}

int main(int argc, char *argv[])
{
	if (argc != 2) // First argument is the program name, never forgetti
	{
		std::cout << "Invalid number of arguments." << std::endl;
		return 1;
	}
	stepmethod = argv[1][0]-48; // Get first char of input (only one char should be the input) then subtract 48 from the ascii number

	std::cout << "Binding exit cleanup function... " << std::flush;
	signal(SIGINT, onExit);
	delay(500);
	std::cout << "Done." << std::endl;

	std::cout << "Setting up... " << std::flush;
	int fd = setup();
	delay(500);
	std::cout << "Done." << std::endl;

	std::cout << "Resetting frives... " << std::flush;
	resetAll(0); // For now just always reset it fully because it seems to make a louder noise when oscillating
	delay(500);
	std::cout << "Done." << std::endl;

	std::cout << "Starting serial thread loop... " << std::flush;
	std::thread sl(serialLoop, fd, std::ref(currentPeriod));
	delay(500);
	std::cout << "Done." << std::endl;

	std::cout << "Ready to begin." << std::endl;

	while(1)
	{
		tick();
	}

	return 0;
}
<commit_msg>Cleaned up a bit<commit_after>// ======================
//    Wesley Miravete
//         2016
// ======================

// Notes:
// Try to use as small of types as you can for optimization purposes
// Avoid operations and conversions when possible
// (Frive == floppy drive)
// Toggling the pin is neccessary because the writing from 0 to 1 to 0 again sometimes overlooks the 1, especially on a crappy frive.

#include <stdint.h>
#include <cstdio>
#include <iostream>
#include <wiringPi.h>
#include <wiringSerial.h>
#include "notes.h"
#include <thread>
#include <signal.h>

// #define STEPFRIVEF stepFrive_sliding // stepFrive_oscillating or stepFrive_sliding

typedef uint8_t byte;

// =========
//  Globals
// =========
// ------------------------------
//  Individual frive information
// ------------------------------
byte pins[4][3] = {
	// Dir, step, led
	{20, 21, 22},
	{23, 24, 25},
	{13, 26, 6},
	{17, 18, 19},
};
// Keep track of amount of frives for loop purposes
byte friveCount = 4;

// 3.5" frives have 80 tracks and 5.25" have 50
// Subtract 8 to add a bit of padding
byte MAX_POSITIONS[]    = {72, 72, 72, 72}; // Constants
byte currentPositions[] = {0, 0, 0, 0};
byte currentDirection[] = {0, 0, 0, 0};
bool currentVoltage[]   = {0, 0, 0, 0};

unsigned int currentPeriod[] = {0, 0, 0, 0}; // Current period is how long until another step
unsigned int currentTick[]   = {0, 0, 0, 0}; // Counts how long has passed since the frive has been stepped

// Initialize the stepmethod variable (0 = Sliding, 1 = Oscillating)
static bool stepmethod;

// =============
//   Functions
// =============
int setup()
{
	wiringPiSetupGpio();
	for (byte i = 0; i < friveCount; i++)
	{
		pinMode(pins[i][0], OUTPUT); // Direction pin
		pinMode(pins[i][1], OUTPUT); // Step pin
		pinMode(pins[i][2], OUTPUT); // LED pin
	}

	// Starting the serial device and returning file descriptor
	int fd;
	if ((fd = serialOpen("/dev/ttyAMA0", 115200)) < 0)
	{
		std::cout << "Could not open serial device." << std::endl;
		return 1;
	}
	return fd;
}

// NOTE: I've noticed that the sliding method seems to produce notes that are an octave higher than they should be.
// This is probably due to the fact that an octave is just double/half the frequency of the previous/next octave, respectably, which
// could mean that for whatever reason the sliding method produces notes of double frequency

void stepFrive_oscillating(byte frive)
{
	// Switch the direction every time the step pin is at a high voltage
	currentDirection[frive] = currentVoltage[frive] ? !currentDirection[frive] : currentDirection[frive];
	digitalWrite(pins[frive][0], currentDirection[frive]);

	// Toggle the step pin
	currentVoltage[frive] = !currentVoltage[frive];
	digitalWrite(pins[frive][1], currentVoltage[frive]);
}

void stepFrive_sliding(byte frive)
{
	// Switch directions when reaching either end
	if (currentPositions[frive] >= MAX_POSITIONS[frive])
	{
		currentDirection[frive] = 1;
		digitalWrite(pins[frive][0], 1);
	}
	else if (currentPositions[frive] <= 0)
	{
		currentDirection[frive] = 0;
		digitalWrite(pins[frive][0], 0);
	}

	// Toggle the step pin
	currentVoltage[frive] = !currentVoltage[frive];
	digitalWrite(pins[frive][1], currentVoltage[frive]);

	// Keep track of current motor arm position
	if (currentVoltage[frive])
	{
		if (currentDirection[frive] == 1)
		{
			currentPositions[frive] -= 1;
		}
		else
		{
			currentPositions[frive] += 1;
		}
	}
}

void STEPFRIVEF(byte frive)
{
	// Just a standin function in case I ever want to go back to the preprocessor definition
	if (stepmethod) stepFrive_oscillating(frive); else stepFrive_sliding(frive);
}

void tick()
{
	// Get delta time
	static unsigned int time = 0;
	static unsigned int lasttime = 0;
	time = micros();
	unsigned int delta = time-lasttime;
	lasttime = time;

	// Go through each frive and check if it's time to call a stepping function
	// (I don't use a for loop to iterate over each one, because I'm afraid it'll slow it down)
	if (currentPeriod[0]>0)
	{
		currentTick[0] += delta;
		if (currentTick[0] >= currentPeriod[0])
		{
			STEPFRIVEF(0);
			currentTick[0]=0;
		}
	}
	if (currentPeriod[1]>0)
	{
		currentTick[1] += delta;
		if (currentTick[1] >= currentPeriod[1])
		{
			STEPFRIVEF(1);
			currentTick[1]=0;
		}
	}
	if (currentPeriod[2]>0)
	{
		currentTick[2] += delta;
		if (currentTick[2] >= currentPeriod[2])
		{
			STEPFRIVEF(2);
			currentTick[2]=0;
		}
	}
	if (currentPeriod[3]>0)
	{
		currentTick[3] += delta;
		if (currentTick[3] >= currentPeriod[3])
		{
			STEPFRIVEF(3);
			currentTick[3]=0;
		}
	}
	delayMicroseconds(5); // Prevent the loop from going too fast and giving itself a bad time
}

void resetAll(bool method)
{
	for (byte i = 0; i < friveCount; i++)
	{
		currentPeriod[i] = 0; // Stop playing any notes

		digitalWrite(pins[i][0], 0);
		for (int pos=0; pos<MAX_POSITIONS[i]; pos++) // MAX_POSITIONS[i] is divided by two because this isn't toggling
		{
			digitalWrite(pins[i][1], 1);
			delay(1);
			digitalWrite(pins[i][1], 0);
			delay(1);
		}

		digitalWrite(pins[i][0], 1);
		for (int pos=0; pos<(method ? MAX_POSITIONS[i]/2 : MAX_POSITIONS[i]); pos++) // Stop halfway if using the oscillating method
		{
			digitalWrite(pins[i][1], 1);
			delay(1);
			digitalWrite(pins[i][1], 0);
			delay(1);
		}
		digitalWrite(pins[i][0], 0);
	}
}

void serialLoop(int fd, unsigned int currentPeriod[])
{
	signed char data;
	byte note;
	byte frive;
	while(1)
	{
		// Read the serial pins hooked up to the other pi to set the notes
		data = serialGetchar(fd);
		if (data==-1) continue; // timeout catcher

		// Get bits out of received char
		note = (data >> 3) & 0b00011111;
		frive = data & 0b00000111;
		if (note < 32 && frive < 8) // Make sure recieved values are in range
			currentPeriod[frive] = notes[note];

		digitalWrite(pins[frive][2], int(note!=0));
	}
}

void onExit(int s)
{
	std::cout << "Resetting pins to 0... ";
	for (byte i = 0; i < friveCount; i++)
	{
		digitalWrite(pins[i][0], 0);
		digitalWrite(pins[i][1], 0);
		digitalWrite(pins[i][2], 0);
	}
	std::cout << "Done." << std::endl;
	exit(1);
}

int main(int argc, char *argv[])
{
	if (argc != 2) // First argument is the program name, never forgetti
	{
		std::cout << "Invalid number of arguments." << std::endl;
		return 1;
	}
	stepmethod = argv[1][0]-48; // Get first char of input (only one char should be the input) then subtract 48 from the ascii number

	std::cout << "Binding exit cleanup function... " << std::flush;
	signal(SIGINT, onExit);
	delay(500);
	std::cout << "Done." << std::endl;

	std::cout << "Setting up... " << std::flush;
	int fd = setup();
	delay(500);
	std::cout << "Done." << std::endl;

	std::cout << "Resetting frives... " << std::flush;
	resetAll(0); // For now just always reset it fully because it seems to make a louder noise when oscillating
	delay(500);
	std::cout << "Done." << std::endl;

	std::cout << "Starting serial thread loop... " << std::flush;
	std::thread sl(serialLoop, fd, std::ref(currentPeriod));
	delay(500);
	std::cout << "Done." << std::endl;

	std::cout << "Ready to begin." << std::endl;

	while(1)
	{
		tick();
	}

	return 0;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <string>
#include <sstream>

#include <boost/thread.hpp>
#include <boost/bind.hpp>

#include <Eigen/Eigen>

#include <ros/ros.h>
#include <tms_ss_kinect_v2/SkeletonArray.h>
#include <tms_ss_kinect_v2/SkeletonStreamWrapper.h>

#include "../calc_joint_angles/for_model01.h"

#include <unistd.h>

#define MAX_USERS 12

//-----------------------------------------------------------------------------
template <class T>
std::string to_str(const T& t)
{
  std::stringstream ss;
  ss << t;
  return ss.str();
}

//-----------------------------------------------------------------------------
inline tms_ss_kinect_v2::Skeleton initialize_skeleton()
{
  tms_ss_kinect_v2::Skeleton ret;
  ret.user_id = -1;
  ret.position.resize(25);
  ret.orientation.resize(25);
  ret.confidence.resize(25);
  for(int i=0; i<25; i++)
  {
    ret.position[i].x = 0.0;
    ret.position[i].y = 0.0;
    ret.position[i].z = 0.0;
    ret.orientation[i].w = 1.0;
    ret.orientation[i].x = 0.0;
    ret.orientation[i].y = 0.0;
    ret.orientation[i].z = 0.0;
    ret.confidence[i] = 0;
  }
  return ret;
}

//-----------------------------------------------------------------------------
class SkeletonIntegrator
{
  public:
    SkeletonIntegrator(const std::vector<int>& camera);
    ~SkeletonIntegrator();

    void callback(const tms_ss_kinect_v2::SkeletonStreamWrapper::ConstPtr& msg);
    void run();
  private:
    ros::NodeHandle nh;
    std::vector<int> array;
    tms_ss_kinect_v2::SkeletonArray skeletons;
    int new_user_;
    int tracked_skeleton_num_;
    int tracking_validity[MAX_USERS];

    void listenSkeletonStream();
};

//-----------------------------------------------------------------------------
SkeletonIntegrator::SkeletonIntegrator(const std::vector<int> &camera) :
  array(camera),
  new_user_(0),
  tracked_skeleton_num_(0)
{
  skeletons.data.resize(MAX_USERS);
  for (int i = 0; i < MAX_USERS; i++)
  {
    skeletons.data[i] = initialize_skeleton();
    tracking_validity[i] = 0;
  }
  return;
}

//-----------------------------------------------------------------------------
SkeletonIntegrator::~SkeletonIntegrator()
{
  return;
}

//-----------------------------------------------------------------------------
void SkeletonIntegrator::callback(const tms_ss_kinect_v2::SkeletonStreamWrapper::ConstPtr& msg)
{
  ROS_INFO("Received skeleton from camera %d", msg->camera_number);

  tms_ss_kinect_v2::Skeleton skeleton = msg->skeleton;
  tms_ss_kinect_v2::CameraPosture camera_posture = msg->camera_posture;

  // Transform to world coordinate
  Eigen::Vector3f translation(
      camera_posture.translation.x,
      camera_posture.translation.y,
      camera_posture.translation.z);
  Eigen::Quaternionf rotation(
      camera_posture.rotation.w,
      camera_posture.rotation.x,
      camera_posture.rotation.y,
      camera_posture.rotation.z);

  tms_ss_kinect_v2::Skeleton integrated_skeleton;
  integrated_skeleton.user_id = 0;
  integrated_skeleton.position.resize(25);
  integrated_skeleton.orientation.resize(25);
  integrated_skeleton.confidence.resize(25);
  for(int i=0; i<25; i++)
  {
    Eigen::Vector3f pos(
        skeleton.position[i].x,
        skeleton.position[i].y,
        skeleton.position[i].z);
    Eigen::Quaternionf ori(
        skeleton.orientation[i].w,
        skeleton.orientation[i].x,
        skeleton.orientation[i].y,
        skeleton.orientation[i].z);
    pos = rotation.matrix() * pos + translation;
    ori = rotation * ori;
    integrated_skeleton.position[i].x = pos[0];
    integrated_skeleton.position[i].y = pos[1];
    integrated_skeleton.position[i].z = pos[2];
    integrated_skeleton.orientation[i].w = ori.w();
    integrated_skeleton.orientation[i].x = ori.x();
    integrated_skeleton.orientation[i].y = ori.y();
    integrated_skeleton.orientation[i].z = ori.z();
    integrated_skeleton.confidence[i] = skeleton.confidence[i];
  }

  // identify received skeleton
  bool already_detect = false;
  for (int i = 0; i < skeletons.data.size(); i++)
  {
    Eigen::Vector3f v1(
        integrated_skeleton.position[SpineMid].x,
        integrated_skeleton.position[SpineMid].y,
        integrated_skeleton.position[SpineMid].z);
    Eigen::Vector3f v2(
        skeletons.data[i].position[SpineMid].x,
        skeletons.data[i].position[SpineMid].y,
        skeletons.data[i].position[SpineMid].z);
    if ((v1-v2).norm() < 0.3)
    {
      int user_id = skeletons.data[i].user_id;
      integrated_skeleton.user_id =  user_id;
      skeletons.data[i] = integrated_skeleton;
      tracking_validity[i] = 10;  // set validity count
      already_detect = true;
    }
    std::cout << (v1-v2).norm() << std::endl;
  }
  if (!already_detect)
  {
    integrated_skeleton.user_id = tracked_skeleton_num_;
    skeletons.data[new_user_] = integrated_skeleton;
    tracking_validity[new_user_] = 10;  // set validity count
    new_user_ = (new_user_ + 1) % MAX_USERS;
    tracked_skeleton_num_++;
  }

  // eliminate invalid skeletons
  for (int i = 0; i < MAX_USERS; i++)
  {
    if (tracking_validity[i] == 0)
    {
      skeletons.data[i] = initialize_skeleton();
    }
  }

  return;
}

//-----------------------------------------------------------------------------
void SkeletonIntegrator::run()
{
  ros::Rate loop_late(10);

  static boost::thread thread_listener(
      boost::bind(&SkeletonIntegrator::listenSkeletonStream, this));

  ros::Publisher pub =
    nh.advertise<tms_ss_kinect_v2::SkeletonArray>(
        "integrated_skeleton_stream", 1);

  while (ros::ok())
  {
    pub.publish(skeletons);
    for (int i = 0; i<MAX_USERS; i++)
    {
      if (tracking_validity[i] > 0)
      {
        tracking_validity[i]--;
      }
    }
    ros::spinOnce();
    loop_late.sleep();
  }

  return;
}

//-----------------------------------------------------------------------------
void SkeletonIntegrator::listenSkeletonStream()
{
  ros::Rate loop_late(10);
  std::vector<ros::Subscriber> sub(array.size());

  std::cout << "Subscribe ===" << std::endl;
  for (int i=0; i < array[i]; i++)
  {
    std::string topic_name("skeleton_stream_wrapper");
    sub[i] =nh.subscribe(topic_name.append(to_str<int>(array[i])),
        1,&SkeletonIntegrator::callback, this);
    std::cout << topic_name << std::endl;
  }

  ros::spin();

  return;
}

//-----------------------------------------------------------------------------
int main(int argc, char **argv)
{
  int option;
  bool got_option = false;
  std::vector<int> cameraID_array;
  while ((option = getopt(argc,argv,"hn:"))!=-1)
  {
    switch(option)
    {
    case 'h':
      std::cout << "-- Usage" << std::endl <<
        std::endl;
      return 0;
      break;
    case 'n':
      for (int i = 0; i < atoi(optarg); i++)
      {
        cameraID_array.push_back(i+1);
      }
      got_option = true;
      break;
    case ':':
      std::cerr << "Need some values" << std::endl;
      break;
    case '?':
      std::cerr << "unknown option" << std::endl;
      break;
    }
  }
  if (!got_option)
  {
    for (int i = 1; i < argc; i++)
    {
      cameraID_array.push_back(atoi(argv[i]));
    }
  }

  int camera_num = cameraID_array.size();
  if (camera_num <= 0 || camera_num >= 7)
  {
    std::cout << "Maybe given invalid input." << std::endl;
    return -2;
  }
  ros::init(argc, argv, "skeleton_integrator");

  SkeletonIntegrator integrator(cameraID_array);

  integrator.run();

  return 0;
}
<commit_msg>tms_ss_kinect_v2: fix some bugs<commit_after>#include <iostream>
#include <string>
#include <sstream>
#include <iomanip>

#include <boost/thread.hpp>
#include <boost/bind.hpp>

#include <Eigen/Eigen>

#include <ros/ros.h>
#include <tms_ss_kinect_v2/SkeletonArray.h>
#include <tms_ss_kinect_v2/SkeletonStreamWrapper.h>

#include "../calc_joint_angles/for_model01.h"

#include <unistd.h>

#define MAX_USERS 12

//-----------------------------------------------------------------------------
template <class T>
std::string to_str(const T& t)
{
  std::stringstream ss;
  ss << t;
  return ss.str();
}

//-----------------------------------------------------------------------------
inline tms_ss_kinect_v2::Skeleton initialize_skeleton()
{
  tms_ss_kinect_v2::Skeleton ret;
  ret.user_id = -1;
  ret.position.resize(25);
  ret.orientation.resize(25);
  ret.confidence.resize(25);
  for(int i=0; i<25; i++)
  {
    ret.position[i].x = 0.0;
    ret.position[i].y = 0.0;
    ret.position[i].z = 0.0;
    ret.orientation[i].w = 1.0;
    ret.orientation[i].x = 0.0;
    ret.orientation[i].y = 0.0;
    ret.orientation[i].z = 0.0;
    ret.confidence[i] = 0;
  }
  return ret;
}

//-----------------------------------------------------------------------------
class SkeletonIntegrator
{
  public:
    SkeletonIntegrator(const std::vector<int>& camera);
    ~SkeletonIntegrator();

    void callback(const tms_ss_kinect_v2::SkeletonStreamWrapper::ConstPtr& msg);
    void run();
  private:
    ros::NodeHandle nh;
    std::vector<int> array;
    tms_ss_kinect_v2::SkeletonArray skeletons;
    int new_user_;
    int tracked_skeleton_num_;
    int tracking_validity[MAX_USERS];

    void listenSkeletonStream();

    // For Debug
    void showStatus();
};

//-----------------------------------------------------------------------------
SkeletonIntegrator::SkeletonIntegrator(const std::vector<int> &camera) :
  array(camera),
  new_user_(0),
  tracked_skeleton_num_(0)
{
  skeletons.data.resize(MAX_USERS);
  for (int i = 0; i < MAX_USERS; i++)
  {
    skeletons.data[i] = initialize_skeleton();
    tracking_validity[i] = 0;
  }
  return;
}

//-----------------------------------------------------------------------------
SkeletonIntegrator::~SkeletonIntegrator()
{
  return;
}

//-----------------------------------------------------------------------------
void SkeletonIntegrator::callback(const tms_ss_kinect_v2::SkeletonStreamWrapper::ConstPtr& msg)
{
  ROS_INFO("Received skeleton from camera %d", msg->camera_number);

  tms_ss_kinect_v2::Skeleton skeleton = msg->skeleton;
  tms_ss_kinect_v2::CameraPosture camera_posture = msg->camera_posture;

  // Transform to world coordinate
  Eigen::Vector3f translation(
      camera_posture.translation.x,
      camera_posture.translation.y,
      camera_posture.translation.z);
  Eigen::Quaternionf rotation(
      camera_posture.rotation.w,
      camera_posture.rotation.x,
      camera_posture.rotation.y,
      camera_posture.rotation.z);

  tms_ss_kinect_v2::Skeleton integrated_skeleton;
  integrated_skeleton.user_id = 0;
  integrated_skeleton.position.resize(25);
  integrated_skeleton.orientation.resize(25);
  integrated_skeleton.confidence.resize(25);
  for(int i=0; i<25; i++)
  {
    Eigen::Vector3f pos(
        skeleton.position[i].x,
        skeleton.position[i].y,
        skeleton.position[i].z);
    Eigen::Quaternionf ori(
        skeleton.orientation[i].w,
        skeleton.orientation[i].x,
        skeleton.orientation[i].y,
        skeleton.orientation[i].z);
    pos = rotation.matrix() * pos + translation;
    ori = rotation * ori;
    integrated_skeleton.position[i].x = pos[0];
    integrated_skeleton.position[i].y = pos[1];
    integrated_skeleton.position[i].z = pos[2];
    integrated_skeleton.orientation[i].w = ori.w();
    integrated_skeleton.orientation[i].x = ori.x();
    integrated_skeleton.orientation[i].y = ori.y();
    integrated_skeleton.orientation[i].z = ori.z();
    integrated_skeleton.confidence[i] = skeleton.confidence[i];
  }

  // identify received skeleton
  bool already_detect = false;
  for (int i = 0; i < skeletons.data.size(); i++)
  {
    Eigen::Vector3f v1(
        integrated_skeleton.position[SpineMid].x,
        integrated_skeleton.position[SpineMid].y,
        integrated_skeleton.position[SpineMid].z);
    Eigen::Vector3f v2(
        skeletons.data[i].position[SpineMid].x,
        skeletons.data[i].position[SpineMid].y,
        skeletons.data[i].position[SpineMid].z);
    if ((v1-v2).norm() < 0.3)
    {
      int user_id = skeletons.data[i].user_id;
      integrated_skeleton.user_id =  user_id;
      skeletons.data[i] = integrated_skeleton;
      tracking_validity[i] = 10;  // set validity count
      already_detect = true;
      break;
    }
  }
  if (!already_detect)
  {
    integrated_skeleton.user_id = tracked_skeleton_num_;
    skeletons.data[new_user_] = integrated_skeleton;
    tracking_validity[new_user_] = 10;  // set validity count
    for (int i=0; i < MAX_USERS && skeletons.data[(new_user_+i)%MAX_USERS].user_id >= 0; i++)
    {
      new_user_ = (new_user_ + i+1) % MAX_USERS;
    }
    tracked_skeleton_num_++;
  }

  return;
}

//-----------------------------------------------------------------------------
void SkeletonIntegrator::run()
{
  ros::Rate loop_late(10);

  static boost::thread thread_listener(
      boost::bind(&SkeletonIntegrator::listenSkeletonStream, this));

  ros::Publisher pub =
    nh.advertise<tms_ss_kinect_v2::SkeletonArray>(
        "integrated_skeleton_stream", 1);

  while (ros::ok())
  {
    pub.publish(skeletons);
    for (int i = 0; i<MAX_USERS; i++)
    {
      // decrease validity of skeleton
      if (tracking_validity[i] > 0)
      {
        tracking_validity[i]--;
      }

      // eliminate invalid skeletons
      if (tracking_validity[i] == 0)
      {
        skeletons.data[i] = initialize_skeleton();
      }
    }

    showStatus();
    ros::spinOnce();
    loop_late.sleep();
  }

  return;
}

//-----------------------------------------------------------------------------
void SkeletonIntegrator::listenSkeletonStream()
{
  ros::Rate loop_late(10);
  std::vector<ros::Subscriber> sub(array.size());

  std::cout << "Subscribe ===" << std::endl;
  for (int i=0; i < array[i]; i++)
  {
    std::string topic_name("skeleton_stream_wrapper");
    sub[i] =nh.subscribe(topic_name.append(to_str<int>(array[i])),
        1,&SkeletonIntegrator::callback, this);
    std::cout << topic_name << std::endl;
  }

  ros::spin();

  return;
}

//-----------------------------------------------------------------------------
void SkeletonIntegrator::showStatus()
{
  std::stringstream ss;
  ss.fill(' ');

  ss << "New user: " << new_user_ << std::endl << std::endl;

  ss << "User ID" << std::endl;
  for (int i = 0; i < MAX_USERS; i++) { ss << "|" << std::setw(5) << i; }
  ss << "|" << std::endl;
  for (int i = 0; i < MAX_USERS; i++) { ss << "|" << std::setw(5) << skeletons.data[i].user_id; }
  ss << "|" << std::endl << std::endl;;

  ss << "Validity table" << std::endl;
  for (int i = 0; i < MAX_USERS; i++) { ss << "|" << std::setw(2) << i; }
  ss << "|" << std::endl;
  for (int i = 0; i < MAX_USERS; i++) { ss << "|" << std::setw(2) << tracking_validity[i]; }
  ss << "|" << std::endl;

  ROS_INFO("%s", ss.str().c_str());
  return;
}

//-----------------------------------------------------------------------------
int main(int argc, char **argv)
{
  int option;
  bool got_option = false;
  std::vector<int> cameraID_array;
  while ((option = getopt(argc,argv,"hn:"))!=-1)
  {
    switch(option)
    {
    case 'h':
      std::cout << "-- Usage" << std::endl <<
        std::endl;
      return 0;
      break;
    case 'n':
      for (int i = 0; i < atoi(optarg); i++)
      {
        cameraID_array.push_back(i+1);
      }
      got_option = true;
      break;
    case ':':
      std::cerr << "Need some values" << std::endl;
      break;
    case '?':
      std::cerr << "unknown option" << std::endl;
      break;
    }
  }
  if (!got_option)
  {
    for (int i = 1; i < argc; i++)
    {
      cameraID_array.push_back(atoi(argv[i]));
    }
  }

  int camera_num = cameraID_array.size();
  if (camera_num <= 0 || camera_num >= 7)
  {
    std::cout << "Maybe given invalid input." << std::endl;
    return -2;
  }
  ros::init(argc, argv, "skeleton_integrator");

  SkeletonIntegrator integrator(cameraID_array);

  integrator.run();

  return 0;
}
<|endoftext|>


<commit_before>#ifndef BULL_EULERANGLES_HPP
#define BULL_EULERANGLES_HPP

#include <Bull/Math/Angle.hpp>

namespace Bull
{
    template <typename T>
    struct EulerAngles
    {
        /*! \brief Default constructor
         *
         */
        EulerAngles();

        /*! \brief Constructor
         *
         * @param roll  The roll of the angle
         * @param pitch the pitch of the angle
         * @param yaw   the yaw of the angle
         *
         */
        EulerAngles(const Angle<T>& roll, const Angle<T>& pitch, const Angle<T>& yaw);

        /*! \brief Set the angle
         *
         * @param copy The EulerAngles to copy
         *
         * @return This
         *
         */
        EulerAngles<T>& set(const EulerAngles<T>& copy);

        /*! \brief Set the angle
         *
         * @param roll  The roll of the angle
         * @param pitch the pitch of the angle
         * @param yaw   the yaw of the angle
         *
         * @return This
         *
         */
        EulerAngles<T>& set(const Angle<T>& roll, const Angle<T>& pitch, const Angle<T>& yaw);

        /*! \brief Normalize the angle of each rotation
         *
         * @return This
         *
         */
        EulerAngles<T>& normalize();

        /*! \brief Compare two EulerAngles
         *
         * @param right The EulerAngles to compare
         *
         * @return True if right and this are equal
         *
         */
        bool operator==(const EulerAngles<T>& right);

        /*! \brief Compare two EulerAngles
         *
         * @param right The EulerAngles to compare
         *
         * @return True if right and this are not equal
         *
         */
        bool operator!=(const EulerAngles<T>& right);

        Angle<T> roll, pitch, yaw;
    };

    typedef EulerAngles<int> EulerAnglesI;
    typedef EulerAngles<float> EulerAnglesF;
    typedef EulerAngles<double> EulerAnglesD;
    typedef EulerAngles<unsigned int> EulerAnglesUI;
}

#include <Bull/Math/EulerAngles.inl>

#endif //BULL_EULERANGLES_HPP
<commit_msg>Math/EulerAngles Improve documentation<commit_after>#ifndef BULL_EULERANGLES_HPP
#define BULL_EULERANGLES_HPP

#include <Bull/Math/Angle.hpp>

namespace Bull
{
    /*! \class EulerAngles
     * \brief Implement 3D rotation with EulerAngles
     *
     * @tparam T The type of angles values
     */
    template <typename T>
    struct EulerAngles
    {
        /*! \brief Default constructor
         *
         */
        EulerAngles();

        /*! \brief Constructor
         *
         * @param roll  The roll of the angle
         * @param pitch the pitch of the angle
         * @param yaw   the yaw of the angle
         *
         */
        EulerAngles(const Angle<T>& roll, const Angle<T>& pitch, const Angle<T>& yaw);

        /*! \brief Set the angle
         *
         * @param copy The EulerAngles to copy
         *
         * @return This
         *
         */
        EulerAngles<T>& set(const EulerAngles<T>& copy);

        /*! \brief Set the angle
         *
         * @param roll  The roll of the angle
         * @param pitch the pitch of the angle
         * @param yaw   the yaw of the angle
         *
         * @return This
         *
         */
        EulerAngles<T>& set(const Angle<T>& roll, const Angle<T>& pitch, const Angle<T>& yaw);

        /*! \brief Normalize the angle of each rotation
         *
         * @return This
         *
         */
        EulerAngles<T>& normalize();

        /*! \brief Compare two EulerAngles
         *
         * @param right The EulerAngles to compare
         *
         * @return True if right and this are equal
         *
         */
        bool operator==(const EulerAngles<T>& right);

        /*! \brief Compare two EulerAngles
         *
         * @param right The EulerAngles to compare
         *
         * @return True if right and this are not equal
         *
         */
        bool operator!=(const EulerAngles<T>& right);

        Angle<T> roll;  /*!< The rotation around the X axis */
        Angle<T> pitch; /*!< The rotation around the Y axis */
        Angle<T> yaw;   /*!< The rotation around the Z axis */
    };

    typedef EulerAngles<int> EulerAnglesI;
    typedef EulerAngles<float> EulerAnglesF;
    typedef EulerAngles<double> EulerAnglesD;
    typedef EulerAngles<unsigned int> EulerAnglesUI;
}

#include <Bull/Math/EulerAngles.inl>

#endif //BULL_EULERANGLES_HPP
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <std_msgs/String.h>
#include <string>
#include "serialDriver.c"
#include <sstream>      // std::stringstream

class serialListener{
	public:
	ros::NodeHandle handle;
	ros::Publisher arduino;
	std_msgs::String serialmsg;
	std::stringstream read;

	int checklastln(const char* toCheck , int maxLength){
		for(int i=0;i<maxLength;i++){
			if(toCheck[i]==10){
				return 1;
			}
			if(toCheck[i]==0){
				return 0;
			}
		}
	}
	int validatemsg(std::string msg){
<<<<<<< HEAD
		ROS_INFO("SerialListener: validatemsg: %c", msg[0]);
		if(msg[0]=='c'||msg[0]=='w')
=======
		if(msg[0]=='c'||msg[0]=='w'){
			ROS_INFO("SerialListener: Valid message received");
>>>>>>> 570983e3ce93bf9a4a32ee15ba877617ac291a58
			return 1;
		}
		ROS_INFO("SerialListener: Invalid message received");
		return 0;
	}
	void writeSerial(std::string shit){
		ROS_INFO("SerialListener: Writing to serial line");
		std::stringstream sysCall;
	        sysCall<<"/home/ubuntu/robotica-minor-5/com/arduino-serial/arduino-serial --port=/dev/ttyACM0 --send="<<shit;
		std::string temp= sysCall.str();
		ROS_INFO("serialListener: syscall: %s", temp.c_str());
		system(temp.c_str());
	}

	void poll(){
		ROS_INFO("polling");
		read<<(char*)readline();

		if(read.str()[0]!=0&&read.str()[0]!=10){
			serialmsg.data=read.str();
			ROS_INFO("SerialListener:received data: %s" , read.str().c_str());
			if(checklastln(read.str().c_str(),255)){
				if(validatemsg(read.str())){
					writeSerial("conf");
					arduino.publish(serialmsg);
					read.str("");
				}
			}
		}
	}

	serialListener():handle("~"){
		arduino=handle.advertise<std_msgs::String>("/tawi/arduino/serial",100);
	}
};

int main (int argc, char **argv){
	ros::init(argc, argv, "serialListener");
	serialListener serial;
	ros::Rate hz(5);
	while(ros::ok()){
		serial.poll();
		ros::spinOnce();
		hz.sleep();
	}
}
<commit_msg>Conflict resolved<commit_after>#include <ros/ros.h>
#include <std_msgs/String.h>
#include <string>
#include "serialDriver.c"
#include <sstream>      // std::stringstream

class serialListener{
	public:
	ros::NodeHandle handle;
	ros::Publisher arduino;
	std_msgs::String serialmsg;
	std::stringstream read;

	int checklastln(const char* toCheck , int maxLength){
		for(int i=0;i<maxLength;i++){
			if(toCheck[i]==10){
				return 1;
			}
			if(toCheck[i]==0){
				return 0;
			}
		}
	}
	int validatemsg(std::string msg){
		ROS_INFO("SerialListener: validatemsg: %c", msg[0]);
		if(msg[0]=='c'||msg[0]=='w'){
			return 1;
		}
		ROS_INFO("SerialListener: Invalid message received");
		return 0;
	}

	void writeSerial(std::string shit){
		ROS_INFO("SerialListener: Writing to serial line");
		std::stringstream sysCall;
	        sysCall<<"/home/ubuntu/robotica-minor-5/com/arduino-serial/arduino-serial --port=/dev/ttyACM0 --send="<<shit;
		std::string temp= sysCall.str();
		ROS_INFO("serialListener: syscall: %s", temp.c_str());
		system(temp.c_str());
	}

	void poll(){
		ROS_INFO("polling");
		read<<(char*)readline();

		if(read.str()[0]!=0&&read.str()[0]!=10){
			serialmsg.data=read.str();
			ROS_INFO("SerialListener:received data: %s" , read.str().c_str());
			if(checklastln(read.str().c_str(),255)){
				if(validatemsg(read.str())){
					writeSerial("conf");
					arduino.publish(serialmsg);
					read.str("");
				}
			}
		}
	}

	serialListener():handle("~"){
		arduino=handle.advertise<std_msgs::String>("/tawi/arduino/serial",100);
	}
};

int main (int argc, char **argv){
	ros::init(argc, argv, "serialListener");
	serialListener serial;
	ros::Rate hz(5);
	while(ros::ok()){
		serial.poll();
		ros::spinOnce();
		hz.sleep();
	}
}
<|endoftext|>


<commit_before>#include <sys/resource.h>
#include <sys/time.h>

#include <fstream>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include "HyperscanEngine.h"
#include "PCRE2Engine.h"
#include "PcapSource.h"
#include "RE2Engine.h"
#include "REmatchEngine.h"
#include "Rule.h"
#include "regexbench.h"

using boost::property_tree::ptree;
using boost::property_tree::read_json;
using boost::property_tree::write_json;

namespace po = boost::program_options;

enum EngineType : uint64_t {
  ENGINE_HYPERSCAN,
  ENGINE_PCRE2,
  ENGINE_PCRE2JIT,
  ENGINE_RE2,
  ENGINE_REMATCH
};

struct Arguments {
  std::string output_file;
  std::string pcap_file;
  std::string rule_file;
  EngineType engine;
  int32_t repeat;
  uint32_t pcre2_concat;
  uint32_t rematch_session;
  char paddings[4];
};

template<typename Derived, typename Base, typename Del>
std::unique_ptr<Derived, Del>
static_unique_ptr_cast( std::unique_ptr<Base, Del>&& p )
{
  auto d = static_cast<Derived *>(p.release());
  return std::unique_ptr<Derived, Del>(d, std::move(p.get_deleter()));
}

static bool endsWith(const std::string &, const char *);
static Arguments parse_options(int argc, const char *argv[]);
static void compilePCRE2(const Arguments &,
                         std::unique_ptr<regexbench::Engine> &);

int main(int argc, const char *argv[]) {
  try {
    auto args = parse_options(argc, argv);
    std::unique_ptr<regexbench::Engine> engine;
    switch (args.engine) {
    case ENGINE_HYPERSCAN:
      engine = std::make_unique<regexbench::HyperscanEngine>();
      engine->compile(regexbench::loadRules(args.rule_file));
      break;
    case ENGINE_PCRE2:
      engine = std::make_unique<regexbench::PCRE2Engine>();
      compilePCRE2(args, engine);
      break;
    case ENGINE_PCRE2JIT:
      engine = std::make_unique<regexbench::PCRE2JITEngine>();
      compilePCRE2(args, engine);
      break;
    case ENGINE_RE2:
      engine = std::make_unique<regexbench::RE2Engine>();
      engine->compile(regexbench::loadRules(args.rule_file));
      break;
    case ENGINE_REMATCH:
      if (args.rematch_session) {
        engine = std::make_unique<regexbench::REmatchAutomataEngineSession>();
        engine->compile(regexbench::loadRules(args.rule_file));
      } else if (endsWith(args.rule_file, ".nfa")) {
        engine = std::make_unique<regexbench::REmatchAutomataEngine>();
        engine->load(args.rule_file);
      } else if (endsWith(args.rule_file, ".so")) {
        engine = std::make_unique<regexbench::REmatchSOEngine>();
        engine->load(args.rule_file);
      } else {
        engine = std::make_unique<regexbench::REmatchAutomataEngine>();
        engine->compile(regexbench::loadRules(args.rule_file));
      }
      break;
    }

    std::string reportFields[]{
        "TotalMatches", "TotalMatchedPackets",  "UserTime",     "SystemTime",
        "TotalTime",    "TotalBytes",           "TotalPackets", "Mbps",
        "Mpps",         "MaximumMemoryUsed(kB)"};
    size_t nsessions = 0;
    std::string prefix = "regexbench.";
    regexbench::PcapSource pcap(args.pcap_file);
    auto match_info = buildMatchMeta(pcap, nsessions);
    engine->init(nsessions);

    regexbench::MatchResult result;
    if (args.engine == ENGINE_REMATCH && args.rematch_session) {
      result = sessionMatch(*engine, pcap, args.repeat, match_info);
    } else {
      result = match(*engine, pcap, args.repeat, match_info);
    }
    boost::property_tree::ptree pt;
    pt.put(prefix + "TotalMatches", result.nmatches);
    pt.put(prefix + "TotalMatchedPackets", result.nmatched_pkts);
    std::stringstream ss;
    ss << result.udiff.tv_sec << "." << result.udiff.tv_usec;
    pt.put(prefix + "UserTime", ss.str());
    ss.str("");
    ss << result.sdiff.tv_sec << "." << result.sdiff.tv_usec;
    pt.put(prefix + "SystemTime", ss.str());
    ss.str("");
    struct timeval total;
    timeradd(&result.udiff, &result.sdiff, &total);
    ss << total.tv_sec << "." << total.tv_usec;
    pt.put(prefix + "TotalTime", ss.str());
    pt.put(prefix + "TotalBytes", pcap.getNumberOfBytes());
    pt.put(prefix + "TotalPackets", pcap.getNumberOfPackets());
    ss.str("");
    ss << boost::format("%1$.6f") %
              (static_cast<double>(pcap.getNumberOfBytes() *
                                   static_cast<unsigned long>(args.repeat)) /
               (total.tv_sec + total.tv_usec * 1e-6) / 1000000 * 8);
    pt.put(prefix + "Mbps", ss.str());

    ss.str("");
    ss << boost::format("%1$.6f") %
              (static_cast<double>(pcap.getNumberOfPackets() *
                                   static_cast<unsigned long>(args.repeat)) /
               (total.tv_sec + total.tv_usec * 1e-6) / 1000000);
    pt.put(prefix + "Mpps", ss.str());
    struct rusage stat;
    getrusage(RUSAGE_SELF, &stat);
    pt.put(prefix + "MaximumMemoryUsed(kB)", stat.ru_maxrss / 1000);

    std::ostringstream buf;
    write_json(buf, pt, false);
    std::ofstream outputFile(args.output_file);
    outputFile << buf.str();

    for (const auto &it : reportFields) {
      std::cout << it << " : " << pt.get<std::string>(prefix + it) << "\n";
    }
  } catch (const std::exception &e) {
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}

bool endsWith(const std::string &obj, const char *end) {
  auto r = obj.rfind(end);
  if ((r != std::string::npos) && (r == obj.size() - std::strlen(end)))
    return true;
  return false;
}

Arguments parse_options(int argc, const char *argv[]) {
  Arguments args;
  std::string engine;

  po::options_description posargs;
  posargs.add_options()("rule_file", po::value<std::string>(&args.rule_file),
                        "Rule (regular expression) file name");
  posargs.add_options()("pcap_file", po::value<std::string>(&args.pcap_file),
                        "pcap file name");
  po::positional_options_description positions;
  positions.add("rule_file", 1).add("pcap_file", 1);

  po::options_description optargs("Options");
  optargs.add_options()("help,h", "Print usage information.");
  optargs.add_options()(
      "engine,e", po::value<std::string>(&engine)->default_value("hyperscan"),
      "Matching engine to run.");
  optargs.add_options()("repeat,r",
                        po::value<int32_t>(&args.repeat)->default_value(1),
                        "Repeat pcap multiple times.");
  optargs.add_options()(
    "concat,c",
    po::value<uint32_t>(&args.pcre2_concat)->default_value(0),
    "Concatenate PCRE2 rules.");
  optargs.add_options()(
    "session,s",
    po::value<uint32_t>(&args.rematch_session)->default_value(0),
    "Rematch session mode.");
  optargs.add_options()(
      "output,o",
      po::value<std::string>(&args.output_file)->default_value("output.json"),
      "Output JSON file.");
  po::options_description cliargs;
  cliargs.add(posargs).add(optargs);
  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv)
                .options(cliargs)
                .positional(positions)
                .run(),
            vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << "Usage: regexbench <rule_file> <pcap_file>" << std::endl;
    std::cout << posargs << "\n" << optargs << "\n";
    std::exit(EXIT_SUCCESS);
  }
  if (engine == "hyperscan")
    args.engine = ENGINE_HYPERSCAN;
  else if (engine == "pcre2")
    args.engine = ENGINE_PCRE2;
  else if (engine == "pcre2jit")
    args.engine = ENGINE_PCRE2JIT;
  else if (engine == "re2")
    args.engine = ENGINE_RE2;
  else if (engine == "rematch")
    args.engine = ENGINE_REMATCH;
  else {
    std::cerr << "unknown engine: " << engine << std::endl;
    std::exit(EXIT_FAILURE);
  }
  if (args.repeat <= 0) {
    std::cerr << "invalid repeat value: " << args.repeat << std::endl;
    std::exit(EXIT_FAILURE);
  }

  if (!vm.count("rule_file")) {
    std::cerr << "error: no rule file" << std::endl;
    std::exit(EXIT_FAILURE);
  }
  if (!vm.count("pcap_file")) {
    std::cerr << "error: no pcap file" << std::endl;
    std::exit(EXIT_FAILURE);
  }
  return args;
}

static void compilePCRE2(const Arguments &args,
                         std::unique_ptr<regexbench::Engine> &engine) {
  if (!args.pcre2_concat)
    engine->compile(regexbench::loadRules(args.rule_file));
  else {
    auto rules = regexbench::loadRules(args.rule_file);
    concatRules(rules);
    engine->compile(rules);
  }
}
<commit_msg>Fix time calculation<commit_after>#include <sys/resource.h>
#include <sys/time.h>

#include <fstream>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include "HyperscanEngine.h"
#include "PCRE2Engine.h"
#include "PcapSource.h"
#include "RE2Engine.h"
#include "REmatchEngine.h"
#include "Rule.h"
#include "regexbench.h"

using boost::property_tree::ptree;
using boost::property_tree::read_json;
using boost::property_tree::write_json;

namespace po = boost::program_options;

enum EngineType : uint64_t {
  ENGINE_HYPERSCAN,
  ENGINE_PCRE2,
  ENGINE_PCRE2JIT,
  ENGINE_RE2,
  ENGINE_REMATCH
};

struct Arguments {
  std::string output_file;
  std::string pcap_file;
  std::string rule_file;
  EngineType engine;
  int32_t repeat;
  uint32_t pcre2_concat;
  uint32_t rematch_session;
  char paddings[4];
};

template<typename Derived, typename Base, typename Del>
std::unique_ptr<Derived, Del>
static_unique_ptr_cast( std::unique_ptr<Base, Del>&& p )
{
  auto d = static_cast<Derived *>(p.release());
  return std::unique_ptr<Derived, Del>(d, std::move(p.get_deleter()));
}

static bool endsWith(const std::string &, const char *);
static Arguments parse_options(int argc, const char *argv[]);
static void compilePCRE2(const Arguments &,
                         std::unique_ptr<regexbench::Engine> &);

int main(int argc, const char *argv[]) {
  try {
    auto args = parse_options(argc, argv);
    std::unique_ptr<regexbench::Engine> engine;
    switch (args.engine) {
    case ENGINE_HYPERSCAN:
      engine = std::make_unique<regexbench::HyperscanEngine>();
      engine->compile(regexbench::loadRules(args.rule_file));
      break;
    case ENGINE_PCRE2:
      engine = std::make_unique<regexbench::PCRE2Engine>();
      compilePCRE2(args, engine);
      break;
    case ENGINE_PCRE2JIT:
      engine = std::make_unique<regexbench::PCRE2JITEngine>();
      compilePCRE2(args, engine);
      break;
    case ENGINE_RE2:
      engine = std::make_unique<regexbench::RE2Engine>();
      engine->compile(regexbench::loadRules(args.rule_file));
      break;
    case ENGINE_REMATCH:
      if (args.rematch_session) {
        engine = std::make_unique<regexbench::REmatchAutomataEngineSession>();
        engine->compile(regexbench::loadRules(args.rule_file));
      } else if (endsWith(args.rule_file, ".nfa")) {
        engine = std::make_unique<regexbench::REmatchAutomataEngine>();
        engine->load(args.rule_file);
      } else if (endsWith(args.rule_file, ".so")) {
        engine = std::make_unique<regexbench::REmatchSOEngine>();
        engine->load(args.rule_file);
      } else {
        engine = std::make_unique<regexbench::REmatchAutomataEngine>();
        engine->compile(regexbench::loadRules(args.rule_file));
      }
      break;
    }

    std::string reportFields[]{
        "TotalMatches", "TotalMatchedPackets",  "UserTime",     "SystemTime",
        "TotalTime",    "TotalBytes",           "TotalPackets", "Mbps",
        "Mpps",         "MaximumMemoryUsed(kB)"};
    size_t nsessions = 0;
    std::string prefix = "regexbench.";
    regexbench::PcapSource pcap(args.pcap_file);
    auto match_info = buildMatchMeta(pcap, nsessions);
    engine->init(nsessions);

    regexbench::MatchResult result;
    if (args.engine == ENGINE_REMATCH && args.rematch_session) {
      result = sessionMatch(*engine, pcap, args.repeat, match_info);
    } else {
      result = match(*engine, pcap, args.repeat, match_info);
    }
    boost::property_tree::ptree pt;
    pt.put(prefix + "TotalMatches", result.nmatches);
    pt.put(prefix + "TotalMatchedPackets", result.nmatched_pkts);
    std::stringstream ss;
    auto t = result.udiff.tv_sec + result.udiff.tv_usec * 1e-6;
    ss << t;
    pt.put(prefix + "UserTime", ss.str());
    ss.str("");
    t = result.sdiff.tv_sec + result.sdiff.tv_usec * 1e-6;
    ss << t;
    pt.put(prefix + "SystemTime", ss.str());
    ss.str("");
    struct timeval total;
    timeradd(&result.udiff, &result.sdiff, &total);
    t = total.tv_sec + total.tv_usec * 1e-6;
    ss << t;
    pt.put(prefix + "TotalTime", ss.str());
    pt.put(prefix + "TotalBytes", pcap.getNumberOfBytes());
    pt.put(prefix + "TotalPackets", pcap.getNumberOfPackets());
    ss.str("");
    ss << boost::format("%1$.6f") %
              (static_cast<double>(pcap.getNumberOfBytes() *
                                   static_cast<unsigned long>(args.repeat)) /
               (total.tv_sec + total.tv_usec * 1e-6) / 1000000 * 8);
    pt.put(prefix + "Mbps", ss.str());

    ss.str("");
    ss << boost::format("%1$.6f") %
              (static_cast<double>(pcap.getNumberOfPackets() *
                                   static_cast<unsigned long>(args.repeat)) /
               (total.tv_sec + total.tv_usec * 1e-6) / 1000000);
    pt.put(prefix + "Mpps", ss.str());
    struct rusage stat;
    getrusage(RUSAGE_SELF, &stat);
    pt.put(prefix + "MaximumMemoryUsed(kB)", stat.ru_maxrss / 1000);

    std::ostringstream buf;
    write_json(buf, pt, false);
    std::ofstream outputFile(args.output_file);
    outputFile << buf.str();

    for (const auto &it : reportFields) {
      std::cout << it << " : " << pt.get<std::string>(prefix + it) << "\n";
    }
  } catch (const std::exception &e) {
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}

bool endsWith(const std::string &obj, const char *end) {
  auto r = obj.rfind(end);
  if ((r != std::string::npos) && (r == obj.size() - std::strlen(end)))
    return true;
  return false;
}

Arguments parse_options(int argc, const char *argv[]) {
  Arguments args;
  std::string engine;

  po::options_description posargs;
  posargs.add_options()("rule_file", po::value<std::string>(&args.rule_file),
                        "Rule (regular expression) file name");
  posargs.add_options()("pcap_file", po::value<std::string>(&args.pcap_file),
                        "pcap file name");
  po::positional_options_description positions;
  positions.add("rule_file", 1).add("pcap_file", 1);

  po::options_description optargs("Options");
  optargs.add_options()("help,h", "Print usage information.");
  optargs.add_options()(
      "engine,e", po::value<std::string>(&engine)->default_value("hyperscan"),
      "Matching engine to run.");
  optargs.add_options()("repeat,r",
                        po::value<int32_t>(&args.repeat)->default_value(1),
                        "Repeat pcap multiple times.");
  optargs.add_options()(
    "concat,c",
    po::value<uint32_t>(&args.pcre2_concat)->default_value(0),
    "Concatenate PCRE2 rules.");
  optargs.add_options()(
    "session,s",
    po::value<uint32_t>(&args.rematch_session)->default_value(0),
    "Rematch session mode.");
  optargs.add_options()(
      "output,o",
      po::value<std::string>(&args.output_file)->default_value("output.json"),
      "Output JSON file.");
  po::options_description cliargs;
  cliargs.add(posargs).add(optargs);
  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv)
                .options(cliargs)
                .positional(positions)
                .run(),
            vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << "Usage: regexbench <rule_file> <pcap_file>" << std::endl;
    std::cout << posargs << "\n" << optargs << "\n";
    std::exit(EXIT_SUCCESS);
  }
  if (engine == "hyperscan")
    args.engine = ENGINE_HYPERSCAN;
  else if (engine == "pcre2")
    args.engine = ENGINE_PCRE2;
  else if (engine == "pcre2jit")
    args.engine = ENGINE_PCRE2JIT;
  else if (engine == "re2")
    args.engine = ENGINE_RE2;
  else if (engine == "rematch")
    args.engine = ENGINE_REMATCH;
  else {
    std::cerr << "unknown engine: " << engine << std::endl;
    std::exit(EXIT_FAILURE);
  }
  if (args.repeat <= 0) {
    std::cerr << "invalid repeat value: " << args.repeat << std::endl;
    std::exit(EXIT_FAILURE);
  }

  if (!vm.count("rule_file")) {
    std::cerr << "error: no rule file" << std::endl;
    std::exit(EXIT_FAILURE);
  }
  if (!vm.count("pcap_file")) {
    std::cerr << "error: no pcap file" << std::endl;
    std::exit(EXIT_FAILURE);
  }
  return args;
}

static void compilePCRE2(const Arguments &args,
                         std::unique_ptr<regexbench::Engine> &engine) {
  if (!args.pcre2_concat)
    engine->compile(regexbench::loadRules(args.rule_file));
  else {
    auto rules = regexbench::loadRules(args.rule_file);
    concatRules(rules);
    engine->compile(rules);
  }
}
<|endoftext|>


<commit_before>#include "shader.hpp"

//////////////
// Includes //
#include <iostream>
#include <fstream>

//////////
// Code //

// Attempting to create a specific shader.
GLuint loadShader(std::string path, GLenum type) {
    std::ifstream file(path);
    if (!file.good())
        return 0;

    std::string contents;
    while (!file.eof()) {
        if (file.peek() == -1)
            continue;
        contents.push_back((char)file.get());
    }

    GLuint shader = glCreateShader(type);
    if (shader == 0)
        return 0;

    const char* contentsCStr = contents.c_str();
    glShaderSource(shader, 1, &contentsCStr, nullptr);

    glCompileShader(shader);

    GLint compiled;
    glGetShaderiv(shader, GL_COMPILE_STATUS, &compiled);
    if (compiled == GL_FALSE) {
        int len;
        GLchar log[1024];
        glGetShaderInfoLog(shader, 1024, &len, log);

        std::cout << "Failed to compile shader '" << path << "':" << std::endl;
        std::cout << log << std::endl;
        return 0;
    }

    return shader;
}

// Constructing a shader from a location on the disk.
Shader::Shader(std::string path) {
    GLuint vertShader = loadShader(path + ".vert", GL_VERTEX_SHADER);
    GLuint fragShader = loadShader(path + ".frag", GL_FRAGMENT_SHADER);
    GLuint geomShader = loadShader(path + ".geom", GL_GEOMETRY_SHADER);

    if (vertShader == 0 && fragShader == 0 && geomShader == 0) {
        this->id = 0;
        std::cout << "Failed to load any shaders for '" << path << "'." << std::endl;
    } else {
        this->id = glCreateProgram();

        if (vertShader != 0)
            glAttachShader(id, vertShader);
        if (fragShader != 0)
            glAttachShader(id, fragShader);
        if (geomShader != 0)
            glAttachShader(id, geomShader);

        glLinkProgram(this->id);

        glDeleteShader(vertShader);
        glDeleteShader(fragShader);
        glDeleteShader(geomShader);
    }
}

// Deleting a shader.
Shader::~Shader() { glDeleteProgram(this->id); }

// Accessing the shader id.
GLuint Shader::getID() const { return this->id; }
<commit_msg>More error handling!<commit_after>#include "shader.hpp"

//////////////
// Includes //
#include <iostream>
#include <fstream>

//////////
// Code //

// Attempting to create a specific shader.
GLuint loadShader(std::string path, GLenum type) {
    std::ifstream file(path);
    if (!file.good())
        return 0;

    std::string contents;
    while (!file.eof()) {
        if (file.peek() == -1)
            continue;
        contents.push_back((char)file.get());
    }

    GLuint shader = glCreateShader(type);
    if (shader == 0)
        return 0;

    const char* contentsCStr = contents.c_str();
    glShaderSource(shader, 1, &contentsCStr, nullptr);

    glCompileShader(shader);

    GLint compiled;
    glGetShaderiv(shader, GL_COMPILE_STATUS, &compiled);
    if (compiled == GL_FALSE) {
        int len;
        GLchar log[1024];
        glGetShaderInfoLog(shader, 1024, &len, log);

        std::cout << "Failed to compile shader '" << path << "':" << std::endl;
        std::cout << log << std::endl;
        glDeleteShader(shader);
        return 0;
    }

    return shader;
}

// Constructing a shader from a location on the disk.
Shader::Shader(std::string path) {
    GLuint vertShader = loadShader(path + ".vert", GL_VERTEX_SHADER);
    GLuint fragShader = loadShader(path + ".frag", GL_FRAGMENT_SHADER);
    GLuint geomShader = loadShader(path + ".geom", GL_GEOMETRY_SHADER);

    if (vertShader == 0 && fragShader == 0 && geomShader == 0) {
        this->id = 0;
        std::cout << "Failed to load any shaders for '" << path << "'." << std::endl;
    } else {
        this->id = glCreateProgram();

        if (vertShader != 0)
            glAttachShader(id, vertShader);
        if (fragShader != 0)
            glAttachShader(id, fragShader);
        if (geomShader != 0)
            glAttachShader(id, geomShader);

        glLinkProgram(this->id);

        int linked;
        glGetProgramiv(this->id, GL_LINK_STATUS, &linked);
        if (linked == GL_FALSE) {
            std::cout << "Failed to link '" << path << "'!" << std::endl;
            glDeleteProgram(this->id);
        }

        glDeleteShader(vertShader);
        glDeleteShader(fragShader);
        glDeleteShader(geomShader);
    }
}

// Deleting a shader.
Shader::~Shader() { glDeleteProgram(this->id); }

// Accessing the shader id.
GLuint Shader::getID() const { return this->id; }
<|endoftext|>


<commit_before>#include <cstdio>
#include <cstring>
#include <cstdlib>

#include <unistd.h>

#include <allegro5/allegro5.h>
#include <allegro5/allegro_primitives.h>

#include "laser_types.h"
#include "macroreader.h"
#include "macroerror.h"

#include "shape_math.h"

#define LIST_SIZE 500

/* screen ratio currently needs to be 1:1 as otherwise circular arc turns into elliptical arc and we have to do maths xP */
const double screen_width = 1024;
const double screen_height = 1024;

const double paper_width = 200000000;
const double paper_height = 150000000;

const double paper_ratio = paper_height/paper_width;

const double x_shift = screen_width/2;
const double y_shift = screen_height/2;

/*
 * Scaling
 *  -6 = 1000 = um
 * const double nm_to_mm = 1000;
 */
const double nm_to_minus4 = 200000;
/* -3 = 1000000 = mm
 *const double nm_to_mm = 1000000;
 */
const double scale_factor = nm_to_minus4;

const double draw_width = paper_width/scale_factor;
const double draw_height = paper_height/scale_factor;

const double left_border = (screen_width/2) - (draw_width/2);
const double right_border = screen_width - left_border;
const double top_border = (screen_height/2) - (draw_height/2);
const double bottom_border = screen_height - top_border;

/* show the undrawable area */
const double blank_top = (1.0-paper_ratio)/2*screen_height;
const double blank_bottom = screen_height - blank_top;

/* these are the origins set by the laser after homing */
double x_origin = +31718800;
double y_origin = -1414600;

void get_screen_coords(double &screen_x, double &screen_y, double laser_x, double laser_y)
{
	screen_x = laser_x / scale_factor;
	screen_x += x_shift;
	screen_y = laser_y / scale_factor;
	screen_y += y_shift;
}
void scale_to_screen(double &screen_radius, double laser_radius)
{
	screen_radius = laser_radius/scale_factor;
}

void get_circle_point(double &xret, double &yret, double x_center, double y_center, double radius, double t_angle){
	/* circle is parametrically defined as x=r*cos(t)+xcenter, y=r*sin(t)+ycenter 0<t<=2*pi */
	xret = radius * cos(t_angle) + x_center;
	yret = radius * sin(t_angle) + y_center;
}

int main(int argc, char *argv[]) {
	if (argc != 2) {
		printf("One file expected\n");
		return -1;
	}

	ALLEGRO_DISPLAY * display = NULL;

	if (!al_init()) {
		fprintf(stderr, "failed to initialize allegro\n");
		return -1;
	}
	if (!al_init_primitives_addon()) {
		fprintf(stderr, "failed to initilize primitivies addon\n");
		return -1;
	}

	display = al_create_display(screen_width, screen_height);
	if (!display) {
		fprintf(stderr, "failed to create display");
		return -1;
	}
	/* clear the drawing area */
	al_clear_to_color(al_map_rgb(255,255,255));
	/* block out the area that isn't paper */
	al_draw_filled_rectangle(0,0,screen_width,top_border, al_map_rgb(0,0,0));
	al_draw_filled_rectangle(0,screen_height,screen_height,bottom_border, al_map_rgb(0,0,0));
	al_draw_filled_rectangle(0,0,left_border, screen_height, al_map_rgb(0,0,0));
	al_draw_filled_rectangle(right_border, 0, screen_width, screen_height, al_map_rgb(0,0,0));

	//gnuplot_ctrl *gnu_plot = gnuplot_init();
	mr_init(argv[1]);

	mr_inst_ptr instruction;
	/* line coordinates */
	double laser_x_new, laser_y_new, laser_x_old, laser_y_old;
	double screen_x_new, screen_y_new, screen_x_old, screen_y_old;
	/* arc coordinates */
	double start_theta, end_theta;
	double laser_x_center, laser_y_center, laser_radius;
	double laser_x_rad_offs, laser_y_rad_offs;

	double screen_x_center, screen_y_center, screen_radius;

	int read_ret = M_ERR_SUCCESS;
	/* get commands one by one from file */
	for (int i=0; read_ret != M_ERR_EOF; i++) {
		enum m_commands_t command_type;
		read_ret = mr_read(instruction, command_type);
		if (instruction != NULL) {
			if (command_type == m_relative) {
				m_line_t *line = (m_line_t *) instruction;
				laser_x_new = atof(line->x);
				laser_y_new = atof(line->y);
				get_screen_coords(screen_x_new, screen_y_new, laser_x_new, laser_y_new);
				printf("drawing from (%f,%f) to (%f,%f)\n", screen_x_old, screen_y_old, screen_x_new, screen_y_new);
				if (atof(line->spacing) != 0.0)
					al_draw_line(screen_x_old, screen_y_old, screen_x_old+screen_x_new, screen_y_old+screen_y_new, al_map_rgb(0,0,0), 2);

				/* copy across current position */
				screen_x_old = screen_x_new;
				screen_y_old = screen_y_new;
				laser_x_old = laser_x_new;
				laser_y_old = laser_y_new;
				mr_free(instruction);
			}
			if (command_type == m_absolute) {
				m_line_t *line = (m_line_t *) instruction;
				laser_x_new = atof(line->x);
				laser_y_new = atof(line->y);
				get_screen_coords(screen_x_new, screen_y_new, laser_x_new, laser_y_new);
				printf("drawing from (%f,%f) to (%f,%f)\n", screen_x_old, screen_y_old, screen_x_new, screen_y_new);
				if (atof(line->spacing) != 0.0)
					al_draw_line(screen_x_old, screen_y_old, screen_x_new, screen_y_new, al_map_rgb(0,0,0), 2);

				/* copy across current position */
				screen_x_old = screen_x_new;
				screen_y_old = screen_y_new;
				laser_x_old = laser_x_new;
				laser_y_old = laser_y_new;
				mr_free(instruction);
			}
			if (command_type == m_origin) {
				m_line_t *line = (m_line_t *) instruction;
				laser_x_new = atof(line->x);
				laser_y_new = atof(line->y);
				get_screen_coords(screen_x_new, screen_y_new, laser_x_new, laser_y_new);
				printf("drawing from (%f,%f) to (%f,%f)\n", screen_x_old, screen_y_old, screen_x_new, screen_y_new);
				if (atof(line->spacing) != 0.0)
					al_draw_line(screen_x_old, screen_y_old, screen_x_new, screen_y_new, al_map_rgb(0,0,0), 2);

				/* copy across current position */
				screen_x_old = screen_x_new;
				screen_y_old = screen_y_new;
				laser_x_old = laser_x_new;
				laser_y_old = laser_y_new;
				mr_free(instruction);
			}
			if (command_type == m_arc) {
				m_arc_t *arc = (m_arc_t *) instruction;

				start_theta = atof(arc->start_angle);
				end_theta = atof(arc->end_angle);

				laser_radius = atof(arc->radius);

				/* get arc center point */
				/* x^2 + y^2 = r^2
				 * r is known x and y are unknown
				 */
				printf("edge at %f,%f\n", laser_x_old, laser_y_old);
				printf("radx: %f, rady%f\n", laser_radius*cos(start_theta), laser_radius*sin(start_theta));

				laser_x_rad_offs = laser_radius * cos(start_theta);
				laser_y_rad_offs = laser_radius * sin(start_theta);

				printf("center shift by %f,%f\n", laser_x_rad_offs, laser_y_rad_offs);

				laser_x_center = laser_x_old - laser_x_rad_offs;
				laser_y_center = laser_y_old - laser_y_rad_offs;

				laser_x_new = laser_radius * cos(end_theta) + laser_x_center;
				laser_y_new = laser_radius * sin(end_theta) + laser_y_center;

				get_screen_coords(screen_x_center, screen_y_center, laser_x_center, laser_y_center);
				scale_to_screen(screen_radius, laser_radius);

				printf("x:%f, y:%f, radius:%f\n", screen_x_center, screen_y_center, screen_radius);
				if ( strcmp(arc->laser_on, "True") == 0) {
					for (double i = start_theta*2*3.14159/180; i<end_theta*2*3.14159/180; i+=0.1) {
						get_circle_point(screen_y_new, screen_x_new, screen_x_center, screen_y_center, screen_radius, i);
					}

					al_draw_arc(screen_x_center, screen_y_center, screen_radius, start_theta*2*3.14159/180, end_theta*2*3.14159/180, al_map_rgb(0,0,0), 2);

				}

				/* copy across current position */
				get_circle_point(screen_y_new, screen_x_new, screen_x_center, screen_y_center, screen_radius, end_theta*2*3.14159/180);
				laser_x_old = laser_x_new;
				laser_y_old = laser_y_new;

				mr_free(instruction);
			}

		}
		else
			printf("Null inst\n");
	}

	mr_close();

	al_flip_display();
	al_rest(10.0);

	return 0;

}
<commit_msg>Well's work, just needed to be regenerated. disabled a number of print statements<commit_after>#include <cstdio>
#include <cstring>
#include <cstdlib>

#include <unistd.h>

#include <allegro5/allegro5.h>
#include <allegro5/allegro_primitives.h>

#include "laser_types.h"
#include "macroreader.h"
#include "macroerror.h"

#include "shape_math.h"

#define LIST_SIZE 500

/* screen ratio currently needs to be 1:1 as otherwise circular arc turns into elliptical arc and we have to do maths xP */
const double screen_width = 1024;
const double screen_height = 1024;

const double paper_width = 200000000;
const double paper_height = 150000000;

const double paper_ratio = paper_height/paper_width;

const double x_shift = screen_width/2;
const double y_shift = screen_height/2;

/*
 * Scaling
 *  -6 = 1000 = um
 * const double nm_to_mm = 1000;
 */
const double nm_to_minus4 = 200000;
/* -3 = 1000000 = mm
 *const double nm_to_mm = 1000000;
 */
const double scale_factor = nm_to_minus4;

const double draw_width = paper_width/scale_factor;
const double draw_height = paper_height/scale_factor;

const double left_border = (screen_width/2) - (draw_width/2);
const double right_border = screen_width - left_border;
const double top_border = (screen_height/2) - (draw_height/2);
const double bottom_border = screen_height - top_border;

/* show the undrawable area */
const double blank_top = (1.0-paper_ratio)/2*screen_height;
const double blank_bottom = screen_height - blank_top;

/* these are the origins set by the laser after homing */
double x_origin = +31718800;
double y_origin = -1414600;

void get_screen_coords(double &screen_x, double &screen_y, double laser_x, double laser_y)
{
	screen_x = laser_x / scale_factor;
	screen_x += x_shift;
	screen_y = laser_y / scale_factor;
	screen_y += y_shift;
}
void scale_to_screen(double &screen_radius, double laser_radius)
{
	screen_radius = laser_radius/scale_factor;
}

void get_circle_point(double &xret, double &yret, double x_center, double y_center, double radius, double t_angle){
	/* circle is parametrically defined as x=r*cos(t)+xcenter, y=r*sin(t)+ycenter 0<t<=2*pi */
	xret = radius * cos(t_angle) + x_center;
	yret = radius * sin(t_angle) + y_center;
}

int main(int argc, char *argv[]) {
	if (argc != 2) {
		printf("One file expected\n");
		return -1;
	}

	ALLEGRO_DISPLAY * display = NULL;

	if (!al_init()) {
		fprintf(stderr, "failed to initialize allegro\n");
		return -1;
	}
	if (!al_init_primitives_addon()) {
		fprintf(stderr, "failed to initilize primitivies addon\n");
		return -1;
	}

	display = al_create_display(screen_width, screen_height);
	if (!display) {
		fprintf(stderr, "failed to create display");
		return -1;
	}
	/* clear the drawing area */
	al_clear_to_color(al_map_rgb(255,255,255));
	/* block out the area that isn't paper */
	al_draw_filled_rectangle(0,0,screen_width,top_border, al_map_rgb(0,0,0));
	al_draw_filled_rectangle(0,screen_height,screen_height,bottom_border, al_map_rgb(0,0,0));
	al_draw_filled_rectangle(0,0,left_border, screen_height, al_map_rgb(0,0,0));
	al_draw_filled_rectangle(right_border, 0, screen_width, screen_height, al_map_rgb(0,0,0));

	//gnuplot_ctrl *gnu_plot = gnuplot_init();
	mr_init(argv[1]);

	mr_inst_ptr instruction;
	/* line coordinates */
	double laser_x_new, laser_y_new, laser_x_old, laser_y_old;
	double screen_x_new, screen_y_new, screen_x_old, screen_y_old;
	/* arc coordinates */
	double start_theta, end_theta;
	double laser_x_center, laser_y_center, laser_radius;
	double laser_x_rad_offs, laser_y_rad_offs;

	double screen_x_center, screen_y_center, screen_radius;

	int read_ret = M_ERR_SUCCESS;
	/* get commands one by one from file */
	for (int i=0; read_ret != M_ERR_EOF; i++) {
		enum m_commands_t command_type;
		read_ret = mr_read(instruction, command_type);
		if (instruction != NULL) {
			if (command_type == m_relative) {
				m_line_t *line = (m_line_t *) instruction;
				laser_x_new = atof(line->x);
				laser_y_new = atof(line->y);
				get_screen_coords(screen_x_new, screen_y_new, laser_x_new, laser_y_new);
				//printf("drawing from (%f,%f) to (%f,%f)\n", screen_x_old, screen_y_old, screen_x_new, screen_y_new);
				if (atof(line->spacing) != 0.0)
					al_draw_line(screen_x_old, screen_y_old, screen_x_old+screen_x_new, screen_y_old+screen_y_new, al_map_rgb(0,0,0), 2);

				/* copy across current position */
				screen_x_old = screen_x_new;
				screen_y_old = screen_y_new;
				laser_x_old = laser_x_new;
				laser_y_old = laser_y_new;
				mr_free(instruction);
			}
			if (command_type == m_absolute) {
				m_line_t *line = (m_line_t *) instruction;
				laser_x_new = atof(line->x);
				laser_y_new = atof(line->y);
				get_screen_coords(screen_x_new, screen_y_new, laser_x_new, laser_y_new);
				//printf("drawing from (%f,%f) to (%f,%f)\n", screen_x_old, screen_y_old, screen_x_new, screen_y_new);
				if (atof(line->spacing) != 0.0)
					al_draw_line(screen_x_old, screen_y_old, screen_x_new, screen_y_new, al_map_rgb(0,0,0), 2);

				/* copy across current position */
				screen_x_old = screen_x_new;
				screen_y_old = screen_y_new;
				laser_x_old = laser_x_new;
				laser_y_old = laser_y_new;
				mr_free(instruction);
			}
			if (command_type == m_origin) {
				m_line_t *line = (m_line_t *) instruction;
				laser_x_new = atof(line->x);
				laser_y_new = atof(line->y);
				get_screen_coords(screen_x_new, screen_y_new, laser_x_new, laser_y_new);
				//printf("drawing from (%f,%f) to (%f,%f)\n", screen_x_old, screen_y_old, screen_x_new, screen_y_new);
				if (atof(line->spacing) != 0.0)
					al_draw_line(screen_x_old, screen_y_old, screen_x_new, screen_y_new, al_map_rgb(0,0,0), 2);

				/* copy across current position */
				screen_x_old = screen_x_new;
				screen_y_old = screen_y_new;
				laser_x_old = laser_x_new;
				laser_y_old = laser_y_new;
				mr_free(instruction);
			}
			if (command_type == m_arc) {
				m_arc_t *arc = (m_arc_t *) instruction;

				start_theta = atof(arc->start_angle);
				end_theta = atof(arc->end_angle);

				laser_radius = atof(arc->radius);

				/* get arc center point */
				/* x^2 + y^2 = r^2
				 * r is known x and y are unknown
				 */
				//printf("edge at %f,%f\n", laser_x_old, laser_y_old);
				//printf("radx: %f, rady%f\n", laser_radius*cos(start_theta), laser_radius*sin(start_theta));

				laser_x_rad_offs = laser_radius * cos(start_theta);
				laser_y_rad_offs = laser_radius * sin(start_theta);

				//printf("center shift by %f,%f\n", laser_x_rad_offs, laser_y_rad_offs);

				laser_x_center = laser_x_old - laser_x_rad_offs;
				laser_y_center = laser_y_old - laser_y_rad_offs;

				laser_x_new = laser_radius * cos(end_theta) + laser_x_center;
				laser_y_new = laser_radius * sin(end_theta) + laser_y_center;

				get_screen_coords(screen_x_center, screen_y_center, laser_x_center, laser_y_center);
				scale_to_screen(screen_radius, laser_radius);
				//printf("lx:%f, ly:%f, lrad:%f\n", laser_x_center, laser_y_center, laser_radius);
				//printf("x:%f, y:%f, radius:%f\n", screen_x_center, screen_y_center, screen_radius);
				if ( strcmp(arc->laser_on, "True") == 0) {
					for (double i = start_theta*2*3.14159/180; i<end_theta*2*3.14159/180; i+=0.1) {
						get_circle_point(screen_y_new, screen_x_new, screen_x_center, screen_y_center, screen_radius, i);
					}

					al_draw_arc(screen_x_center, screen_y_center, screen_radius, start_theta*2*3.14159/180, end_theta*2*3.14159/180, al_map_rgb(0,0,0), 2);

				}

				/* copy across current position */
				get_circle_point(screen_y_new, screen_x_new, screen_x_center, screen_y_center, screen_radius, end_theta*2*3.14159/180);
				laser_x_old = laser_x_new;
				laser_y_old = laser_y_new;

				mr_free(instruction);
			}

		}
		else
			printf("Null inst\n");
	}

	mr_close();

	al_flip_display();
	al_rest(10.0);

	return 0;

}
<|endoftext|>