<commit_before>#include "Performance.hpp"
#include <stdio.h>
#include <float.h>
#include <thread>
#include <atomic>

/**
 * Repeat the same lambda expression for a defined amout of seconds and print out
 * the number of iterations as well as the execution time for a single operation.
 *
 * @param seconds Time in seconds (How long should @b func be executed)
 * @param description Short description for this Performance Test. Will be printed to the console
 * @param func Lambda expression or Functor class to be profiled (simply use \a[&]{code} )
 *
 * @param multiThreadingEnabled If \b true automatically detach new threads for all iterations (default: false)
 */
void Performance::howManyOperationsInSeconds(const double seconds, const char* description, std::function<void()> func, bool multiThreadingEnabled)
{
	if (multiThreadingEnabled)
	{
		static const unsigned int hwThreadCount = std::thread::hardware_concurrency();
		std::atomic<unsigned long> iters(0); // Use GCC 4.7+ (GCC 4.6 atomics are not lock free)
		std::atomic<unsigned int> threadsRunning(0);

		Timer t;
		while (t.elapsed() < seconds) {
			if (threadsRunning < hwThreadCount) {
				++threadsRunning;
				std::thread([&]{
					func();
					++iters;
					--threadsRunning;
				}).detach();
			}
		}
		double time = t.elapsed();
		unsigned long numberOfIterations = iters;

		PerformancePrintOperationsPerSecond(description, time, numberOfIterations);

		// wait till all previous threads are finished
		while (threadsRunning) {
			std::this_thread::sleep_for(std::chrono::milliseconds(50));
		}
	}
	else // single core (And single thread? Or does C++ optimize for multi-thread automatically?)
	{
		unsigned long numberOfIterations = 0;
		Timer t;
		while (t.elapsed() < seconds) {
			func();
			++numberOfIterations;
		}
		double time = t.elapsed();

		PerformancePrintOperationsPerSecond(description, time, numberOfIterations);
	}
}

/**
 * Repeat two different algorithms with the same iteration count and print out which one is faster.
 * Print out will always compare \b funcB in relation to \b funcA
 * (if funcA is faster than funcB value will be less than 1.0x)
 *
 * @param iterations How often to repeat both functions
 * @param description Short description for this Test. Will be printed to the console
 * @param funcA Lambda expression or Functor class to be compared (simply use \a[&]{code} )
 * @param funcB Lambda expression or Functor class to be compared (simply use \a[&]{code} )
 */
void Performance::compareExecutionTime(const unsigned long iterations, const char* description, std::function<void()> funcA, std::function<void()> funcB) {

	size_t repeatA = iterations;
	Timer tA;
	while (repeatA--) {
		funcA();
	}
	double timeA = tA.elapsed();

	size_t repeatB = iterations;
	Timer tB;
	while (repeatB--) {
		funcB();
	}
	double timeB = tB.elapsed();

	printf("Testing <%s> Algorithm B is %1.2fx faster (%1.4f, %1.4f)\n", description, timeA / timeB, timeA, timeB);
}

/**
 * Run the provided code once and print out the execution time
 * @param func Lambda expression or Functor class to be profiled (simply use \a[&]{code} )
 */
void Performance::time(std::function<void()> func) {
	repeat(1, 1, [&func](size_t){func();});
}

/**
 * Run the provided code multiple times and print out the fastest iteration.
 * For further info why it's not avarage time, refer to the implementation body.
 *
 * Use this code inside your Functor:
 @code
 [](size_t numberOfElements){
   <#init#>
   while (numberOfElements--) {
     <#code#>
   }
 }
 @endcode
 *
 * @param iterations How often should \b func be executed in one round
 * @param rounds Multiple rounds will increase the chance for a better time
 * @param func Lambda expression or Functor class to be profiled (simply use \a[&]{code} )
 */
void Performance::repeat(const unsigned long iterations, const unsigned long rounds, std::function<void(unsigned long numberOfElements)> func) {
	size_t r = rounds;

	double fastest = DBL_MAX;
	while (r--) {
		Timer t;
		unsigned long it = iterations;
		func(it);
		double time = t.elapsed();

		if (time < fastest) {
			// We use the shortest elapsed time here instead of an avarage time over all iterations.
			// We want to evaluate the runtime of the core algorithm without OS dependent operations.
			// The thinking is that the algorithm will always run in the same amount of time but will
			// be interrupted by the operating system or other running application.
			// An average runtime may be useful for the end user, but not for the purpose of comparing
			// two different implementations for the same logical algorithm.
			fastest = time;
		}
	}

	// generate output
	if (iterations > 1)
		printf("%lu iterations, ", iterations);

	printf("%lf seconds (%lfms)", fastest, fastest*1000);

	if (rounds > 1)
		printf(" on average (%lu times)", rounds);
	printf(".\n");
}

<commit_msg>Improved image queue based multi threading<commit_after>#include "Performance.hpp"
#include <stdio.h>
#include <float.h>
#include <thread>
#include <atomic>

/**
 * Repeat the same lambda expression for a defined amout of seconds and print out
 * the number of iterations as well as the execution time for a single operation.
 *
 * @param seconds Time in seconds (How long should @b func be executed)
 * @param description Short description for this Performance Test. Will be printed to the console
 * @param func Lambda expression or Functor class to be profiled (simply use \a[&]{code} )
 *
 * @param multiThreadingEnabled If \b true automatically detach new threads for all iterations (default: false)
 */
void Performance::howManyOperationsInSeconds(const double seconds, const char* description, std::function<void()> func, bool multiThreadingEnabled)
{
	if (multiThreadingEnabled)
	{
		static const unsigned int hwThreadCount = std::thread::hardware_concurrency();
		unsigned long* counters = new unsigned long[hwThreadCount];
		std::thread* threads = new std::thread[hwThreadCount];

		unsigned int i = hwThreadCount;
		while (i--) {
			threads[i] = std::thread([seconds, i, &func, &counters]{
				Timer inner;
				unsigned long innerCounter = 0;
				while (inner.elapsed() < seconds) {
					func();
					++innerCounter;
				}
				counters[i] = innerCounter;
			});
		}

		unsigned long numberOfIterations = 0;
		i = hwThreadCount;
		Timer t;
		while (i--) {
			threads[i].join();
			numberOfIterations += counters[i];
		}

		double time = t.elapsed();
		PerformancePrintOperationsPerSecond(description, time, numberOfIterations);
	}
	else // single core (And single thread? Or does C++ optimize for multi-thread automatically?)
	{
		unsigned long numberOfIterations = 0;
		Timer t;
		while (t.elapsed() < seconds) {
			func();
			++numberOfIterations;
		}
		double time = t.elapsed();

		PerformancePrintOperationsPerSecond(description, time, numberOfIterations);
	}
}

/**
 * Repeat two different algorithms with the same iteration count and print out which one is faster.
 * Print out will always compare \b funcB in relation to \b funcA
 * (if funcA is faster than funcB value will be less than 1.0x)
 *
 * @param iterations How often to repeat both functions
 * @param description Short description for this Test. Will be printed to the console
 * @param funcA Lambda expression or Functor class to be compared (simply use \a[&]{code} )
 * @param funcB Lambda expression or Functor class to be compared (simply use \a[&]{code} )
 */
void Performance::compareExecutionTime(const unsigned long iterations, const char* description, std::function<void()> funcA, std::function<void()> funcB) {

	size_t repeatA = iterations;
	Timer tA;
	while (repeatA--) {
		funcA();
	}
	double timeA = tA.elapsed();

	size_t repeatB = iterations;
	Timer tB;
	while (repeatB--) {
		funcB();
	}
	double timeB = tB.elapsed();

	printf("Testing <%s> Algorithm B is %1.2fx faster (%1.4f, %1.4f)\n", description, timeA / timeB, timeA, timeB);
}

/**
 * Run the provided code once and print out the execution time
 * @param func Lambda expression or Functor class to be profiled (simply use \a[&]{code} )
 */
void Performance::time(std::function<void()> func) {
	repeat(1, 1, [&func](size_t){func();});
}

/**
 * Run the provided code multiple times and print out the fastest iteration.
 * For further info why it's not avarage time, refer to the implementation body.
 *
 * Use this code inside your Functor:
 @code
 [](size_t numberOfElements){
   <#init#>
   while (numberOfElements--) {
     <#code#>
   }
 }
 @endcode
 *
 * @param iterations How often should \b func be executed in one round
 * @param rounds Multiple rounds will increase the chance for a better time
 * @param func Lambda expression or Functor class to be profiled (simply use \a[&]{code} )
 */
void Performance::repeat(const unsigned long iterations, const unsigned long rounds, std::function<void(unsigned long numberOfElements)> func) {
	size_t r = rounds;

	double fastest = DBL_MAX;
	while (r--) {
		Timer t;
		unsigned long it = iterations;
		func(it);
		double time = t.elapsed();

		if (time < fastest) {
			// We use the shortest elapsed time here instead of an avarage time over all iterations.
			// We want to evaluate the runtime of the core algorithm without OS dependent operations.
			// The thinking is that the algorithm will always run in the same amount of time but will
			// be interrupted by the operating system or other running application.
			// An average runtime may be useful for the end user, but not for the purpose of comparing
			// two different implementations for the same logical algorithm.
			fastest = time;
		}
	}

	// generate output
	if (iterations > 1)
		printf("%lu iterations, ", iterations);

	printf("%lf seconds (%lfms)", fastest, fastest*1000);

	if (rounds > 1)
		printf(" on average (%lu times)", rounds);
	printf(".\n");
}

<|endoftext|>


<commit_before>#include <stdio.h>
#include <stdlib.h>

#include "App.h"
#include <Directory.h>
#include <NodeMonitor.h>

App::App(void)
  : BApplication("application/x-vnd.lh-MyDropboxClient")
{
  //start watching ~/Dropbox folder
  BDirectory dir("/boot/home/Dropbox");
  node_ref nref;
  status_t err;
  if(dir.InitCheck() == B_OK){
    dir.GetNodeRef(&nref);
    err = watch_node(&nref, B_WATCH_DIRECTORY, BMessenger(this));
    if(err != B_OK)
      printf("Watch Node: Not OK\n");
  }
}

int
run_script(char *cmd)
{
  char buf[BUFSIZ];
  FILE *ptr;

  if ((ptr = popen(cmd, "r")) != NULL)
    while (fgets(buf, BUFSIZ, ptr) != NULL)
      (void) printf("RAWR%s", buf);
  (void) pclose(ptr);
  return 0;
}

void
delete_file_on_dropbox(char * filepath)
{
  run_script("python db_ls.py");
}

void
add_file_to_dropbox(char * filepath)
{
  run_script("python db_ls.py");
}

void
moved_file(BMessage *msg)
{
  //is this file being move into or out of ~/Dropbox?
  run_script("python db_ls.py");
}

void
App::MessageReceived(BMessage *msg)
{
  switch(msg->what)
  {
    case B_NODE_MONITOR:
    {
      printf("Received Node Monitor Alert\n");
      status_t err;
      int32 opcode;
      err = msg->FindInt32("opcode",&opcode);
      if(err == B_OK)
      {
        printf("what:%d\topcode:%d\n",msg->what, opcode);
        switch(opcode)
        {
          case B_ENTRY_CREATED:
          {
            printf("NEW FILE\n");
            add_file_to_dropbox("hello");
            break;
          }
          case B_ENTRY_MOVED:
          {
            printf("MOVED FILE\n");
            moved_file(msg);
            break;
          }
          case B_ENTRY_REMOVED:
          {
            printf("DELETED FILE\n");
            delete_file_on_dropbox("hi");
            break;
          }
          default:
          {
            printf("default case opcode...\n");
          }
        }
      }
      break;
    }
    default:
    {
      printf("default msg\n");
      BApplication::MessageReceived(msg);
      break;
    }
  }
}

int
main(void)
{
  //Haiku make window code
  App *app = new App();

  app->Run();
  delete app;
  return 0;
}
<commit_msg>when a new file is added to Dropbox, add it as an absolute path to Dropbox.<commit_after>#include <stdio.h>
#include <stdlib.h>

#include "App.h"
#include <Directory.h>
#include <NodeMonitor.h>
#include <Entry.h>
#include <Path.h>
#include <String.h>

App::App(void)
  : BApplication("application/x-vnd.lh-MyDropboxClient")
{
  //start watching ~/Dropbox folder
  BDirectory dir("/boot/home/Dropbox");
  node_ref nref;
  status_t err;
  if(dir.InitCheck() == B_OK){
    dir.GetNodeRef(&nref);
    err = watch_node(&nref, B_WATCH_DIRECTORY, BMessenger(this));
    if(err != B_OK)
      printf("Watch Node: Not OK\n");
  }
}

int
run_script(const char *cmd)
{
  char buf[BUFSIZ];
  FILE *ptr;

  if ((ptr = popen(cmd, "r")) != NULL)
    while (fgets(buf, BUFSIZ, ptr) != NULL)
      (void) printf("RAWR%s", buf);
  (void) pclose(ptr);
  return 0;
}

void
delete_file_on_dropbox(char * filepath)
{
  run_script("python db_ls.py");
}

void
add_file_to_dropbox(const char * filepath)
{
  BString s;
  s << "python db_put.py " << BString(filepath) << " " << BString(filepath);
  run_script(s.String());
}

void
moved_file(BMessage *msg)
{
  //is this file being move into or out of ~/Dropbox?
  run_script("python db_ls.py");
}

void
App::MessageReceived(BMessage *msg)
{
  switch(msg->what)
  {
    case B_NODE_MONITOR:
    {
      printf("Received Node Monitor Alert\n");
      status_t err;
      int32 opcode;
      err = msg->FindInt32("opcode",&opcode);
      if(err == B_OK)
      {
        printf("what:%d\topcode:%d\n",msg->what, opcode);
        switch(opcode)
        {
          case B_ENTRY_CREATED:
          {
            printf("NEW FILE\n");
            entry_ref ref;
            BPath path;
            const char * name;
            msg->FindInt32("device",&ref.device);
            msg->FindInt64("directory",&ref.directory);
            msg->FindString("name",&name);
            ref.set_name(name);
            BEntry new_file = BEntry(&ref);
            new_file.GetPath(&path);
            add_file_to_dropbox(path.Path());
            break;
          }
          case B_ENTRY_MOVED:
          {
            printf("MOVED FILE\n");
            moved_file(msg);
            break;
          }
          case B_ENTRY_REMOVED:
          {
            printf("DELETED FILE\n");
            delete_file_on_dropbox("hi");
            break;
          }
          default:
          {
            printf("default case opcode...\n");
          }
        }
      }
      break;
    }
    default:
    {
      printf("default msg\n");
      BApplication::MessageReceived(msg);
      break;
    }
  }
}

int
main(void)
{
  //Haiku make window code
  App *app = new App();

  app->Run();
  delete app;
  return 0;
}
<|endoftext|>


<commit_before><commit_msg>Fix build break.<commit_after><|endoftext|>


<commit_before>/*
 * Jamoma DSP Wavetable Oscillator
 * Copyright © 2003, Timothy Place
 *
 * License: This code is licensed under the terms of the GNU LGPL
 * http://www.gnu.org/licenses/lgpl.html
 */

#include "TTWavetable.h"

#define thisTTClass			TTWavetable
#define thisTTClassName		"wavetable"
#define thisTTClassTags		"audio, generator, oscillator, buffer"

TT_AUDIO_CONSTRUCTOR,
	mIndex(0.0),
	mIndexDelta(0.0),
	mBuffer(NULL)
{
	TTUInt16	initialMaxNumChannels = arguments;

	addAttributeWithSetter(Frequency,		kTypeFloat64);
	addAttributeWithSetter(Mode,			kTypeSymbol);
	addAttributeWithSetter(Gain,			kTypeFloat64);
	addAttributeWithSetter(Interpolation,	kTypeSymbol);
	addAttributeWithSetter(Size,			kTypeUInt32);

	addUpdates(SampleRate);

	TTObjectInstantiate(TT("buffer"), (TTObjectPtr*)&mBuffer, kTTValNONE);
	if (!mBuffer)
		throw TTException("Could not create internal buffer object");
	mBuffer->setNumChannels(TTUInt32(1));

	// Set Defaults...
	setAttributeValue(TT("maxNumChannels"),	initialMaxNumChannels);
	setAttributeValue(TT("size"), 8192);
	setAttributeValue(TT("mode"), kTTSym_sine);
	setAttributeValue(TT("frequency"), 440.0);
	setAttributeValue(TT("gain"), 0.0);			// 0 dB
	setAttributeValue(TT("interpolation"), TT("linear"));
}

TTWavetable::~TTWavetable()
{
	TTObjectRelease((TTObject**)&mBuffer);
}

TTErr TTWavetable::updateSampleRate(const TTValue&, TTValue&)
{
	setAttributeValue(TT("frequency"), mFrequency);
	return kTTErrNone;
}

TTErr TTWavetable::setFrequency(const TTValue& newValue)
{
	mFrequency = TTClip<TTFloat64>(newValue, 0.0, sr/2.0);
	mIndexDelta = mFrequency * mSize * srInv;
	return kTTErrNone;
}

TTErr TTWavetable::setMode(const TTValue& newValue)
{
	mMode = newValue;	// TODO: should be newValue[0]

	if (mMode != TT("externalBuffer"))
		return mBuffer->fill(newValue, kTTValNONE);
	else {
		// TODO: implement the ability to use an externally defined buffer
		return kTTErrInvalidValue;
	}
}

TTErr TTWavetable::setInterpolation(const TTValue& newValue)
{
	mMode = newValue;
	if (mMode == TT("linear"))
		setProcessMethod(processWithLinearInterpolation);
	else if (mMode == TT("lfo"))
		setProcessMethod(processAsLFO);
	else
		setProcessMethod(processWithNoInterpolation);
	return kTTErrNone;
}

TTErr TTWavetable::setGain(const TTValue& newValue)
{
	mGain = newValue;
	mLinearGain = TTDecibelsToLinearGain(mGain);
	return kTTErrNone;
}

TTErr TTWavetable::setSize(const TTValue& newSize)
{
	mSize = newSize;
	mBuffer->setLengthInSamples(mSize);
	return setFrequency(mFrequency); // touch the frequency so that the step size is updated
}

// LFO mode only updates values once per vector in order to be as fast as possible
// LFO mode cannot be modulated
TTErr TTWavetable::processAsLFO(TTAudioSignalArrayPtr, TTAudioSignalArrayPtr outputs)
{
	TTAudioSignal&	out = outputs->getSignal(0);
	TTSampleValue	tempSample;
	TTUInt16		vs = out.getVectorSizeAsInt();
	TTUInt16		i=0;
	TTUInt16		numChannels = out.getNumChannelsAsInt();
	TTUInt16		channel;
	TTUInt32		p1 = (TTUInt32)mIndex;						// playback index
	TTSampleValue*	contents = NULL;
	TTUInt32		bufferChannelCount;

	mBuffer->getContents(contents);
	mBuffer->lengthInSamples(bufferChannelCount);

	// Move the play head
	mIndex += (mIndexDelta * vs);

	// Wrap the play head
	if (mIndex >= mSize)
		mIndex -= mSize;
	else if (mIndex < 0)
		mIndex += mSize;

	// table lookup (no interpolation)
	// bufferChannelCount is used as the stride
	// thus, if the buffer is mono then we are simply indexing directly, but
	// if the buffer is stereo, then we look at every other sample because the samples are interleaved
	// CURRENTLY: this means this is hard coded to look only at the first channel, and all other channels in the buffer are ignored
	tempSample = contents[p1*bufferChannelCount] * mLinearGain;

	// TODO: in TTBlue 0.2.x this code only assigned the first sample value to save cpu -- should we bring this back as an option?
	while (vs--) {
		for (channel=0; channel<numChannels; channel++)
			out.mSampleVectors[channel][i] = tempSample;
		i++;
	}
	return kTTErrNone;
}

TTErr TTWavetable::processWithNoInterpolation(TTAudioSignalArrayPtr inputs, TTAudioSignalArrayPtr outputs)
{
	TTAudioSignalPtr	in; // can't call getSignal if there is no signal! = inputs->getSignal(0);
	TTAudioSignal&		out = outputs->getSignal(0);
	TTSampleValue		*inSample = NULL;
	TTSampleValue		tempSample;
	TTUInt16			vs = out.getVectorSizeAsInt();
	TTUInt16			i=0;
	TTUInt16			numChannels = out.getNumChannelsAsInt();
	TTUInt16			channel;
	TTBoolean			hasModulation = true;
	TTSampleValuePtr	contents = NULL;
	TTUInt32			bufferChannelCount;

	mBuffer->getContents(contents);
	mBuffer->lengthInSamples(bufferChannelCount);

	// If the input and output signals are the same, then there really isn't an input signal
	// In that case we don't modulate the oscillator with it
	if (inputs->numAudioSignals == 0)
		hasModulation = false;
	else {
		in = &inputs->getSignal(0);
		inSample = in->mSampleVectors[0];
	}
	while (vs--) {
		TTUInt32	p1 = (TTUInt32)mIndex;	// playback index

		// TODO: all of this access of mIndex and mIndexDelta etc is really going to be dereference pointers in our struct/class
		//		 This likely means that the values are not cached (or at least not cached together) in the processors registers
		//		 We should copy these to local variables at the vector start and then copy them back at the vector's end

		// Move the play head
		mIndex += mIndexDelta;

		// Apply modulation to the play head
		if (hasModulation)
			mIndex += *inSample++ * mSize / sr;

		// Wrap the play head
		if (mIndex >= mSize)
			mIndex -= mSize;
		else if (mIndex < 0)
			mIndex += mSize;

		// table lookup (no interpolation)
		tempSample = contents[p1*bufferChannelCount] * mLinearGain;
		for (channel=0; channel<numChannels; channel++)
			out.mSampleVectors[channel][i] = tempSample;
		i++;
	}
	return kTTErrNone;
}

TTErr TTWavetable::processWithLinearInterpolation(TTAudioSignalArrayPtr inputs, TTAudioSignalArrayPtr outputs)
{
	TTAudioSignalPtr	in; // can't call getSignal if there is no signal! = inputs->getSignal(0);
	TTAudioSignal&	out = outputs->getSignal(0);
	TTSampleValue	*inSample = NULL;
	TTSampleValue	tempSample;
	TTUInt16		vs = out.getVectorSizeAsInt();
	TTUInt16		i=0;
	TTUInt16		numChannels = out.getNumChannelsAsInt();
	TTUInt16		channel;
	TTBoolean		hasModulation = true;
	TTUInt32		p1, p2;									// two playback indices
	TTFloat64		diff;
	TTSampleValue*	contents = NULL;
	TTUInt32		bufferChannelCount;

	mBuffer-> getContents(contents);
	mBuffer->lengthInSamples(bufferChannelCount);

	// If the input and output signals are the same, then there really isn't an input signal
	// In that case we don't modulate the oscillator with it
	if (inputs->numAudioSignals == 0)
		hasModulation = false;
	else {
		in = &inputs->getSignal(0);
		inSample = in->mSampleVectors[0];
	}
	while (vs--) {
		// Move the play head
		mIndex += mIndexDelta;

		// Apply modulation to the play head
		if (hasModulation)
			mIndex += *inSample++ * mSize / sr;

		// Wrap the play head
		if (mIndex >= mSize)
			mIndex -= mSize;
		else if (mIndex < 0)
			mIndex += mSize;

		// table lookup (linear interpolation)
		p1 = (TTUInt32)mIndex;
		p2 = p1 + 1;
		diff = mIndex - p1;
		p2 &= (mSize - 1);	// fast modulo

		tempSample = ((contents[p2*bufferChannelCount] * diff) + (contents[p1*bufferChannelCount] * (1.0 - diff))) * mLinearGain;
		for (channel=0; channel<numChannels; channel++)
			out.mSampleVectors[channel][i] = tempSample;
		i++;
	}
	return kTTErrNone;
}
<commit_msg>Wavetable: first try at checkOut/In of SampleMatrixPtr, compiles but crashes<commit_after>/*
 * Jamoma DSP Wavetable Oscillator
 * Copyright © 2003, Timothy Place
 *
 * License: This code is licensed under the terms of the GNU LGPL
 * http://www.gnu.org/licenses/lgpl.html
 */

#include "TTWavetable.h"

#define thisTTClass			TTWavetable
#define thisTTClassName		"wavetable"
#define thisTTClassTags		"audio, generator, oscillator, buffer"

TT_AUDIO_CONSTRUCTOR,
	mIndex(0.0),
	mIndexDelta(0.0),
	mBuffer(NULL)
{
	TTUInt16	initialMaxNumChannels = arguments;

	addAttributeWithSetter(Frequency,		kTypeFloat64);
	addAttributeWithSetter(Mode,			kTypeSymbol);
	addAttributeWithSetter(Gain,			kTypeFloat64);
	addAttributeWithSetter(Interpolation,	kTypeSymbol);
	addAttributeWithSetter(Size,			kTypeUInt32);

	addUpdates(SampleRate);

	TTObjectInstantiate(TT("buffer"), (TTObjectPtr*)&mBuffer, kTTValNONE);
	if (!mBuffer)
		throw TTException("Could not create internal buffer object");
	mBuffer->setNumChannels(TTUInt32(1));

	// Set Defaults...
	setAttributeValue(TT("maxNumChannels"),	initialMaxNumChannels);
	setAttributeValue(TT("size"), 8192);
	setAttributeValue(TT("mode"), kTTSym_sine);
	setAttributeValue(TT("frequency"), 440.0);
	setAttributeValue(TT("gain"), 0.0);			// 0 dB
	setAttributeValue(TT("interpolation"), TT("linear"));
}

TTWavetable::~TTWavetable()
{
	TTObjectRelease((TTObject**)&mBuffer);
}

TTErr TTWavetable::updateSampleRate(const TTValue&, TTValue&)
{
	setAttributeValue(TT("frequency"), mFrequency);
	return kTTErrNone;
}

TTErr TTWavetable::setFrequency(const TTValue& newValue)
{
	mFrequency = TTClip<TTFloat64>(newValue, 0.0, sr/2.0);
	mIndexDelta = mFrequency * mSize * srInv;
	return kTTErrNone;
}

TTErr TTWavetable::setMode(const TTValue& newValue)
{
	mMode = newValue;	// TODO: should be newValue[0]

	if (mMode != TT("externalBuffer"))
		return mBuffer->fill(newValue, kTTValNONE);
	else {
		// TODO: implement the ability to use an externally defined buffer
		return kTTErrInvalidValue;
	}
}

TTErr TTWavetable::setInterpolation(const TTValue& newValue)
{
	mMode = newValue;
	if (mMode == TT("linear"))
		setProcessMethod(processWithLinearInterpolation);
	else if (mMode == TT("lfo"))
		setProcessMethod(processAsLFO);
	else
		setProcessMethod(processWithNoInterpolation);
	return kTTErrNone;
}

TTErr TTWavetable::setGain(const TTValue& newValue)
{
	mGain = newValue;
	mLinearGain = TTDecibelsToLinearGain(mGain);
	return kTTErrNone;
}

TTErr TTWavetable::setSize(const TTValue& newSize)
{
	mSize = newSize;
	mBuffer->setLengthInSamples(mSize);
	return setFrequency(mFrequency); // touch the frequency so that the step size is updated
}

// LFO mode only updates values once per vector in order to be as fast as possible
// LFO mode cannot be modulated
TTErr TTWavetable::processAsLFO(TTAudioSignalArrayPtr, TTAudioSignalArrayPtr outputs)
{
	TTAudioSignal&	out = outputs->getSignal(0);
	TTSampleValue	tempSample;
	TTUInt16		vs = out.getVectorSizeAsInt();
	TTUInt16		i=0;
	TTUInt16		numChannels = out.getNumChannelsAsInt();
	TTUInt16		channel;
	TTUInt64		p1 = (TTUInt64)mIndex;						// playback index
	TTSampleMatrixPtr	contents = NULL;

	mBuffer->checkOutMatrix(contents);

	// Move the play head
	mIndex += (mIndexDelta * vs);

	// Wrap the play head
	if (mIndex >= mSize)
		mIndex -= mSize;
	else if (mIndex < 0)
		mIndex += mSize;

	// table lookup (no interpolation)
	// CURRENTLY: this is hard coded to look only at the first channel, and all other channels in the buffer are ignored
	contents->peek(p1,0,tempSample);
	tempSample *= mLinearGain;

	// TODO: in TTBlue 0.2.x this code only assigned the first sample value to save cpu -- should we bring this back as an option?
	while (vs--) {
		for (channel=0; channel<numChannels; channel++)
			out.mSampleVectors[channel][i] = tempSample;
		i++;
	}
	mBuffer->checkInMatrix(contents);
	return kTTErrNone;
}

TTErr TTWavetable::processWithNoInterpolation(TTAudioSignalArrayPtr inputs, TTAudioSignalArrayPtr outputs)
{
	TTAudioSignalPtr	in; // can't call getSignal if there is no signal! = inputs->getSignal(0);
	TTAudioSignal&		out = outputs->getSignal(0);
	TTSampleValue		*inSample = NULL;
	TTSampleValue		tempSample;
	TTUInt16			vs = out.getVectorSizeAsInt();
	TTUInt16			i=0;
	TTUInt16			numChannels = out.getNumChannelsAsInt();
	TTUInt16			channel;
	TTBoolean			hasModulation = true;
	TTSampleMatrixPtr	contents = NULL;

	mBuffer->checkOutMatrix(contents);

	// If the input and output signals are the same, then there really isn't an input signal
	// In that case we don't modulate the oscillator with it
	if (inputs->numAudioSignals == 0)
		hasModulation = false;
	else {
		in = &inputs->getSignal(0);
		inSample = in->mSampleVectors[0];
	}
	while (vs--) {
		TTUInt64	p1 = (TTUInt64)mIndex;	// playback index

		// TODO: all of this access of mIndex and mIndexDelta etc is really going to be dereference pointers in our struct/class
		//		 This likely means that the values are not cached (or at least not cached together) in the processors registers
		//		 We should copy these to local variables at the vector start and then copy them back at the vector's end

		// Move the play head
		mIndex += mIndexDelta;

		// Apply modulation to the play head
		if (hasModulation)
			mIndex += *inSample++ * mSize / sr;

		// Wrap the play head
		if (mIndex >= mSize)
			mIndex -= mSize;
		else if (mIndex < 0)
			mIndex += mSize;

		// table lookup (no interpolation)
		// CURRENTLY: this is hard coded to look only at the first channel, and all other channels in the buffer are ignored
		contents->peek(p1,0,tempSample);
		tempSample *= mLinearGain;
		for (channel=0; channel<numChannels; channel++)
			out.mSampleVectors[channel][i] = tempSample;
		i++;
	}
	mBuffer->checkInMatrix(contents);
	return kTTErrNone;
}

TTErr TTWavetable::processWithLinearInterpolation(TTAudioSignalArrayPtr inputs, TTAudioSignalArrayPtr outputs)
{
	TTAudioSignalPtr	in; // can't call getSignal if there is no signal! = inputs->getSignal(0);
	TTAudioSignal&	out = outputs->getSignal(0);
	TTSampleValue	*inSample = NULL;
	TTSampleValue	tempSample;
	TTUInt16		vs = out.getVectorSizeAsInt();
	TTUInt16		i=0;
	TTUInt16		numChannels = out.getNumChannelsAsInt();
	TTUInt16		channel;
	TTBoolean		hasModulation = true;
	TTUInt32		p1, p2;									// two playback indices
	TTFloat64		diff;
	TTSampleMatrixPtr	contents = NULL;

	mBuffer->checkOutMatrix(contents);

	// If the input and output signals are the same, then there really isn't an input signal
	// In that case we don't modulate the oscillator with it
	if (inputs->numAudioSignals == 0)
		hasModulation = false;
	else {
		in = &inputs->getSignal(0);
		inSample = in->mSampleVectors[0];
	}
	while (vs--) {
		// Move the play head
		mIndex += mIndexDelta;

		// Apply modulation to the play head
		if (hasModulation)
			mIndex += *inSample++ * mSize / sr;

		// Wrap the play head
		if (mIndex >= mSize)
			mIndex -= mSize;
		else if (mIndex < 0)
			mIndex += mSize;

		// table lookup (linear interpolation)
		// CURRENTLY: this is hard coded to look only at the first channel, and all other channels in the buffer are ignored
		contents->peek(mIndex,0,tempSample);
		tempSample *= mLinearGain;
		for (channel=0; channel<numChannels; channel++)
			out.mSampleVectors[channel][i] = tempSample;
		i++;
	}
	mBuffer->checkInMatrix(contents);
	return kTTErrNone;
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2010 Telmo Menezes.
 * telmo@telmomenezes.com
 */

#include "gpgenerator.h"
#include "node.h"
#include "utils.h"
#include "distmatrix.h"

#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <sstream>

using std::cout;
using std::endl;

using namespace std;

namespace syn
{

GPGenerator::GPGenerator()
{
    edges = 0;
    cycle = 0;
    prog_origin = new GPTree(4);
    prog_origin->init_random(0.2, 2, 5);
    prog_target = new GPTree(8);
    prog_target->init_random(0.2, 2, 5);
}

GPGenerator::~GPGenerator()
{
    delete prog_origin;
    delete prog_target;
}

GPGenerator* GPGenerator::clone()
{
    GPGenerator* gen_clone = new GPGenerator();

    gen_clone->edges = 0;
    gen_clone->cycle = 0;
    gen_clone->prog_origin = prog_origin->clone();
    gen_clone->prog_target = prog_target->clone();

    return gen_clone;
}

Net* GPGenerator::run(unsigned int node_count, unsigned int edge_count)
{
    // init DistMatrix
    DistMatrix::get_instance().set_nodes(node_count);

    Net* net = new Net();

    cycle = 0;

    // create nodes
    for (unsigned int i = 0; i < node_count; i++) {
        net->add_node_with_id(i, 0);
    }

    // create edges
    Node* orig_node;
    Node* targ_node;
    double total_weight;
    double weight;

    double po, pt, io, oo, it, ot, t, ao, at, r, d;

    for (unsigned int i = 0; i < edge_count; i++) {
        total_weight = 0;
        orig_node = net->get_nodes();
        while (orig_node) {
            po = (double)orig_node->id;
            io = oo = t = ao = at = 0;
            if (edges > 0) {
                io = (double)orig_node->in_degree;
                oo = (double)orig_node->out_degree;
                t = (double)cycle;
            }

            /*
            if (orig_node->birth >= 0) {
                ao = (double)(cycle - orig_node->birth);
            }
            */

            prog_origin->vars[0] = po;
            prog_origin->vars[1] = io;
            prog_origin->vars[2] = oo;
            prog_origin->vars[3] = t;
            //prog_origin->vars[4] = ao;
            weight = prog_origin->eval();
            if (weight < 0) {
                weight = 0;
            }

            orig_node->genweight = weight;
            total_weight += weight;

            orig_node = orig_node->next;
        }

        // if total weight is zero, make every node's weight = 1
        if (total_weight == 0) {
            orig_node = net->get_nodes();
            while (orig_node) {
                orig_node->genweight = 1.0;
                total_weight += 1.0;
                orig_node = orig_node->next;
            }
        }

        weight = RANDOM_UNIFORM * total_weight;
        orig_node = net->get_nodes();
        total_weight = orig_node->genweight;
        //printf("w: %f; tw: %f\n", weight, total_weight);
        while (total_weight < weight) {
            orig_node = orig_node->next;
            total_weight += orig_node->genweight;
        }

        total_weight = 0;
        targ_node = net->get_nodes();
        while (targ_node) {
            po = (double)orig_node->id;
            pt = (double)targ_node->id;

            io = oo = it = ot = t = ao = at = 0;

            if (edges > 0) {
                io = (double)orig_node->in_degree;
                oo = (double)orig_node->out_degree;
                it = (double)targ_node->in_degree;
                ot = (double)targ_node->out_degree;
                t = (double)cycle;
            }

            unsigned int dist = DistMatrix::get_instance().get_udistance(orig_node->id, targ_node->id);
            if (dist > 0) {
                d = 1.0 / ((double)dist);
            }
            // lim d->inf 1/d
            else {
                d = 0;
            }

            /*
            if (orig_node->birth >= 0) {
                ao = (double)(cycle - orig_node->birth);
            }
            if (targ_node->birth >= 0) {
                at = (double)(cycle - targ_node->birth);
            }
            */

            //cout << "ages: " << ao << "; " << at << endl;

            //r = targ_node->edge_exists(orig_node);

            prog_target->vars[0] = po;
            prog_target->vars[1] = io;
            prog_target->vars[2] = oo;
            prog_target->vars[3] = t;
            prog_target->vars[4] = pt;
            prog_target->vars[5] = it;
            prog_target->vars[6] = ot;
            prog_target->vars[7] = d;
            //prog_target->vars[7] = ao;
            //prog_target->vars[8] = at;
            //prog_target->vars[9] = r;
            weight = prog_target->eval();
            if (weight < 0) {
                weight = 0;
            }
            //printf("weight: %f; po: %f; pt: %f; io: %f; oo: %f; it: %f; ot: %f; ep: %f\n", weight, po, pt, io, oo, it, ot, ep);

            //if ((orig_node == targ_node) || orig_node->edge_exists(targ_node)){
            if (orig_node == targ_node) {
                weight = 0;
            }

            targ_node->genweight = weight;
            total_weight += weight;

            targ_node = targ_node->next;
        }

        // if total weight is zero, make every node's weight = 1
        if (total_weight == 0) {
            targ_node = net->get_nodes();
            while (targ_node) {
                targ_node->genweight = 1.0;
                total_weight += 1.0;
                targ_node = targ_node->next;
            }
        }

        weight = RANDOM_UNIFORM * total_weight;
        targ_node = net->get_nodes();
        total_weight = targ_node->genweight;
        while (total_weight < weight) {
            targ_node = targ_node->next;
            total_weight += targ_node->genweight;
        }

        // set ages
        if (orig_node->birth < 0) {
            orig_node->birth = cycle;
        }
        if (targ_node->birth < 0) {
            targ_node->birth = cycle;
        }

        net->add_edge_to_net(orig_node, targ_node, cycle);
        // update distances
        DistMatrix::get_instance().update_distances(orig_node->id, targ_node->id);

        cycle++;
    }

    return net;
}

void GPGenerator::write(string file_path)
{
    ofstream f;
    f.open(file_path.c_str());

    f << "#PROG ORIGIN" << endl;
    f << prog_origin->to_string();
    f << endl << "#PROG TARGET" << endl;
    f << prog_target->to_string();

    f.close();
}

GPGenerator* GPGenerator::recombine(GPGenerator* gen)
{
    GPGenerator* child = new GPGenerator();

    edges = 0;
    cycle = 0;
    switch(random() % 3) {
    case 0:
        child->prog_origin = prog_origin->recombine(gen->prog_origin);
        child->prog_target = prog_target->clone();
        break;
    case 1:
        child->prog_origin = prog_origin->clone();
        child->prog_target = prog_target->recombine(gen->prog_target);
        break;
    case 2:
        child->prog_origin = prog_origin->recombine(gen->prog_origin);
        child->prog_target = prog_target->recombine(gen->prog_target);
        break;
    }

    return child;
}

void GPGenerator::load(string filepath)
{
    std::ifstream file(filepath.c_str());
    string line;

    std::getline(file, line);

    for (int i = 0; i < 2; i++) {
        while ((line.compare("") == 0)
                || (line[0] == '\n')
                || (line[0] == '#')) {
            std::getline(file, line);
        }

        std::stringstream prog;
        while (line.compare("")
                && (line[0] != '\n')
                && (line[0] != '#')) {
            prog << line;
            if (!std::getline(file, line))
                break;
        }

        if (i == 0) {
            prog_origin->parse(prog.str());
        }
        else {
            prog_target->parse(prog.str());
        }
    }
}

void GPGenerator::simplify() {
    prog_origin->dyn_pruning();
    prog_target->dyn_pruning();
}

}
<commit_msg>adding undirected distance to gpgen variables<commit_after>/*
 * Copyright (C) 2010 Telmo Menezes.
 * telmo@telmomenezes.com
 */

#include "gpgenerator.h"
#include "node.h"
#include "utils.h"
#include "distmatrix.h"

#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <sstream>

using std::cout;
using std::endl;

using namespace std;

namespace syn
{

GPGenerator::GPGenerator()
{
    edges = 0;
    cycle = 0;
    prog_origin = new GPTree(4);
    prog_origin->init_random(0.2, 2, 5);
    prog_target = new GPTree(9);
    prog_target->init_random(0.2, 2, 5);
}

GPGenerator::~GPGenerator()
{
    delete prog_origin;
    delete prog_target;
}

GPGenerator* GPGenerator::clone()
{
    GPGenerator* gen_clone = new GPGenerator();

    gen_clone->edges = 0;
    gen_clone->cycle = 0;
    gen_clone->prog_origin = prog_origin->clone();
    gen_clone->prog_target = prog_target->clone();

    return gen_clone;
}

Net* GPGenerator::run(unsigned int node_count, unsigned int edge_count)
{
    // init DistMatrix
    DistMatrix::get_instance().set_nodes(node_count);

    Net* net = new Net();

    cycle = 0;

    // create nodes
    for (unsigned int i = 0; i < node_count; i++) {
        net->add_node_with_id(i, 0);
    }

    // create edges
    Node* orig_node;
    Node* targ_node;
    double total_weight;
    double weight;

    double po, pt, io, oo, it, ot, t, ud, dd;

    for (unsigned int i = 0; i < edge_count; i++) {
        total_weight = 0;
        orig_node = net->get_nodes();
        while (orig_node) {
            po = (double)orig_node->id;
            io = oo = t = 0;
            if (edges > 0) {
                io = (double)orig_node->in_degree;
                oo = (double)orig_node->out_degree;
                t = (double)cycle;
            }

            prog_origin->vars[0] = po;
            prog_origin->vars[1] = io;
            prog_origin->vars[2] = oo;
            prog_origin->vars[3] = t;
            weight = prog_origin->eval();
            if (weight < 0) {
                weight = 0;
            }

            orig_node->genweight = weight;
            total_weight += weight;

            orig_node = orig_node->next;
        }

        // if total weight is zero, make every node's weight = 1
        if (total_weight == 0) {
            orig_node = net->get_nodes();
            while (orig_node) {
                orig_node->genweight = 1.0;
                total_weight += 1.0;
                orig_node = orig_node->next;
            }
        }

        weight = RANDOM_UNIFORM * total_weight;
        orig_node = net->get_nodes();
        total_weight = orig_node->genweight;
        //printf("w: %f; tw: %f\n", weight, total_weight);
        while (total_weight < weight) {
            orig_node = orig_node->next;
            total_weight += orig_node->genweight;
        }

        total_weight = 0;
        targ_node = net->get_nodes();
        while (targ_node) {
            po = (double)orig_node->id;
            pt = (double)targ_node->id;

            io = oo = it = ot = t = 0;

            if (edges > 0) {
                io = (double)orig_node->in_degree;
                oo = (double)orig_node->out_degree;
                it = (double)targ_node->in_degree;
                ot = (double)targ_node->out_degree;
                t = (double)cycle;
            }

            unsigned int dist = DistMatrix::get_instance().get_udistance(orig_node->id, targ_node->id);
            if (dist > 0) {
                ud = 1.0 / ((double)dist);
            }
            // lim d->inf 1/d
            else {
                ud = 0;
            }

            dist = DistMatrix::get_instance().get_ddistance(orig_node->id, targ_node->id);
            if (dist > 0) {
                dd = 1.0 / ((double)dist);
            }
            // lim d->inf 1/d
            else {
                dd = 0;
            }

            prog_target->vars[0] = po;
            prog_target->vars[1] = io;
            prog_target->vars[2] = oo;
            prog_target->vars[3] = t;
            prog_target->vars[4] = pt;
            prog_target->vars[5] = it;
            prog_target->vars[6] = ot;
            prog_target->vars[7] = ud;
            prog_target->vars[8] = dd;

            weight = prog_target->eval();
            if (weight < 0) {
                weight = 0;
            }
            //printf("weight: %f; po: %f; pt: %f; io: %f; oo: %f; it: %f; ot: %f; ep: %f\n", weight, po, pt, io, oo, it, ot, ep);

            //if ((orig_node == targ_node) || orig_node->edge_exists(targ_node)){
            if (orig_node == targ_node) {
                weight = 0;
            }

            targ_node->genweight = weight;
            total_weight += weight;

            targ_node = targ_node->next;
        }

        // if total weight is zero, make every node's weight = 1
        if (total_weight == 0) {
            targ_node = net->get_nodes();
            while (targ_node) {
                targ_node->genweight = 1.0;
                total_weight += 1.0;
                targ_node = targ_node->next;
            }
        }

        weight = RANDOM_UNIFORM * total_weight;
        targ_node = net->get_nodes();
        total_weight = targ_node->genweight;
        while (total_weight < weight) {
            targ_node = targ_node->next;
            total_weight += targ_node->genweight;
        }

        // set ages
        if (orig_node->birth < 0) {
            orig_node->birth = cycle;
        }
        if (targ_node->birth < 0) {
            targ_node->birth = cycle;
        }

        net->add_edge_to_net(orig_node, targ_node, cycle);
        // update distances
        DistMatrix::get_instance().update_distances(orig_node->id, targ_node->id);

        cycle++;
    }

    return net;
}

void GPGenerator::write(string file_path)
{
    ofstream f;
    f.open(file_path.c_str());

    f << "#PROG ORIGIN" << endl;
    f << prog_origin->to_string();
    f << endl << "#PROG TARGET" << endl;
    f << prog_target->to_string();

    f.close();
}

GPGenerator* GPGenerator::recombine(GPGenerator* gen)
{
    GPGenerator* child = new GPGenerator();

    edges = 0;
    cycle = 0;
    switch(random() % 3) {
    case 0:
        child->prog_origin = prog_origin->recombine(gen->prog_origin);
        child->prog_target = prog_target->clone();
        break;
    case 1:
        child->prog_origin = prog_origin->clone();
        child->prog_target = prog_target->recombine(gen->prog_target);
        break;
    case 2:
        child->prog_origin = prog_origin->recombine(gen->prog_origin);
        child->prog_target = prog_target->recombine(gen->prog_target);
        break;
    }

    return child;
}

void GPGenerator::load(string filepath)
{
    std::ifstream file(filepath.c_str());
    string line;

    std::getline(file, line);

    for (int i = 0; i < 2; i++) {
        while ((line.compare("") == 0)
                || (line[0] == '\n')
                || (line[0] == '#')) {
            std::getline(file, line);
        }

        std::stringstream prog;
        while (line.compare("")
                && (line[0] != '\n')
                && (line[0] != '#')) {
            prog << line;
            if (!std::getline(file, line))
                break;
        }

        if (i == 0) {
            prog_origin->parse(prog.str());
        }
        else {
            prog_target->parse(prog.str());
        }
    }
}

void GPGenerator::simplify() {
    prog_origin->dyn_pruning();
    prog_target->dyn_pruning();
}

}
<|endoftext|>


<commit_before>/***************************************************************************
 *   Copyright (C) 2003 by Jonathan Singer                                             *
 *   jsinger@leeta.net                                                                                *
 *   Calendar routines from Hebrew Calendar by Frank Yellin                     *
 *                                                                                *
 *   This program is free software; you can redistribute it and/or modify      *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or      *
 *   (at your option) any later version.                                                       *
 ***************************************************************************/
#include "converter.h"
#include <klocale.h>

Converter::Converter()
{

}

Converter::~Converter()
{
}

long Converter::absolute_from_gregorian(int year, int month, int day)
{
  int xyear, day_number;

  xyear = year - 1;
  day_number = day + 31 * (month - 1);
  if (month > 2)
      {
        day_number -= (23 + (4 * month)) / 10;
        if (gregorian_leap_year_p(year))
          day_number++;
      }
  return day_number +     /* the day number within the current year */
    365L * xyear +        /* days in prior years */
    (xyear / 4) +         /* Julian leap years */
    (-(xyear / 100)) +    /* deduct century years */
    (xyear / 400);        /* add Gregorian leap years */
}

/* Given a Hebrew date, calculate the number of days since
 * January 0, 0001, Gregorian
 */
long Converter::absolute_from_hebrew(int year, int month, int day)
{
  long sum = day + hebrew_elapsed_days(year) - 1373429L;
  int i;

  if (month < 7)
      {
        int months = hebrew_months_in_year(year);

        for (i = 7; i <= months; ++i)
          sum += hebrew_month_length(year, i);
        for (i = 1; i < month; ++i)
          sum += hebrew_month_length(year, i);
      }
  else
      {
        for (i = 7; i < month; ++i)
          sum += hebrew_month_length(year, i);
      }
  return sum;
}

/* Given an absolute date, calculate the gregorian date  */
void
  Converter::gregorian_from_absolute(long date, int *yearp,
                                     int *monthp, int *dayp)
{
  int year, month, day;

  for (year = date / 366;
       date >= absolute_from_gregorian(year + 1, 1, 1); ++year) ;
  for (month = 1;
       (month <= 11)
       && (date >= absolute_from_gregorian(year, 1 + month, 1));
       ++month ) ;
  day = 1 + date - absolute_from_gregorian(year, month, 1);
  *yearp = year;
  *monthp = month;
  *dayp = day;
}

/* Given an absolute date, calculate the Hebrew date */
void
  Converter::hebrew_from_absolute(long date, int *yearp, int *monthp,
                                  int *dayp)
{
  int year, month, day, gyear, gmonth, gday, months;

  gregorian_from_absolute(date, &gyear, &gmonth, &gday);
  year = gyear + 3760;
  while (date >= absolute_from_hebrew(1 + year, 7, 1))
    year++;
  months = hebrew_months_in_year(year);
  for (month = 7;
       date > absolute_from_hebrew(year, month,
                                   hebrew_month_length(year, month));
       month = 1 + (month % months)) ;
  day = 1 + date - absolute_from_hebrew(year, month, 1);
  *yearp = year;
  *monthp = month;
  *dayp = day;
}

/* Number of months in a Hebrew year */
int Converter::hebrew_months_in_year(int year)
{
  if (hebrew_leap_year_p(year))
    return 13;
  else
    return 12;
}

enum
{ Nissan =
    1, Iyar, Sivan, Tamuz, Ab, Elul, Tishrei, Cheshvan, Kislev, Tevet,
  Shvat, Adar, AdarII, AdarI = 12
};

enum
{ January =
    1, February, March, April, May, June, July, August, September,
  October, November, December
};

/* Number of days in a Hebrew month */
int Converter::hebrew_month_length(int year, int month)
{
  switch (month)
      {
      case Tishrei:
      case Shvat:
      case Nissan:
      case Sivan:
      case Ab:
        return 30;

      case Tevet:
      case Iyar:
      case Tamuz:
      case Elul:
      case AdarII:
        return 29;

      case Cheshvan:
        // 29 days, unless it's a long year.
        if ((hebrew_year_length(year) % 10) == 5)
          return 30;
        else
          return 29;

      case Kislev:
        // 30 days, unless it's a short year.
        if ((hebrew_year_length(year) % 10) == 3)
          return 29;
        else
          return 30;

      case Adar:
        // Adar (non-leap year) has 29 days.  Adar I has 30 days.
        if (hebrew_leap_year_p(year))
          return 30;
        else
          return 29;

      default:
        return 0;
      }
}

/* Number of days in a Julian or gregorian month */
int
  Converter::secular_month_length(int year,
                                  int month /*, bool julianp */ )
{
  switch (month)
      {
      case January:
      case March:
      case May:
      case July:
      case August:
      case October:
      case December:
        return 31;
      case April:
      case June:
      case September:
      case November:
        return 30;
      case February:
        if (gregorian_leap_year_p(year))
          return 29;
        else
          return 28;
      default:
        return 0;
      }
}

/* Is it a Leap year in the gregorian calendar */
bool Converter::gregorian_leap_year_p(int year)
{
  if ((year % 4) != 0)
    return 0;
  if ((year % 400) == 0)
    return 1;
  if ((year % 100) == 0)
    return 0;
  return 1;
}

/* Is it a leap year in the Jewish Calendar */
bool Converter::hebrew_leap_year_p(int year)
{
  switch (year % 19)
      {
      case 0:
      case 3:
      case 6:
      case 8:
      case 11:
      case 14:
      case 17:
        return 1;
      default:
        return 0;
      }
}

/* Return the number of days from 1 Tishrei 0001 to the beginning of the given year.
 * Since this routine gets called frequently with the same year arguments, we cache
 * the most recent values.
 */
#define MEMORY 5
long Converter::hebrew_elapsed_days(int year)
{
  static int saved_year[MEMORY] = { -1, -1, -1, -1, -1 };
  static long saved_value[MEMORY];
  int i;

  for (i = 0; i < MEMORY; ++i)
    if (year == saved_year[i])
      return saved_value[i];
  for (i = 0; i < MEMORY; ++i)
    saved_year[i] = saved_year[1 + i], saved_value[i] =
      saved_value[1 + i];
  saved_year[MEMORY - 1] = year;
  saved_value[MEMORY - 1] = hebrew_elapsed_days2(year);
  return saved_value[MEMORY - 1];
}

long Converter::hebrew_elapsed_days2(int year)
{
  long prev_year = year - 1;
  long months_elapsed = 235L * (prev_year / 19)  /* months in complete cycles so far */
    + 12L * (prev_year % 19)                     /* regular months in this cycle */
    + (((prev_year % 19) * 7 + 1) / 19);         /* leap months this cycle */
  long parts_elapsed = 5604 + 13753 * months_elapsed;
  long day = 1 + 29 * months_elapsed + parts_elapsed / 25920;
  long parts = parts_elapsed % 25920;
  int weekday = (day % 7);
  long alt_day = ((parts >= 19440)
                  || (weekday == 2 && (parts >= 9924)
                      && !hebrew_leap_year_p(year)) || (weekday == 1
                                                        && (parts >=
                                                            16789)
                                                        &&
                                                        hebrew_leap_year_p
                                                        (prev_year)))
    ? day + 1 : day;
  switch (alt_day % 7)
      {
      case 0:
      case 3:
      case 5:
        return 1 + alt_day;
      default:
        return alt_day;
      }
}

/* Number of days in the given Hebrew year */
int Converter::hebrew_year_length(int year)
{
  return hebrew_elapsed_days(1 + year) - hebrew_elapsed_days(year);
}

/* Fill in the DateResult structure based on the given secular date */
void
  Converter::SecularToHebrewConversion(int syear, int smonth,
                                       int sday,
                                       struct DateResult *result)
{
  int hyear, hmonth, hday;
  long absolute;

  absolute = absolute_from_gregorian(syear, smonth, sday);

  hebrew_from_absolute(absolute, &hyear, &hmonth, &hday);

  result->year = hyear;
  result->month = hmonth;
  result->day = hday;
  finish_up(absolute, hyear, hmonth, syear, smonth, result);
}

/* Fill in the DateResult structure based on the given Hebrew date */
void
  Converter::HebrewToSecularConversion(int hyear, int hmonth,
                                       int hday,
                                       struct DateResult *result)
{
  int syear, smonth, sday;
  long absolute;

  absolute = absolute_from_hebrew(hyear, hmonth, hday);
  gregorian_from_absolute(absolute, &syear, &smonth, &sday);
  result->year = hyear;
  result->month = hmonth;
  result->day = hday;
  finish_up(absolute, hyear, hmonth, syear, smonth, result);
}

/* This is common code for filling up the DateResult structure */
void
  Converter::finish_up(long absolute, int hyear, int hmonth,
                       int syear, int smonth,
                       struct DateResult *result)
{
  result->hebrew_month_length = hebrew_month_length(hyear, hmonth);
  result->secular_month_length = secular_month_length(syear, smonth);
  result->hebrew_leap_year_p = hebrew_leap_year_p(hyear);
  result->secular_leap_year_p = gregorian_leap_year_p(syear);
  result->kvia = (hebrew_year_length(hyear) % 10) - 3;
  // absolute is -1 on 1/1/0001 Julian
  result->day_of_week = (7 + absolute) % 7;
  result->hebrew_day_number =
    absolute - absolute_from_hebrew(hyear, 7, 1) + 1;

}
<commit_msg>fix buffer overflow (CID 1375)<commit_after>/***************************************************************************
 *   Copyright (C) 2003 by Jonathan Singer                                             *
 *   jsinger@leeta.net                                                                                *
 *   Calendar routines from Hebrew Calendar by Frank Yellin                     *
 *                                                                                *
 *   This program is free software; you can redistribute it and/or modify      *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or      *
 *   (at your option) any later version.                                                       *
 ***************************************************************************/
#include "converter.h"
#include <klocale.h>

Converter::Converter()
{

}

Converter::~Converter()
{
}

long Converter::absolute_from_gregorian(int year, int month, int day)
{
  int xyear, day_number;

  xyear = year - 1;
  day_number = day + 31 * (month - 1);
  if (month > 2)
      {
        day_number -= (23 + (4 * month)) / 10;
        if (gregorian_leap_year_p(year))
          day_number++;
      }
  return day_number +     /* the day number within the current year */
    365L * xyear +        /* days in prior years */
    (xyear / 4) +         /* Julian leap years */
    (-(xyear / 100)) +    /* deduct century years */
    (xyear / 400);        /* add Gregorian leap years */
}

/* Given a Hebrew date, calculate the number of days since
 * January 0, 0001, Gregorian
 */
long Converter::absolute_from_hebrew(int year, int month, int day)
{
  long sum = day + hebrew_elapsed_days(year) - 1373429L;
  int i;

  if (month < 7)
      {
        int months = hebrew_months_in_year(year);

        for (i = 7; i <= months; ++i)
          sum += hebrew_month_length(year, i);
        for (i = 1; i < month; ++i)
          sum += hebrew_month_length(year, i);
      }
  else
      {
        for (i = 7; i < month; ++i)
          sum += hebrew_month_length(year, i);
      }
  return sum;
}

/* Given an absolute date, calculate the gregorian date  */
void
  Converter::gregorian_from_absolute(long date, int *yearp,
                                     int *monthp, int *dayp)
{
  int year, month, day;

  for (year = date / 366;
       date >= absolute_from_gregorian(year + 1, 1, 1); ++year) ;
  for (month = 1;
       (month <= 11)
       && (date >= absolute_from_gregorian(year, 1 + month, 1));
       ++month ) ;
  day = 1 + date - absolute_from_gregorian(year, month, 1);
  *yearp = year;
  *monthp = month;
  *dayp = day;
}

/* Given an absolute date, calculate the Hebrew date */
void
  Converter::hebrew_from_absolute(long date, int *yearp, int *monthp,
                                  int *dayp)
{
  int year, month, day, gyear, gmonth, gday, months;

  gregorian_from_absolute(date, &gyear, &gmonth, &gday);
  year = gyear + 3760;
  while (date >= absolute_from_hebrew(1 + year, 7, 1))
    year++;
  months = hebrew_months_in_year(year);
  for (month = 7;
       date > absolute_from_hebrew(year, month,
                                   hebrew_month_length(year, month));
       month = 1 + (month % months)) ;
  day = 1 + date - absolute_from_hebrew(year, month, 1);
  *yearp = year;
  *monthp = month;
  *dayp = day;
}

/* Number of months in a Hebrew year */
int Converter::hebrew_months_in_year(int year)
{
  if (hebrew_leap_year_p(year))
    return 13;
  else
    return 12;
}

enum
{ Nissan =
    1, Iyar, Sivan, Tamuz, Ab, Elul, Tishrei, Cheshvan, Kislev, Tevet,
  Shvat, Adar, AdarII, AdarI = 12
};

enum
{ January =
    1, February, March, April, May, June, July, August, September,
  October, November, December
};

/* Number of days in a Hebrew month */
int Converter::hebrew_month_length(int year, int month)
{
  switch (month)
      {
      case Tishrei:
      case Shvat:
      case Nissan:
      case Sivan:
      case Ab:
        return 30;

      case Tevet:
      case Iyar:
      case Tamuz:
      case Elul:
      case AdarII:
        return 29;

      case Cheshvan:
        // 29 days, unless it's a long year.
        if ((hebrew_year_length(year) % 10) == 5)
          return 30;
        else
          return 29;

      case Kislev:
        // 30 days, unless it's a short year.
        if ((hebrew_year_length(year) % 10) == 3)
          return 29;
        else
          return 30;

      case Adar:
        // Adar (non-leap year) has 29 days.  Adar I has 30 days.
        if (hebrew_leap_year_p(year))
          return 30;
        else
          return 29;

      default:
        return 0;
      }
}

/* Number of days in a Julian or gregorian month */
int
  Converter::secular_month_length(int year,
                                  int month /*, bool julianp */ )
{
  switch (month)
      {
      case January:
      case March:
      case May:
      case July:
      case August:
      case October:
      case December:
        return 31;
      case April:
      case June:
      case September:
      case November:
        return 30;
      case February:
        if (gregorian_leap_year_p(year))
          return 29;
        else
          return 28;
      default:
        return 0;
      }
}

/* Is it a Leap year in the gregorian calendar */
bool Converter::gregorian_leap_year_p(int year)
{
  if ((year % 4) != 0)
    return 0;
  if ((year % 400) == 0)
    return 1;
  if ((year % 100) == 0)
    return 0;
  return 1;
}

/* Is it a leap year in the Jewish Calendar */
bool Converter::hebrew_leap_year_p(int year)
{
  switch (year % 19)
      {
      case 0:
      case 3:
      case 6:
      case 8:
      case 11:
      case 14:
      case 17:
        return 1;
      default:
        return 0;
      }
}

/* Return the number of days from 1 Tishrei 0001 to the beginning of the given year.
 * Since this routine gets called frequently with the same year arguments, we cache
 * the most recent values.
 */
#define MEMORY 5
long Converter::hebrew_elapsed_days(int year)
{
  static int saved_year[MEMORY] = { -1, -1, -1, -1, -1 };
  static long saved_value[MEMORY];
  int i;

  for (i = 0; i < MEMORY; ++i)
    if (year == saved_year[i])
      return saved_value[i];
  for (i = 0; i < MEMORY-1; ++i) {
    saved_year[i] = saved_year[1 + i];
    saved_value[i] = saved_value[1 + i];
  }
  saved_year[MEMORY - 1] = year;
  saved_value[MEMORY - 1] = hebrew_elapsed_days2(year);
  return saved_value[MEMORY - 1];
}

long Converter::hebrew_elapsed_days2(int year)
{
  long prev_year = year - 1;
  long months_elapsed = 235L * (prev_year / 19)  /* months in complete cycles so far */
    + 12L * (prev_year % 19)                     /* regular months in this cycle */
    + (((prev_year % 19) * 7 + 1) / 19);         /* leap months this cycle */
  long parts_elapsed = 5604 + 13753 * months_elapsed;
  long day = 1 + 29 * months_elapsed + parts_elapsed / 25920;
  long parts = parts_elapsed % 25920;
  int weekday = (day % 7);
  long alt_day = ((parts >= 19440)
                  || (weekday == 2 && (parts >= 9924)
                      && !hebrew_leap_year_p(year)) || (weekday == 1
                                                        && (parts >=
                                                            16789)
                                                        &&
                                                        hebrew_leap_year_p
                                                        (prev_year)))
    ? day + 1 : day;
  switch (alt_day % 7)
      {
      case 0:
      case 3:
      case 5:
        return 1 + alt_day;
      default:
        return alt_day;
      }
}

/* Number of days in the given Hebrew year */
int Converter::hebrew_year_length(int year)
{
  return hebrew_elapsed_days(1 + year) - hebrew_elapsed_days(year);
}

/* Fill in the DateResult structure based on the given secular date */
void
  Converter::SecularToHebrewConversion(int syear, int smonth,
                                       int sday,
                                       struct DateResult *result)
{
  int hyear, hmonth, hday;
  long absolute;

  absolute = absolute_from_gregorian(syear, smonth, sday);

  hebrew_from_absolute(absolute, &hyear, &hmonth, &hday);

  result->year = hyear;
  result->month = hmonth;
  result->day = hday;
  finish_up(absolute, hyear, hmonth, syear, smonth, result);
}

/* Fill in the DateResult structure based on the given Hebrew date */
void
  Converter::HebrewToSecularConversion(int hyear, int hmonth,
                                       int hday,
                                       struct DateResult *result)
{
  int syear, smonth, sday;
  long absolute;

  absolute = absolute_from_hebrew(hyear, hmonth, hday);
  gregorian_from_absolute(absolute, &syear, &smonth, &sday);
  result->year = hyear;
  result->month = hmonth;
  result->day = hday;
  finish_up(absolute, hyear, hmonth, syear, smonth, result);
}

/* This is common code for filling up the DateResult structure */
void
  Converter::finish_up(long absolute, int hyear, int hmonth,
                       int syear, int smonth,
                       struct DateResult *result)
{
  result->hebrew_month_length = hebrew_month_length(hyear, hmonth);
  result->secular_month_length = secular_month_length(syear, smonth);
  result->hebrew_leap_year_p = hebrew_leap_year_p(hyear);
  result->secular_leap_year_p = gregorian_leap_year_p(syear);
  result->kvia = (hebrew_year_length(hyear) % 10) - 3;
  // absolute is -1 on 1/1/0001 Julian
  result->day_of_week = (7 + absolute) % 7;
  result->hebrew_day_number =
    absolute - absolute_from_hebrew(hyear, 7, 1) + 1;

}
<|endoftext|>


<commit_before>#include "stdafx.h"
#include "network_address.h"
#include "url_parser.h"

vds::network_address::network_address()
: addr_size_(sizeof(this->addr_)) {
  memset((char *)&this->addr_, 0, sizeof(this->addr_));
}

vds::expected<void> vds::network_address::reset(
  sa_family_t af,
  int sock_type,
  int ai_flags,
  int ai_protocol,
  const std::string &server,
  uint16_t port) {
  memset((char *)&this->addr_, 0, sizeof(this->addr_));

  addrinfo hints;

  memset(&hints, 0, sizeof(hints));
  hints.ai_family = af;
  hints.ai_socktype = sock_type;//SOCK_DGRAM;
  hints.ai_protocol = ai_protocol;// IPPROTO_UDP;
  hints.ai_flags = ai_flags | AI_NUMERICHOST; // AI_NUMERICSERV | AI_ALL | AI_V4MAPPED

  addrinfo * buffer;
  auto status = getaddrinfo(server.c_str(), std::to_string(port).c_str(), &hints, &buffer);
  if (status) {
    hints.ai_flags = ai_flags;
    status = getaddrinfo(server.c_str(), std::to_string(port).c_str(), &hints, &buffer);
    if (status) {
      return vds::make_unexpected<std::system_error>(status, std::system_category(), "Parse address " + server);
    }
  }

  for (auto res = buffer; res != nullptr; res = res->ai_next) {
    if(res->ai_family == af) {
      switch(af) {
      case AF_INET: {
        this->addr_size_ = sizeof(sockaddr_in);
        memcpy(&this->addr_, res->ai_addr, this->addr_size_);
        freeaddrinfo(buffer);
        return expected<void>();
      }
      case AF_INET6: {
        this->addr_size_ = sizeof(sockaddr_in6);
        memcpy(&this->addr_, res->ai_addr, this->addr_size_);
        freeaddrinfo(buffer);
        return  expected<void>();
      }
      }
    }
  }
  freeaddrinfo(buffer);

  return vds::make_unexpected<std::runtime_error>("Unable to resolve " + server);
}

std::string vds::network_address::to_string() const {
  auto s = this->server();

  return
    ((AF_INET6 == this->family())
      ? "udp6://"
      : "udp://")
    + (s.has_error() ? s.error()->what() : s.value())
    + ":" + std::to_string(this->port());
}

vds::expected<vds::network_address> vds::network_address::parse(const std::string& address) {
  vds::network_address result;
  CHECK_EXPECTED(url_parser::parse_addresses(
      address,
      [&result](const std::string &protocol, const std::string &address) -> expected<bool> {
        if ("udp" == protocol || "udp6" == protocol) {
          auto na = url_parser::parse_network_address(address);
          if (na.protocol != "udp" && na.protocol != "udp6") {
            return vds::make_unexpected<std::invalid_argument>("address");
          }

          CHECK_EXPECTED(result.reset(
              (na.protocol == "udp") ? AF_INET : AF_INET6,
              SOCK_DGRAM,
              AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
              IPPROTO_UDP,
              na.server,
              (uint16_t) atoi(na.port.c_str())));
        } else if ("tcp" == protocol || "tcp6" == protocol) {
            auto na = url_parser::parse_network_address(address);
            if (na.protocol != "tcp" && na.protocol != "tcp6") {
              return vds::make_unexpected<std::invalid_argument>("address");
            }

            CHECK_EXPECTED(result.reset(
              (na.protocol == "tcp") ? AF_INET : AF_INET6,
              SOCK_STREAM,
              AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
              0,
              na.server,
              (uint16_t)atoi(na.port.c_str())));
          }
          else {
            return vds::make_unexpected<std::runtime_error>("Invalid addresss");
        }
        return true;
      }));
  if (!result) {
    return vds::make_unexpected<std::runtime_error>("Invalid address");
  }
  return result;
}

vds::expected<vds::network_address> vds::network_address::parse(sa_family_t family, const std::string& address) {
  vds::network_address result;
  CHECK_EXPECTED(url_parser::parse_addresses(
    address,
    [&result, family](const std::string &protocol, const std::string &address) -> expected<bool> {
    if ("udp" == protocol || "udp6" == protocol) {
      auto na = url_parser::parse_network_address(address);
      if (na.protocol != "udp" && na.protocol != "udp6") {
        return vds::make_unexpected<std::invalid_argument>("address");
      }
      switch (family) {
        case AF_INET6: {
          if ("udp" == na.protocol) {
            CHECK_EXPECTED(result.reset(
              AF_INET6,
              SOCK_DGRAM,
              AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
              IPPROTO_UDP,
              na.server,
              (uint16_t)atoi(na.port.c_str())));
          }
          else if ("udp6" == na.protocol) {
            CHECK_EXPECTED(result.reset(
              AF_INET6,
                SOCK_DGRAM,
                AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
                IPPROTO_UDP,
                na.server,
                (uint16_t)atoi(na.port.c_str())));
            }
          break;
        }
        case AF_INET: {
          if ("udp" == na.protocol) {
            CHECK_EXPECTED(result.reset(
              AF_INET,
              SOCK_DGRAM,
              AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
              IPPROTO_UDP,
              na.server,
              (uint16_t)atoi(na.port.c_str())));
          }
          else if ("udp6" == na.protocol) {
            return vds::make_unexpected<std::runtime_error>("Invalid addresss");
          }
          break;
        }
      }
    }
    else {
      return vds::make_unexpected<std::runtime_error>("Invalid addresss");
    }
    return true;
  }));
  if (!result) {
    return vds::make_unexpected<std::runtime_error>("Invalid address");
  }
  return result;
}

vds::expected<vds::network_address> vds::network_address::tcp_ip4(const std::string & server, uint16_t port)
{
  vds::network_address result;
  CHECK_EXPECTED(result.reset(AF_INET, SOCK_STREAM, AI_NUMERICSERV | AI_ALL | AI_V4MAPPED, 0, server, port));
  return result;
}

vds::expected<vds::network_address> vds::network_address::tcp_ip6(const std::string & server, uint16_t port)
{
  vds::network_address result;
  CHECK_EXPECTED(result.reset(AF_INET6, SOCK_STREAM, AI_NUMERICSERV | AI_ALL | AI_V4MAPPED, 0, server, port));
  return result;
}

vds::expected<vds::network_address> vds::network_address::udp_ip4(const std::string & server, uint16_t port)
{
  vds::network_address result;
  CHECK_EXPECTED(result.reset(AF_INET, SOCK_DGRAM, AI_NUMERICSERV | AI_ALL | AI_V4MAPPED, IPPROTO_UDP, server, port));
  return result;
}

vds::expected<vds::network_address> vds::network_address::udp_ip6(const std::string & server, uint16_t port)
{
  vds::network_address result;
  CHECK_EXPECTED(result.reset(AF_INET6, SOCK_DGRAM, AI_NUMERICSERV | AI_ALL | AI_V4MAPPED, IPPROTO_UDP, server, port));
  return result;
}

sa_family_t vds::network_address::family() const {
  return reinterpret_cast<const sockaddr *>(&this->addr_)->sa_family;
}

vds::expected<std::string> vds::network_address::server() const {
//  char buffer[512];
//
//  auto addr = (sockaddr *)&this->addr_;
//  inet_ntop(addr->sa_family, addr, buffer, this->addr_size_);
//
//  return buffer;
  char buffer[512];

  const auto err = getnameinfo(
      (struct sockaddr *)&this->addr_,
      this->addr_size_,
      buffer,
      sizeof(buffer),
      0,
      0,
      NI_NUMERICHOST);

  if (0 != err) {
    const auto error = errno;
    return vds::make_unexpected<std::system_error>(error, std::system_category(), "getnameinfo");
  }

  return buffer;
}

uint16_t vds::network_address::port() const {
  auto addr = (sockaddr *)&this->addr_;
  switch(addr->sa_family) {
    case AF_INET:{
      return ntohs(reinterpret_cast<const sockaddr_in *>(&this->addr_)->sin_port);
    }
    case AF_INET6:{
      return ntohs(reinterpret_cast<const sockaddr_in6 *>(&this->addr_)->sin6_port);
    }
    default: {
      vds_assert(false);
      return -1;
    }
  }
}

vds::network_address::network_address(
  sa_family_t af,
  uint16_t port) {
  memset((char *)&this->addr_, 0, sizeof(this->addr_));

  switch(af) {
    case AF_INET: {
      auto addr = (sockaddr_in *) &this->addr_;
      addr->sin_family = af;
      addr->sin_port = htons(port);
      addr->sin_addr.s_addr = INADDR_ANY;

      this->addr_size_ = sizeof(sockaddr_in);
      break;
    }
    case AF_INET6: {
      auto addr = (sockaddr_in6 *) &this->addr_;
      addr->sin6_family = af;
      addr->sin6_port = htons(port);
      addr->sin6_addr = IN6ADDR_ANY_INIT;

      this->addr_size_ = sizeof(sockaddr_in6);
      break;
    }
    default:
      vds_assert(false);
    break;
  }
}

bool vds::network_address::is_martian() const {
  auto addr = (const sockaddr *)&this->addr_;
  switch(addr->sa_family) {
    case AF_INET: {
      struct sockaddr_in *sin = (struct sockaddr_in*)&this->addr_;
      const unsigned char *address = (const unsigned char*)&sin->sin_addr;
      return sin->sin_port == 0 ||
             (address[0] == 0) ||
             (address[0] == 127) ||
             ((address[0] & 0xE0) == 0xE0);
    }
    case AF_INET6: {
      static const unsigned char zeroes[20] = {0};
      static const unsigned char v4prefix[16] = {
          0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFF, 0xFF, 0, 0, 0, 0
      };
      struct sockaddr_in6 *sin6 = (struct sockaddr_in6*)&this->addr_;
      const unsigned char *address = (const unsigned char*)&sin6->sin6_addr;
      return sin6->sin6_port == 0 ||
             (address[0] == 0xFF) ||
             (address[0] == 0xFE && (address[1] & 0xC0) == 0x80) ||
             (memcmp(address, zeroes, 15) == 0 &&
              (address[15] == 0 || address[15] == 1)) ||
             (memcmp(address, v4prefix, 12) == 0);
    }
    default:
      vds_assert(false);
      break;
  }
}

int vds::network_address::compare(const network_address & other) const {
  if (this->addr_.ss_family == other.addr_.ss_family) {
    switch (this->addr_.ss_family) {
    case AF_INET: {
      const auto result = reinterpret_cast<const sockaddr_in *>(&this->addr_)->sin_addr.s_addr -
        reinterpret_cast<const sockaddr_in *>(&other.addr_)->sin_addr.s_addr;
      if (0 != result) {
        return result;
      }

      return reinterpret_cast<const sockaddr_in *>(&this->addr_)->sin_port -
        reinterpret_cast<const sockaddr_in *>(&other.addr_)->sin_port;
    }
    case AF_INET6: {
      const auto l = reinterpret_cast<const sockaddr_in6 *>(&this->addr_);
      const auto r = reinterpret_cast<const sockaddr_in6 *>(&other.addr_);
      auto result = memcmp(
        &l->sin6_addr,
        &r->sin6_addr,
        sizeof(l->sin6_addr));
      if (0 != result) {
        return result;
      }

      result = ntohs(l->sin6_port) - ntohs(r->sin6_port);
      if (0 != result) {
        return result;
      }

      result = l->sin6_flowinfo - r->sin6_flowinfo;
      if (0 != result) {
        return result;
      }

      return l->sin6_scope_id - r->sin6_scope_id;
    }
    default: {
      vds_assert(false);
      break;
    }
    }
  }
  else {
    if (this->addr_.ss_family == AF_INET && other.addr_.ss_family == AF_INET6) {
      const auto l = reinterpret_cast<const sockaddr_in *>(&this->addr_);
      const auto r = reinterpret_cast<const sockaddr_in6 *>(&other.addr_);
      if (IN6_IS_ADDR_V4MAPPED(&r->sin6_addr)) {
#ifdef _WIN32
        const auto result = l->sin_addr.s_addr - r->sin6_addr.u.Word[3];
#else
        const auto result = l->sin_addr.s_addr - r->sin6_addr.s6_addr32[3];
#endif
        if (0 != result) {
          return result;
        }

        return l->sin_port - r->sin6_port;
      }
    }
    else
      if (this->addr_.ss_family == AF_INET6 && other.addr_.ss_family == AF_INET) {
        const auto l = reinterpret_cast<const sockaddr_in6 *>(&this->addr_);
        const auto r = reinterpret_cast<const sockaddr_in *>(&other.addr_);
        if (IN6_IS_ADDR_V4MAPPED(&l->sin6_addr)) {
#ifdef _WIN32
          const auto result = l->sin6_addr.u.Word[3] - r->sin_addr.s_addr;
#else
          const auto result = l->sin6_addr.s6_addr32[3] - r->sin_addr.s_addr;
#endif
          if (0 != result) {
            return result;
          }

          return l->sin6_port - r->sin_port;
        }
      }

    return this->addr_.ss_family - other.addr_.ss_family;
  }
}

void vds::network_address::clear() {
  this->addr_size_ = sizeof(this->addr_);
  memset(&this->addr_, 0, sizeof(this->addr_));
}

<commit_msg>android build<commit_after>#include "stdafx.h"
#include "network_address.h"
#include "url_parser.h"

vds::network_address::network_address()
: addr_size_(sizeof(this->addr_)) {
  memset((char *)&this->addr_, 0, sizeof(this->addr_));
}

vds::expected<void> vds::network_address::reset(
  sa_family_t af,
  int sock_type,
  int ai_flags,
  int ai_protocol,
  const std::string &server,
  uint16_t port) {
  memset((char *)&this->addr_, 0, sizeof(this->addr_));

  addrinfo hints;

  memset(&hints, 0, sizeof(hints));
  hints.ai_family = af;
  hints.ai_socktype = sock_type;//SOCK_DGRAM;
  hints.ai_protocol = ai_protocol;// IPPROTO_UDP;
  hints.ai_flags = ai_flags | AI_NUMERICHOST; // AI_NUMERICSERV | AI_ALL | AI_V4MAPPED

  addrinfo * buffer;
  auto status = getaddrinfo(server.c_str(), std::to_string(port).c_str(), &hints, &buffer);
  if (status) {
    hints.ai_flags = ai_flags;
    status = getaddrinfo(server.c_str(), std::to_string(port).c_str(), &hints, &buffer);
    if (status) {
      return vds::make_unexpected<std::system_error>(status, std::system_category(), "Parse address " + server);
    }
  }

  for (auto res = buffer; res != nullptr; res = res->ai_next) {
    if(res->ai_family == af) {
      switch(af) {
      case AF_INET: {
        this->addr_size_ = sizeof(sockaddr_in);
        memcpy(&this->addr_, res->ai_addr, this->addr_size_);
        freeaddrinfo(buffer);
        return expected<void>();
      }
      case AF_INET6: {
        this->addr_size_ = sizeof(sockaddr_in6);
        memcpy(&this->addr_, res->ai_addr, this->addr_size_);
        freeaddrinfo(buffer);
        return  expected<void>();
      }
      }
    }
  }
  freeaddrinfo(buffer);

  return vds::make_unexpected<std::runtime_error>("Unable to resolve " + server);
}

std::string vds::network_address::to_string() const {
  auto s = this->server();

  return
    ((AF_INET6 == this->family())
      ? "udp6://"
      : "udp://")
    + (s.has_error() ? s.error()->what() : s.value())
    + ":" + std::to_string(this->port());
}

vds::expected<vds::network_address> vds::network_address::parse(const std::string& address) {
  vds::network_address result;
  CHECK_EXPECTED(url_parser::parse_addresses(
      address,
      [&result](const std::string &protocol, const std::string &address) -> expected<bool> {
        if ("udp" == protocol || "udp6" == protocol) {
          auto na = url_parser::parse_network_address(address);
          if (na.protocol != "udp" && na.protocol != "udp6") {
            return vds::make_unexpected<std::invalid_argument>("address");
          }

          CHECK_EXPECTED(result.reset(
              (na.protocol == "udp") ? AF_INET : AF_INET6,
              SOCK_DGRAM,
              AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
              IPPROTO_UDP,
              na.server,
              (uint16_t) atoi(na.port.c_str())));
        } else if ("tcp" == protocol || "tcp6" == protocol) {
            auto na = url_parser::parse_network_address(address);
            if (na.protocol != "tcp" && na.protocol != "tcp6") {
              return vds::make_unexpected<std::invalid_argument>("address");
            }

            CHECK_EXPECTED(result.reset(
              (na.protocol == "tcp") ? AF_INET : AF_INET6,
              SOCK_STREAM,
              AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
              0,
              na.server,
              (uint16_t)atoi(na.port.c_str())));
          }
          else {
            return vds::make_unexpected<std::runtime_error>("Invalid addresss");
        }
        return true;
      }));
  if (!result) {
    return vds::make_unexpected<std::runtime_error>("Invalid address");
  }
  return result;
}

vds::expected<vds::network_address> vds::network_address::parse(sa_family_t family, const std::string& address) {
  vds::network_address result;
  CHECK_EXPECTED(url_parser::parse_addresses(
    address,
    [&result, family](const std::string &protocol, const std::string &address) -> expected<bool> {
    if ("udp" == protocol || "udp6" == protocol) {
      auto na = url_parser::parse_network_address(address);
      if (na.protocol != "udp" && na.protocol != "udp6") {
        return vds::make_unexpected<std::invalid_argument>("address");
      }
      switch (family) {
        case AF_INET6: {
          if ("udp" == na.protocol) {
            CHECK_EXPECTED(result.reset(
              AF_INET6,
              SOCK_DGRAM,
              AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
              IPPROTO_UDP,
              na.server,
              (uint16_t)atoi(na.port.c_str())));
          }
          else if ("udp6" == na.protocol) {
            CHECK_EXPECTED(result.reset(
              AF_INET6,
                SOCK_DGRAM,
                AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
                IPPROTO_UDP,
                na.server,
                (uint16_t)atoi(na.port.c_str())));
            }
          break;
        }
        case AF_INET: {
          if ("udp" == na.protocol) {
            CHECK_EXPECTED(result.reset(
              AF_INET,
              SOCK_DGRAM,
              AI_NUMERICSERV | AI_ALL | AI_V4MAPPED,
              IPPROTO_UDP,
              na.server,
              (uint16_t)atoi(na.port.c_str())));
          }
          else if ("udp6" == na.protocol) {
            return vds::make_unexpected<std::runtime_error>("Invalid addresss");
          }
          break;
        }
      }
    }
    else {
      return vds::make_unexpected<std::runtime_error>("Invalid addresss");
    }
    return true;
  }));
  if (!result) {
    return vds::make_unexpected<std::runtime_error>("Invalid address");
  }
  return result;
}

vds::expected<vds::network_address> vds::network_address::tcp_ip4(const std::string & server, uint16_t port)
{
  vds::network_address result;
  CHECK_EXPECTED(result.reset(AF_INET, SOCK_STREAM, AI_NUMERICSERV | AI_ALL | AI_V4MAPPED, 0, server, port));
  return result;
}

vds::expected<vds::network_address> vds::network_address::tcp_ip6(const std::string & server, uint16_t port)
{
  vds::network_address result;
  CHECK_EXPECTED(result.reset(AF_INET6, SOCK_STREAM, AI_NUMERICSERV | AI_ALL | AI_V4MAPPED, 0, server, port));
  return result;
}

vds::expected<vds::network_address> vds::network_address::udp_ip4(const std::string & server, uint16_t port)
{
  vds::network_address result;
  CHECK_EXPECTED(result.reset(AF_INET, SOCK_DGRAM, AI_NUMERICSERV | AI_ALL | AI_V4MAPPED, IPPROTO_UDP, server, port));
  return result;
}

vds::expected<vds::network_address> vds::network_address::udp_ip6(const std::string & server, uint16_t port)
{
  vds::network_address result;
  CHECK_EXPECTED(result.reset(AF_INET6, SOCK_DGRAM, AI_NUMERICSERV | AI_ALL | AI_V4MAPPED, IPPROTO_UDP, server, port));
  return result;
}

sa_family_t vds::network_address::family() const {
  return reinterpret_cast<const sockaddr *>(&this->addr_)->sa_family;
}

vds::expected<std::string> vds::network_address::server() const {
//  char buffer[512];
//
//  auto addr = (sockaddr *)&this->addr_;
//  inet_ntop(addr->sa_family, addr, buffer, this->addr_size_);
//
//  return buffer;
  char buffer[512];

  const auto err = getnameinfo(
      (struct sockaddr *)&this->addr_,
      this->addr_size_,
      buffer,
      sizeof(buffer),
      0,
      0,
      NI_NUMERICHOST);

  if (0 != err) {
    const auto error = errno;
    return vds::make_unexpected<std::system_error>(error, std::system_category(), "getnameinfo");
  }

  return buffer;
}

uint16_t vds::network_address::port() const {
  auto addr = (sockaddr *)&this->addr_;
  switch(addr->sa_family) {
    case AF_INET:{
      return ntohs(reinterpret_cast<const sockaddr_in *>(&this->addr_)->sin_port);
    }
    case AF_INET6:{
      return ntohs(reinterpret_cast<const sockaddr_in6 *>(&this->addr_)->sin6_port);
    }
    default: {
      vds_assert(false);
      return -1;
    }
  }
}

vds::network_address::network_address(
  sa_family_t af,
  uint16_t port) {
  memset((char *)&this->addr_, 0, sizeof(this->addr_));

  switch(af) {
    case AF_INET: {
      auto addr = (sockaddr_in *) &this->addr_;
      addr->sin_family = af;
      addr->sin_port = htons(port);
      addr->sin_addr.s_addr = INADDR_ANY;

      this->addr_size_ = sizeof(sockaddr_in);
      break;
    }
    case AF_INET6: {
      auto addr = (sockaddr_in6 *) &this->addr_;
      addr->sin6_family = af;
      addr->sin6_port = htons(port);
      addr->sin6_addr = IN6ADDR_ANY_INIT;

      this->addr_size_ = sizeof(sockaddr_in6);
      break;
    }
    default:
      vds_assert(false);
    break;
  }
}

bool vds::network_address::is_martian() const {
  auto addr = (const sockaddr *)&this->addr_;
  switch(addr->sa_family) {
    case AF_INET: {
      struct sockaddr_in *sin = (struct sockaddr_in*)&this->addr_;
      const unsigned char *address = (const unsigned char*)&sin->sin_addr;
      return sin->sin_port == 0 ||
             (address[0] == 0) ||
             (address[0] == 127) ||
             ((address[0] & 0xE0) == 0xE0);
    }
    case AF_INET6: {
      static const unsigned char zeroes[20] = {0};
      static const unsigned char v4prefix[16] = {
          0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFF, 0xFF, 0, 0, 0, 0
      };
      struct sockaddr_in6 *sin6 = (struct sockaddr_in6*)&this->addr_;
      const unsigned char *address = (const unsigned char*)&sin6->sin6_addr;
      return sin6->sin6_port == 0 ||
             (address[0] == 0xFF) ||
             (address[0] == 0xFE && (address[1] & 0xC0) == 0x80) ||
             (memcmp(address, zeroes, 15) == 0 &&
              (address[15] == 0 || address[15] == 1)) ||
             (memcmp(address, v4prefix, 12) == 0);
    }
    default:
      vds_assert(false);
      break;
  }

  return false;
}

int vds::network_address::compare(const network_address & other) const {
  if (this->addr_.ss_family == other.addr_.ss_family) {
    switch (this->addr_.ss_family) {
    case AF_INET: {
      const auto result = reinterpret_cast<const sockaddr_in *>(&this->addr_)->sin_addr.s_addr -
        reinterpret_cast<const sockaddr_in *>(&other.addr_)->sin_addr.s_addr;
      if (0 != result) {
        return result;
      }

      return reinterpret_cast<const sockaddr_in *>(&this->addr_)->sin_port -
        reinterpret_cast<const sockaddr_in *>(&other.addr_)->sin_port;
    }
    case AF_INET6: {
      const auto l = reinterpret_cast<const sockaddr_in6 *>(&this->addr_);
      const auto r = reinterpret_cast<const sockaddr_in6 *>(&other.addr_);
      auto result = memcmp(
        &l->sin6_addr,
        &r->sin6_addr,
        sizeof(l->sin6_addr));
      if (0 != result) {
        return result;
      }

      result = ntohs(l->sin6_port) - ntohs(r->sin6_port);
      if (0 != result) {
        return result;
      }

      result = l->sin6_flowinfo - r->sin6_flowinfo;
      if (0 != result) {
        return result;
      }

      return l->sin6_scope_id - r->sin6_scope_id;
    }
    default: {
      vds_assert(false);
      break;
    }
    }
  }
  else {
    if (this->addr_.ss_family == AF_INET && other.addr_.ss_family == AF_INET6) {
      const auto l = reinterpret_cast<const sockaddr_in *>(&this->addr_);
      const auto r = reinterpret_cast<const sockaddr_in6 *>(&other.addr_);
      if (IN6_IS_ADDR_V4MAPPED(&r->sin6_addr)) {
#ifdef _WIN32
        const auto result = l->sin_addr.s_addr - r->sin6_addr.u.Word[3];
#else
        const auto result = l->sin_addr.s_addr - r->sin6_addr.s6_addr32[3];
#endif
        if (0 != result) {
          return result;
        }

        return l->sin_port - r->sin6_port;
      }
    }
    else
      if (this->addr_.ss_family == AF_INET6 && other.addr_.ss_family == AF_INET) {
        const auto l = reinterpret_cast<const sockaddr_in6 *>(&this->addr_);
        const auto r = reinterpret_cast<const sockaddr_in *>(&other.addr_);
        if (IN6_IS_ADDR_V4MAPPED(&l->sin6_addr)) {
#ifdef _WIN32
          const auto result = l->sin6_addr.u.Word[3] - r->sin_addr.s_addr;
#else
          const auto result = l->sin6_addr.s6_addr32[3] - r->sin_addr.s_addr;
#endif
          if (0 != result) {
            return result;
          }

          return l->sin6_port - r->sin_port;
        }
      }

    return this->addr_.ss_family - other.addr_.ss_family;
  }
}

void vds::network_address::clear() {
  this->addr_size_ = sizeof(this->addr_);
  memset(&this->addr_, 0, sizeof(this->addr_));
}

<|endoftext|>


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

/* Author: Taehoon Lim (Darby) */

#include <fcntl.h>          // FILE control
#include <termios.h>        // Terminal IO

#include <ros/ros.h>
#include <dynamixel_workbench_msgs/WheelCommand.h>

#define ESC_ASCII_VALUE             0x1b
#define FORWARD                     0x77
#define BACKWARD                    0x78
#define LEFT                        0x61
#define RIGHT                       0x64
#define STOPS                       0x73

int getch(void)
{
  struct termios oldt, newt;
  int ch;

  tcgetattr( STDIN_FILENO, &oldt );
  newt = oldt;
  newt.c_lflag &= ~(ICANON | ECHO);
  newt.c_cc[VMIN] = 0;
  newt.c_cc[VTIME] = 1;
  tcsetattr( STDIN_FILENO, TCSANOW, &newt );
  ch = getchar();
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt );

  return ch;
}

int kbhit(void)
{
  struct termios oldt, newt;
  int ch;
  int oldf;

  tcgetattr(STDIN_FILENO, &oldt);
  newt = oldt;
  newt.c_lflag &= ~(ICANON | ECHO);
  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
  oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
  fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);

  ch = getchar();

  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
  fcntl(STDIN_FILENO, F_SETFL, oldf);

  if (ch != EOF)
  {
    ungetc(ch, stdin);
    return 1;
  }
  return 0;
}

int main(int argc, char **argv)
{
  // Init ROS node
  ros::init(argc, argv, "wheel_operator");

  ROS_INFO("Set angular velocity(+-0.2 rad/sec) to your Dynamixel!! by using keyboard");
  ROS_INFO("w : Forward");
  ROS_INFO("x : Backward");
  ROS_INFO("a : Left");
  ROS_INFO("d : Right");
  ROS_INFO("s : STOPS\n");
  ROS_INFO("ESC : exit");

  dynamixel_workbench_msgs::WheelCommand wheel_command;
  ros::NodeHandle node_handle;

  ros::ServiceClient wheel_command_client =
                node_handle.serviceClient<dynamixel_workbench_msgs::WheelCommand>("/wheel_command");
  ros::Rate loop_rate(250);

  while(ros::ok())
  {
    if (kbhit())
    {
      char c = getch();

      if (c == ESC_ASCII_VALUE)
      {
        break;
      }

      if (c == FORWARD)
      {
        wheel_command.request.right_wheel_velocity = 0.2;
        wheel_command.request.left_wheel_velocity = 0.2;
      }
      else if (c == BACKWARD)
      {
        wheel_command.request.right_wheel_velocity = -0.2;
        wheel_command.request.left_wheel_velocity = -0.2;
      }
      else if (c == LEFT)
      {
        wheel_command.request.right_wheel_velocity = 0.2;
        wheel_command.request.left_wheel_velocity = -0.2;
      }
      else if (c == RIGHT)
      {
        wheel_command.request.right_wheel_velocity = -0.2;
        wheel_command.request.left_wheel_velocity = 0.2;
      }
      else if (c == STOPS)
      {
        wheel_command.request.right_wheel_velocity = 0.0;
        wheel_command.request.left_wheel_velocity = 0.0;
      }
      else
      {
        wheel_command.request.right_wheel_velocity = 0.2;
        wheel_command.request.left_wheel_velocity = 0.2;
      }

      if (wheel_command_client.call(wheel_command))
      {
        ROS_INFO("[LEFT_WHEEL_VELOCITY]: %.2f, [RIGHT_WHEEL_VELOCITY]: %.2f", wheel_command.response.left_wheel_velocity, wheel_command.response.right_wheel_velocity);
      }
      else
      {
        ROS_ERROR("Failed to call service /wheel_command");
      }
    }

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

/* Author: Taehoon Lim (Darby) */

#include <fcntl.h>          // FILE control
#include <termios.h>        // Terminal IO

#include <ros/ros.h>
#include <dynamixel_workbench_msgs/WheelCommand.h>

#define ESC_ASCII_VALUE             0x1b
#define FORWARD                     0x77
#define BACKWARD                    0x78
#define LEFT                        0x61
#define RIGHT                       0x64
#define STOPS                       0x73

int getch(void)
{
  struct termios oldt, newt;
  int ch;

  tcgetattr( STDIN_FILENO, &oldt );
  newt = oldt;
  newt.c_lflag &= ~(ICANON | ECHO);
  newt.c_cc[VMIN] = 0;
  newt.c_cc[VTIME] = 1;
  tcsetattr( STDIN_FILENO, TCSANOW, &newt );
  ch = getchar();
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt );

  return ch;
}

int kbhit(void)
{
  struct termios oldt, newt;
  int ch;
  int oldf;

  tcgetattr(STDIN_FILENO, &oldt);
  newt = oldt;
  newt.c_lflag &= ~(ICANON | ECHO);
  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
  oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
  fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);

  ch = getchar();

  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
  fcntl(STDIN_FILENO, F_SETFL, oldf);

  if (ch != EOF)
  {
    ungetc(ch, stdin);
    return 1;
  }
  return 0;
}

int main(int argc, char **argv)
{
  // Init ROS node
  ros::init(argc, argv, "wheel_operator");

  ROS_INFO("Set angular velocity(+-0.2 rad/sec) to your Dynamixel!! by using keyboard");
  ROS_INFO("w : Forward");
  ROS_INFO("x : Backward");
  ROS_INFO("a : Left");
  ROS_INFO("d : Right");
  ROS_INFO("s : STOPS\n");
  ROS_INFO("ESC : exit");

  dynamixel_workbench_msgs::WheelCommand wheel_command;
  ros::NodeHandle node_handle;

  ros::ServiceClient wheel_command_client =
                node_handle.serviceClient<dynamixel_workbench_msgs::WheelCommand>("/wheel_command");
  ros::Rate loop_rate(250);

  while(ros::ok())
  {
    if (kbhit())
    {
      char c = getch();

      if (c == ESC_ASCII_VALUE)
      {
        break;
      }

      if (c == FORWARD)
      {
        wheel_command.request.right_vel = 0.2;
        wheel_command.request.left_vel  = 0.2;
      }
      else if (c == BACKWARD)
      {
        wheel_command.request.right_vel = -0.2;
        wheel_command.request.left_vel  = -0.2;
      }
      else if (c == LEFT)
      {
        wheel_command.request.right_vel = 0.2;
        wheel_command.request.left_vel  = -0.2;
      }
      else if (c == RIGHT)
      {
        wheel_command.request.right_vel = -0.2;
        wheel_command.request.left_vel  = 0.2;
      }
      else if (c == STOPS)
      {
        wheel_command.request.right_vel = 0.0;
        wheel_command.request.left_vel  = 0.0;
      }
      else
      {
        wheel_command.request.right_vel = 0.2;
        wheel_command.request.left_vel  = 0.2;
      }

      if (wheel_command_client.call(wheel_command))
      {
        ROS_INFO("[LEFT_VELOCITY]: %.2f, [RIGHT_VELOCITY]: %.2f", wheel_command.response.left_vel, wheel_command.response.right_vel);
      }
      else
      {
        ROS_ERROR("Failed to call service /wheel_command");
      }
    }

    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
<|endoftext|>