<commit_before>//Write in VC++6.0

#include <stdio.h>
#include <stdlib.h>         //exit() is in the header
#include <string.h>
#include <string>

using namespace std;

#define u32 unsigned int
#define u8 unsigned char

int main(int argc,char *argv[])
{
	unsigned char a;
	if(argc>2)
	{
		printf("Wrong arguments or too many files, you can only draw them in one by one!\nPress Enter to Exit...");
		a=getchar();
		exit(0);
	}
	if(argc==1)
	{
		printf("No files loaded, draw one to the icon of the application!\nPress Enter to Exit...");
		a=getchar();
		exit(0);
	}
	string FileType;
	FileType=argv[1];

	if( (FileType.find(".gba",0)+4)!=FileType.length() )
	{
		printf("Wrong File Type!\nPress Enter to Exit...");
		a=getchar();
		exit(0);
	}

	FILE *gbafp;
	if((gbafp=fopen(argv[1],"r+b"))==NULL)
	{
		printf("Cannot Open file!\nPress Enter to Exit...");
		a=getchar();
		exit(0);
	}
	//Get all the pointers point to Level Pointers and Flags Stream
	fseek(gbafp,7926400L,0);     //78F280
	u8 ptrStream[96];
	unsigned int i,j;
	for(i=0;i<96;i++)
		fread(&ptrStream[i],sizeof(u8),1,gbafp);
	u32 pointers[24];
	for(i=0;i<96;i+=4)
		pointers[i/4]=(ptrStream[i+3]<<24)+(ptrStream[i+2]<<16)+(ptrStream[i+1]<<8)+ptrStream[i]-134217728L;
	//Then Load Room number after 639068 for each level
	u8 Bytes[288];
	rewind(gbafp);
	fseek(gbafp,6525032L,0);     //639068
	for(i=0;i<288;i++)
		fread(&Bytes[i],sizeof(u8),1,gbafp);

	unsigned char B[24][704];        //For Save all the Bytes
	unsigned int LevelIndex[17]={17,17,17,17,17,17,17,17,17,17,17,17,17,17,17,17};
	unsigned int n=0;
	for(i=0;i<24;i++)
	{
		rewind(gbafp);
		fseek(gbafp,pointers[i],0);
		fread(&B[i],704*sizeof(u8),1,gbafp);
	}
	printf("This patch is recommanded to use with WL4 MultiEditor.exe\n");
	printf("Open file successfully.\n");
	printf("The following level probably can be maximized\n");
	for(i=0;i<17;i++)                //for The 17 Level and the No.24 can be check somewhere else
	{
		for(j=0;j<17;++j)
			if( ((unsigned int) Bytes[12*j]==i) & ((unsigned int) Bytes[12*j+1]<16) )
				break;
		if(j!=17)             //probably there is no way to judge if the Data areas of Levels have been maximized, is there?
		{
			printf("%4d",(unsigned int) Bytes[12*j]);     //temporary output for check
			LevelIndex[n]=j;
			n++;
		}
	}
	printf("\nInput one number stand for the level whose data area you want to maximize (end with Enter): ");
	scanf("%d", &j);
	a=getchar();
	for(i=0;i<17;++i)
		if(LevelIndex[i]==j)
			break;
	if(i==16)
	{
		printf("Wrong Index or don't need to maximize tha level!\tPress Enter to exit...");
		a=getchar();
		exit(0);
	}
	printf("Level%3d, start processing...",(unsigned int) LevelIndex[i]);     //temporary output for check

	fclose(gbafp);
	printf("\nPress Enter to exit...");
	a=getchar();
	return 0;
}
<commit_msg>Update main.cpp<commit_after>//Write in VC++6.0

#include <stdio.h>
#include <stdlib.h>         //exit() is in the header
#include <string>

using namespace std;

#define u32 unsigned int
#define u8 unsigned char

int main(int argc,char *argv[])
{
	unsigned char a;
	if(argc>2)
	{
		printf("Wrong arguments or too many files, you can only draw them in one by one!\nPress Enter to Exit...");
		a=getchar();
		exit(0);
	}
	if(argc==1)
	{
		printf("No files loaded, draw one to the icon of the application!\nPress Enter to Exit...");
		a=getchar();
		exit(0);
	}
	string FileType;
	FileType=argv[1];

	if( (FileType.find(".gba",0)+4)!=FileType.length() )
	{
		printf("Wrong File Type!\nPress Enter to Exit...");
		a=getchar();
		exit(0);
	}

	FILE *gbafp;
	if((gbafp=fopen(argv[1],"r+b"))==NULL)
	{
		printf("Cannot Open file!\nPress Enter to Exit...");
		a=getchar();
		exit(0);
	}
	//Get all the pointers point to Level Pointers and Flags Stream
	fseek(gbafp,7926400L,0);     //78F280
	u8 ptrStream[96];
	unsigned int i,j;
	for(i=0;i<96;i++)
		fread(&ptrStream[i],sizeof(u8),1,gbafp);
	u32 pointers[24];
	for(i=0;i<96;i+=4)
		pointers[i/4]=(ptrStream[i+3]<<24)+(ptrStream[i+2]<<16)+(ptrStream[i+1]<<8)+ptrStream[i]-134217728L;
	//Then Load Room number after 639068 for each level
	u8 Bytes[348];
	rewind(gbafp);
	fseek(gbafp,6525032L,0);     //639068
	for(i=0;i<348;i++)
		fread(&Bytes[i],sizeof(u8),1,gbafp);

	unsigned char B[24][704];        //For Save all the Bytes
	unsigned int LevelIndex[17]={17,17,17,17,17,17,17,17,17,17,17,17,17,17,17,17};
	unsigned int n=0;
	for(i=0;i<24;i++)
	{
		rewind(gbafp);
		fseek(gbafp,pointers[i],0);
		fread(&B[i],704*sizeof(u8),1,gbafp);
	}
	printf("This patch is recommanded to use with WL4 MultiEditor.exe\n");
	printf("Open file successfully.\n");
	printf("The following level probably can be maximized\n");
	for(i=0;i<17;i++)                //for The 17 Level and the No.24 can be check somewhere else
	{
		for(j=0;j<29;++j)
			if( ((unsigned int) Bytes[12*j]==i) & ((unsigned int) Bytes[12*j+1]<16) )
				break;
		if(j!=29)             //probably there is no way to judge if the Data areas of Levels have been maximized, is there?
		{
			printf("%4d",(unsigned int) Bytes[12*j]);     //output for checking
			LevelIndex[n]=i;
			n++;
		}
	}
	for(j=0;j<29;j++)                //for the No.24
	{
		if( ((unsigned int) Bytes[12*j]==23) & ((unsigned int) Bytes[12*j+1]<16) )
		{
			printf("%4d",(unsigned int) Bytes[12*j]);
			LevelIndex[n]=23;
			n++;
		}
	}
	printf("\nInput one number stand for the level whose data area you want to maximize (end with Enter): ");
	scanf("%d", &j);
	a=getchar();
	for(i=0;i<17;++i)
		if(LevelIndex[i]==j)
			break;
	if(i==17)
	{
		printf("Wrong Index or don't need to maximize tha level!\tPress Enter to exit...");
		a=getchar();
		fclose(gbafp);
		exit(0);
	}
	printf("Level%3d, start processing...\n",(unsigned int) LevelIndex[i]);     //output for checking
	n=(unsigned int) LevelIndex[i];      //n=level_Index

	for(j=0;j<17;++j)
		if((unsigned int) Bytes[12*j]==n)
			break;
	i=(unsigned int) Bytes[12*j+1];

	unsigned char New_Bytes[704];
	for(j=0;j<(44*i);j++)
		New_Bytes[j]=B[n][j];
	j--;
	for(i=44*j;i<704;i++)
		New_Bytes[i]=(unsigned char) 241;
	//read from 78F970 to find space
	rewind(gbafp);
	fseek(gbafp,7928176L,0);
	u8 SpaceOffset[4];
	u32 SpaceOffset2,offset2;
	for(i=0;i<4;i++)
		fread(&SpaceOffset[i],sizeof(u8),1,gbafp);
	SpaceOffset2=(unsigned int) (SpaceOffset[0]<<24)+(unsigned int) (SpaceOffset[1]<<16)+(unsigned int) (SpaceOffset[2]<<8)+(unsigned int) (SpaceOffset[3]);
	if(SpaceOffset2==4294967295)
		SpaceOffset2=7928192;
	if(SpaceOffset2%4!=0)
		SpaceOffset2=(SpaceOffset2/4)*4+4;
	offset2=SpaceOffset2;
	SpaceOffset2=SpaceOffset2+134217728;         //add 08000000
	//rewrite the pointer in small endian
	rewind(gbafp);
	fseek(gbafp,(7926400+4*n),0);
	fwrite(&SpaceOffset2,sizeof(u32),1,gbafp);
	//rewrite the offset in small endian
	SpaceOffset2=SpaceOffset2+704-134217728;
	SpaceOffset[0]=SpaceOffset2>>24;
	SpaceOffset[1]=(SpaceOffset2<<8)>>24;
	SpaceOffset[2]=(SpaceOffset2<<16)>>24;
	SpaceOffset[3]=(SpaceOffset2<<24)>>24;
	SpaceOffset2=(unsigned int) (SpaceOffset[0])+(unsigned int) (SpaceOffset[1]<<8)+(unsigned int) (SpaceOffset[2]<<16)+(unsigned int) (SpaceOffset[3]<<24);
	rewind(gbafp);
	fseek(gbafp,7928176L,0);   //78F970
	fwrite(&SpaceOffset2,sizeof(u32),1,gbafp);
	//rewrite the data area, the original area won't be cleared
	rewind(gbafp);
	fseek(gbafp,offset2,0);
	for(i=0;i<704;i++)
		fwrite(&New_Bytes[i],sizeof(u8),1,gbafp);
	printf("Finish!");
	fclose(gbafp);
	printf("\nPress Enter to exit...");
	a=getchar();
	return 0;
}
<|endoftext|>


<commit_before>#pragma once

#include <memory>

// Christopher D. Canfield
// November 2013
// make_unique.hpp
// make_unique is missing from the c++11 standard.
// Adapted from Herb Sutter: http://herbsutter.com/gotw/_102/
// However, VS2012 does not support variadic templates.

template<class T, class Arg1>
std::unique_ptr<T> make_unique(Arg1&& arg1)
{
    return std::unique_ptr<T>(new T(std::forward<Arg1>(arg1)));
}

template<class T, class Arg1, class Arg2>
std::unique_ptr<T> make_unique(Arg1&& arg1, Arg2&& arg2)
{
    return std::unique_ptr<T>(new T(std::forward<Arg1>(arg1), std::forward<Arg2>(arg2)));
}

template<class T, class Arg1, class Arg2, class Arg3>
std::unique_ptr<T> make_unique(Arg1&& arg1, Arg2&& arg2, Arg3&& arg3)
{
    return std::unique_ptr<T>(new T(std::forward<Arg1>(arg1), std::forward<Arg2>(arg2),
									std::forward<Arg3>(arg3)));
}

template<class T, class Arg1, class Arg2, class Arg3, class Arg4>
std::unique_ptr<T> make_unique(Arg1&& arg1, Arg2&& arg2, Arg3&& arg3, Arg4&& arg4)
{
    return std::unique_ptr<T>(new T(std::forward<Arg1>(arg1), std::forward<Arg2>(arg2),
									std::forward<Arg3>(arg3), std::forward<Arg4>(arg4)));
}<commit_msg>Converted spaces to tabs in make_unique<commit_after>#pragma once

#include <memory>

// Christopher D. Canfield
// November 2013
// make_unique.hpp
// make_unique is missing from the c++11 standard.
// Adapted from Herb Sutter: http://herbsutter.com/gotw/_102/
// However, VS2012 does not support variadic templates.

template<class T, class Arg1>
std::unique_ptr<T> make_unique(Arg1&& arg1)
{
	return std::unique_ptr<T>(new T(std::forward<Arg1>(arg1)));
}

template<class T, class Arg1, class Arg2>
std::unique_ptr<T> make_unique(Arg1&& arg1, Arg2&& arg2)
{
	return std::unique_ptr<T>(new T(std::forward<Arg1>(arg1), std::forward<Arg2>(arg2)));
}

template<class T, class Arg1, class Arg2, class Arg3>
std::unique_ptr<T> make_unique(Arg1&& arg1, Arg2&& arg2, Arg3&& arg3)
{
	return std::unique_ptr<T>(new T(std::forward<Arg1>(arg1), std::forward<Arg2>(arg2),
									std::forward<Arg3>(arg3)));
}

template<class T, class Arg1, class Arg2, class Arg3, class Arg4>
std::unique_ptr<T> make_unique(Arg1&& arg1, Arg2&& arg2, Arg3&& arg3, Arg4&& arg4)
{
	return std::unique_ptr<T>(new T(std::forward<Arg1>(arg1), std::forward<Arg2>(arg2),
									std::forward<Arg3>(arg3), std::forward<Arg4>(arg4)));
}<|endoftext|>


<commit_before>///
/// @file  S2LoadBalancer.cpp
/// @brief The S2LoadBalancer evenly distributes the work load between
///        the threads in the computation of the special leaves.
///
/// Simply parallelizing the computation of the special leaves in the
/// Lagarias-Miller-Odlyzko and Deleglise-Rivat algorithms by
/// subdividing the sieve interval by the number of threads into
/// equally sized subintervals does not scale because the distribution
/// of the special leaves is highly skewed and most special leaves are
/// in the first few segments whereas later on there are very few
/// special leaves.
///
/// Based on the above observations it is clear that we need some kind
/// of load balancing in order to scale our parallel algorithm for
/// computing special leaves. Below are the ideas I used to develop a
/// load balancing algorithm that achieves a high load balance by
/// dynamically increasing or decreasing the interval size based on
/// the relative standard deviation of the thread run-times.
///
/// 1) Start with a tiny segment size of x^(1/3) / (log x * log log x)
///    and one segment per thread. Our algorithm uses equally sized
///    intervals, for each thread the interval_size is
///    segment_size * segments_per_thread and the threads process
///    adjacent intervals i.e.
///    [base + interval_size * thread_id, base + interval_size * (thread_id + 1)].
///
/// 2) If the relative standard deviation of the thread run-times is
///    large then we know the special leaves are distributed unevenly,
///    else if the relative standard deviation is low the special
///    leaves are more evenly distributed.
///
/// 3) If the special leaves are distributed unevenly then we can
///    increase the load balance by decreasing the interval_size.
///    Contrary if the special leaves are more evenly distributed
///    we can increase the interval_size in order to improve the
///    algorithm's efficiency.
///
/// 4) We can't use a static threshold for as to when the relative
///    standard deviation is low or large as this threshold varies for
///    different PC architectures. So instead we compare the current
///    relative standard deviation to the previous one in order to
///    decide whether to increase or decrease the interval_size.
///
/// Copyright (C) 2014 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <S2LoadBalancer.hpp>
#include <aligned_vector.hpp>
#include <pmath.hpp>
#include <int128.hpp>

#include <stdint.h>
#include <algorithm>
#include <cmath>

using namespace std;
using namespace primecount;

namespace {

double get_average(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double sum = 0;

  for (size_t i = 0; i < n; i++)
    sum += timings[i];

  return sum / n;
}

double relative_standard_deviation(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double average = get_average(timings);
  double sum = 0;

  if (average == 0)
    return 0;

  for (size_t i = 0; i < n; i++)
  {
    double mean = timings[i] - average;
    sum += mean * mean;
  }

  double std_dev = sqrt(sum / max(1.0, n - 1.0));
  double rsd = 100 * std_dev / average;

  return rsd;
}

} // namespace

namespace primecount {

S2LoadBalancer::S2LoadBalancer(maxint_t x, int64_t z, int64_t threads) :
  x_((double) x),
  z_((double) z),
  rsd_(40),
  avg_seconds_(0),
  count_(0)
{
  double log_threads = max(1.0, log((double) threads));
  decrease_dividend_ = max(0.5, log_threads / 3);
  min_seconds_ = 0.02 * log_threads;
  max_size_ = next_power_of_2(isqrt(z));
  update_min_size(log(x_) * log(log(x_)));
}

double S2LoadBalancer::get_rsd() const
{
  return rsd_;
}

int64_t S2LoadBalancer::get_min_segment_size() const
{
  return min_size_;
}

bool S2LoadBalancer::decrease_size(double seconds, double decrease) const
{
  return seconds > min_seconds_ &&
         rsd_ > decrease;
}

bool S2LoadBalancer::increase_size(double seconds, double decrease) const
{
  return seconds < avg_seconds_ &&
        !decrease_size(seconds, decrease);
}

/// Used to decide whether to use a smaller or larger
/// segment_size and/or segments_per_thread.
///
double S2LoadBalancer::get_decrease_threshold(double seconds) const
{
  double log_seconds = max(min_seconds_, log(seconds));
  double dont_decrease = min(decrease_dividend_ / (seconds * log_seconds), rsd_);
  return rsd_ + dont_decrease;
}

void S2LoadBalancer::update_avg_seconds(double seconds)
{
  seconds = max(seconds, min_seconds_);
  double dividend = avg_seconds_ * count_ + seconds;
  avg_seconds_ = dividend / (count_ + 1);
  count_++;
}

void S2LoadBalancer::update_min_size(double divisor)
{
  double size = sqrt(z_) / max(1.0, divisor);
  min_size_ = max((int64_t) (1 << 9), (int64_t) size);
  min_size_ = next_power_of_2(min_size_);
}

/// Balance the load in the computation of the special leaves
/// by dynamically adjusting the segment_size and segments_per_thread.
/// @param timings  Timings of the threads.
///
void S2LoadBalancer::update(int64_t low,
                            int64_t threads,
                            int64_t* segment_size,
                            int64_t* segments_per_thread,
                            aligned_vector<double>& timings)
{
  double seconds = get_average(timings);
  update_avg_seconds(seconds);
  double decrease_threshold = get_decrease_threshold(seconds);
  rsd_ = max(0.1, relative_standard_deviation(timings));

  // if low > sqrt(z) we use a larger min_size_ as the
  // special leaves are distributed more evenly
  if (low > max_size_)
  {
    update_min_size(log(x_));
    *segment_size = max(*segment_size, min_size_);
  }

  // 1 segment per thread
  if (*segment_size < max_size_)
  {
    if (increase_size(seconds, decrease_threshold))
      *segment_size <<= 1;
    else if (decrease_size(seconds, decrease_threshold))
      if (*segment_size > min_size_)
        *segment_size >>= 1;

    // near sqrt(z) there is a short peak of special
    // leaves so we use the minium segment size
    int64_t high = low + *segment_size * *segments_per_thread * threads;
    if (low <= max_size_ && high > max_size_)
      *segment_size = min_size_;
  }
  else // many segments per thread
  {
    double factor = decrease_threshold / rsd_;
    factor = in_between(0.5, factor, 2);
    double n = *segments_per_thread * factor;
    n = max(1.0, n);

    if ((n < *segments_per_thread && seconds > min_seconds_) ||
        (n > *segments_per_thread && seconds < avg_seconds_))
    {
      *segments_per_thread = (int) n;
    }
  }
}

} // namespace
<commit_msg>Fix comment<commit_after>///
/// @file  S2LoadBalancer.cpp
/// @brief The S2LoadBalancer evenly distributes the work load between
///        the threads in the computation of the special leaves.
///
/// Simply parallelizing the computation of the special leaves in the
/// Lagarias-Miller-Odlyzko and Deleglise-Rivat algorithms by
/// subdividing the sieve interval by the number of threads into
/// equally sized subintervals does not scale because the distribution
/// of the special leaves is highly skewed and most special leaves are
/// in the first few segments whereas later on there are very few
/// special leaves.
///
/// Based on the above observations it is clear that we need some kind
/// of load balancing in order to scale our parallel algorithm for
/// computing special leaves. Below are the ideas I used to develop a
/// load balancing algorithm that achieves a high load balance by
/// dynamically increasing or decreasing the interval size based on
/// the relative standard deviation of the thread run-times.
///
/// 1) Start with a tiny segment size of x^(1/3) / (log x * log log x)
///    and one segment per thread. Our algorithm uses equally sized
///    intervals, for each thread the interval_size is
///    segment_size * segments_per_thread and the threads process
///    adjacent intervals i.e.
///    [base + interval_size * thread_id, base + interval_size * (thread_id + 1)].
///
/// 2) If the relative standard deviation of the thread run-times is
///    large then we know the special leaves are distributed unevenly,
///    else if the relative standard deviation is low the special
///    leaves are more evenly distributed.
///
/// 3) If the special leaves are distributed unevenly then we can
///    increase the load balance by decreasing the interval_size.
///    Contrary if the special leaves are more evenly distributed
///    we can increase the interval_size in order to improve the
///    algorithm's efficiency.
///
/// 4) We can't use a static threshold for as to when the relative
///    standard deviation is low or large as this threshold varies for
///    different PC architectures. So instead we compare the current
///    relative standard deviation to the previous one in order to
///    decide whether to increase or decrease the interval_size.
///
/// Copyright (C) 2015 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <S2LoadBalancer.hpp>
#include <aligned_vector.hpp>
#include <pmath.hpp>
#include <int128.hpp>

#include <stdint.h>
#include <algorithm>
#include <cmath>

using namespace std;
using namespace primecount;

namespace {

double get_average(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double sum = 0;

  for (size_t i = 0; i < n; i++)
    sum += timings[i];

  return sum / n;
}

double relative_standard_deviation(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double average = get_average(timings);
  double sum = 0;

  if (average == 0)
    return 0;

  for (size_t i = 0; i < n; i++)
  {
    double mean = timings[i] - average;
    sum += mean * mean;
  }

  double std_dev = sqrt(sum / max(1.0, n - 1.0));
  double rsd = 100 * std_dev / average;

  return rsd;
}

} // namespace

namespace primecount {

S2LoadBalancer::S2LoadBalancer(maxint_t x, int64_t z, int64_t threads) :
  x_((double) x),
  z_((double) z),
  rsd_(40),
  avg_seconds_(0),
  count_(0)
{
  double log_threads = max(1.0, log((double) threads));
  decrease_dividend_ = max(0.5, log_threads / 3);
  min_seconds_ = 0.02 * log_threads;
  max_size_ = next_power_of_2(isqrt(z));
  update_min_size(log(x_) * log(log(x_)));
}

double S2LoadBalancer::get_rsd() const
{
  return rsd_;
}

int64_t S2LoadBalancer::get_min_segment_size() const
{
  return min_size_;
}

bool S2LoadBalancer::decrease_size(double seconds, double decrease) const
{
  return seconds > min_seconds_ &&
         rsd_ > decrease;
}

bool S2LoadBalancer::increase_size(double seconds, double decrease) const
{
  return seconds < avg_seconds_ &&
        !decrease_size(seconds, decrease);
}

/// Used to decide whether to use a smaller or larger
/// segment_size and/or segments_per_thread.
///
double S2LoadBalancer::get_decrease_threshold(double seconds) const
{
  double log_seconds = max(min_seconds_, log(seconds));
  double dont_decrease = min(decrease_dividend_ / (seconds * log_seconds), rsd_);
  return rsd_ + dont_decrease;
}

void S2LoadBalancer::update_avg_seconds(double seconds)
{
  seconds = max(seconds, min_seconds_);
  double dividend = avg_seconds_ * count_ + seconds;
  avg_seconds_ = dividend / (count_ + 1);
  count_++;
}

void S2LoadBalancer::update_min_size(double divisor)
{
  double size = sqrt(z_) / max(1.0, divisor);
  min_size_ = max((int64_t) (1 << 9), (int64_t) size);
  min_size_ = next_power_of_2(min_size_);
}

/// Balance the load in the computation of the special leaves
/// by dynamically adjusting the segment_size and segments_per_thread.
/// @param timings  Timings of the threads.
///
void S2LoadBalancer::update(int64_t low,
                            int64_t threads,
                            int64_t* segment_size,
                            int64_t* segments_per_thread,
                            aligned_vector<double>& timings)
{
  double seconds = get_average(timings);
  update_avg_seconds(seconds);
  double decrease_threshold = get_decrease_threshold(seconds);
  rsd_ = max(0.1, relative_standard_deviation(timings));

  // if low > sqrt(z) we use a larger min_size_ as the
  // special leaves are distributed more evenly
  if (low > max_size_)
  {
    update_min_size(log(x_));
    *segment_size = max(*segment_size, min_size_);
  }

  // 1 segment per thread
  if (*segment_size < max_size_)
  {
    if (increase_size(seconds, decrease_threshold))
      *segment_size <<= 1;
    else if (decrease_size(seconds, decrease_threshold))
      if (*segment_size > min_size_)
        *segment_size >>= 1;

    // near sqrt(z) there is a short peak of special
    // leaves so we use the minimum segment size
    int64_t high = low + *segment_size * *segments_per_thread * threads;
    if (low <= max_size_ && high > max_size_)
      *segment_size = min_size_;
  }
  else // many segments per thread
  {
    double factor = decrease_threshold / rsd_;
    factor = in_between(0.5, factor, 2);
    double n = *segments_per_thread * factor;
    n = max(1.0, n);

    if ((n < *segments_per_thread && seconds > min_seconds_) ||
        (n > *segments_per_thread && seconds < avg_seconds_))
    {
      *segments_per_thread = (int) n;
    }
  }
}

} // namespace
<|endoftext|>


<commit_before>#include "dynet/io.h"
#include "dynet/tensor.h"
#include "dynet/except.h"
#include "dynet/str-util.h"

using namespace std;
using namespace dynet;

// Precision required not to lose accuracy when serializing float32 to text.
// We should probably use std::hexfloat, but it's not supported by some
// older incomplete implementations of C++11.
static const int FLOAT32_PRECISION = 8;

bool valid_key(const std::string & s) {
  if (s.size() == 0) return true;
  if (s == "/") return false;
  auto it = std::find_if(s.begin(), s.end(),
                         [] (char ch) { return ch == ' ' || ch == '#';});
  return it == s.end();
}

bool valid_pc_key(const std::string & s) {
  if (s.size() == 0) return true;
  if (!(startswith(s, "/") && endswith(s, "/"))) return false;
  return valid_key(s);
}

TextFileSaver::TextFileSaver(const string & filename, bool append) :
        datastream(filename, append ? ofstream::app : ofstream::out) {
  if(!datastream)
    DYNET_RUNTIME_ERR("Could not write model to " << filename);
}

void TextFileSaver::save(const ParameterCollection & model,
                         const string & key) {
  if (!valid_pc_key(key))
    DYNET_INVALID_ARG("Key should start with, end with '/' and could not include ' ' or '#': " << key);
  const ParameterCollectionStorage & storage = model.get_storage();
  if(key.size() == 0) {
    for (auto & p : storage.params) save(*p, key);
    for (auto & p : storage.lookup_params) save(*p, key);
  } else {
    size_t strip_size = model.get_fullname().size();
    for (auto & p : storage.params)
      save(*p, key + p->name.substr(strip_size));
    for (auto & p : storage.lookup_params)
      save(*p, key + p->name.substr(strip_size));
  }
}

void TextFileSaver::save(const Parameter & param,
                         const string & key) {
  if (!valid_key(key))
    DYNET_INVALID_ARG("Key could not include ' ' or '#': " << key);
  save(*param.p, key);
}

void TextFileSaver::save(const LookupParameter & param,
                         const string & key) {
  if (!valid_key(key))
    DYNET_INVALID_ARG("Key could not include ' ' or '#': " << key);
  save(*param.p, key);
}

void TextFileSaver::save(const ParameterStorage & p,
                         const string & key) {
  std::ostringstream buffer;
  buffer.precision(FLOAT32_PRECISION);
  buffer << dynet::as_vector(p.values) << endl;
  buffer << dynet::as_vector(p.g) << endl;
  datastream << "#Parameter# " << (key.size() > 0 ? key : p.name) << ' '
    << p.dim << ' ' << buffer.str().size() << endl;
  datastream.write(buffer.str().c_str(), buffer.str().size());
}

void TextFileSaver::save(const LookupParameterStorage & p,
                         const string & key) {
  std::ostringstream buffer;
  buffer.precision(FLOAT32_PRECISION);
  buffer << dynet::as_vector(p.all_values) << endl;
  buffer << dynet::as_vector(p.all_grads) << endl;
  datastream << "#LookupParameter# " << (key.size() > 0 ? key : p.name) << ' ' << p.all_dim << ' ' << buffer.str().size() << endl;
  datastream.write(buffer.str().c_str(), buffer.str().size());
}

TextFileLoader::TextFileLoader(const string & filename) :
        dataname(filename) { }

void TextFileLoader::populate(ParameterCollection & model, const string & key) {
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  vector<float> values;
  Tensor *value_t, *grad_t;
  size_t param_id = 0, lookup_id = 0;
  ParameterCollectionStorage & storage = model.get_storage();
  string key_ = key;
  if (key_.back() != '/') key_ += "/";
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    // Skip ones that don't match
    if(key.size() != 0 && name.substr(0, key_.size()) != key_) {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
      continue;
    // Load a parameter
    } else if(type == "#Parameter#") {
      values.resize(dim.size());
      if(param_id >= storage.params.size())
        DYNET_RUNTIME_ERR("Too many parameters to load in populated model at " << name);
      ParameterStorage & param = *storage.params[param_id++];
      if(param.dim != dim)
        DYNET_RUNTIME_ERR("Dimensions of parameter " << name << " looked up from file (" << dim <<
                            ") do not match parameters to be populated (" << param.dim << ")");
      value_t = &param.values;
      grad_t = &param.g;
    // Load a lookup parameter
    } else if(type == "#LookupParameter#") {
      values.resize(dim.size());
      if(lookup_id >= storage.lookup_params.size())
        DYNET_RUNTIME_ERR("Too many lookup parameters in populated model at " << name);
      LookupParameterStorage & param = *storage.lookup_params[lookup_id++];
      if(param.all_dim != dim)
        DYNET_RUNTIME_ERR("Dimensions of lookup parameter " << name << " lookup up from file (" << dim <<
                            ") do not match parameters to be populated (" << param.all_dim << ")");
      value_t = &param.all_values;
      grad_t = &param.all_grads;
    } else {
      DYNET_RUNTIME_ERR("Bad parameter specification in model: " << line);
    }
    { getline(datastream, line); istringstream iss(line); iss >> values; }
    TensorTools::set_elements(*value_t, values);
    { getline(datastream, line); istringstream iss(line); iss >> values; }
    TensorTools::set_elements(*grad_t, values);
  }
  if(param_id != storage.params.size() || lookup_id != storage.lookup_params.size())
    DYNET_RUNTIME_ERR("Number of parameter/lookup parameter objects loaded from file (" <<
                      param_id << '/' << lookup_id << ") did not match number to be populated (" <<
                      storage.params.size() << '/' << storage.lookup_params.size() << ')');
}

void TextFileLoader::populate(Parameter & param,
                    const string & key) {
  if(key == "")
    DYNET_INVALID_ARG("TextFileLoader.populate() requires non-empty key");
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    if(type == "#Parameter#" && name == key) {
      if(param.p->dim != dim)
        DYNET_RUNTIME_ERR("Attempted to populate parameter where arguments don't match (" << param.p->dim << " != " << dim << ")");
      vector<float> values(dim.size());
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(param.get_storage().values, values);
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(param.get_storage().g, values);
      return;
    } else {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
    }
  }
  DYNET_RUNTIME_ERR("Could not find key " << key << " in the model file");
}

void TextFileLoader::populate(LookupParameter & lookup_param,
                              const string & key) {
  if(key == "")
    DYNET_INVALID_ARG("TextFileLoader.populate() requires non-empty key");
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    if(type == "#LookupParameter#" && name == key) {
      if(lookup_param.p->all_dim != dim)
        DYNET_RUNTIME_ERR("Attempted to populate lookup parameter where arguments don't match (" << lookup_param.p->all_dim << " != " << dim << ")");
      vector<float> values(dim.size());
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(lookup_param.get_storage().all_values, values);
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(lookup_param.get_storage().all_grads, values);
      return;
    } else {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
    }
  }
  DYNET_RUNTIME_ERR("Could not find key " << key << " in the model file");
}

Parameter TextFileLoader::load_param(ParameterCollection & model,
                                     const string & key) {
  if(key == "")
    DYNET_INVALID_ARG("TextFileLoader.load_param() requires non-empty key");
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    if(type == "#Parameter#" && name == key) {
      Parameter param = model.add_parameters(dim);
      param.get_storage().name = name;
      vector<float> values(dim.size());
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(param.get_storage().values, values);
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(param.get_storage().g, values);
      return param;
    } else {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
    }
  }
  DYNET_RUNTIME_ERR("Could not find key " << key << " in the model file");
}

LookupParameter TextFileLoader::load_lookup_param(ParameterCollection & model,
                                                  const string & key) {
  if(key == "")
    DYNET_INVALID_ARG("TextFileLoader.load_lookup_param() requires non-empty key");
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    if(type == "#LookupParameter#" && name == key) {
      vector<float> values(dim.size());
      size_t size = dim[dim.nd-1]; dim.nd--;
      LookupParameter lookup_param = model.add_lookup_parameters(size, dim);
      lookup_param.get_storage().name = name;
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(lookup_param.get_storage().all_values, values);
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(lookup_param.get_storage().all_grads, values);
      return lookup_param;
    } else {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
    }
  }
  DYNET_RUNTIME_ERR("Could not find key " << key << " in the model file");
}
<commit_msg>unify usage of key for populate parameterCollection in cpp end<commit_after>#include "dynet/io.h"
#include "dynet/tensor.h"
#include "dynet/except.h"
#include "dynet/str-util.h"

using namespace std;
using namespace dynet;

// Precision required not to lose accuracy when serializing float32 to text.
// We should probably use std::hexfloat, but it's not supported by some
// older incomplete implementations of C++11.
static const int FLOAT32_PRECISION = 8;

bool valid_key(const std::string & s) {
  if (s.size() == 0) return true;
  if (s == "/") return false;
  auto it = std::find_if(s.begin(), s.end(),
                         [] (char ch) { return ch == ' ' || ch == '#';});
  return it == s.end();
}

bool valid_pc_key(const std::string & s) {
  if (s.size() == 0) return true;
  if (!(startswith(s, "/")) return false;
  return valid_key(s);
}

TextFileSaver::TextFileSaver(const string & filename, bool append) :
        datastream(filename, append ? ofstream::app : ofstream::out) {
  if(!datastream)
    DYNET_RUNTIME_ERR("Could not write model to " << filename);
}

void TextFileSaver::save(const ParameterCollection & model,
                         const string & key) {
  if (!valid_pc_key(key))
    DYNET_INVALID_ARG("Key should start with '/' and could not include ' ' or '#': " << key);
  string key_ = key;
  if (key_.back() != '/') key_ += "/";
  const ParameterCollectionStorage & storage = model.get_storage();
  if(key.size() == 0) {
    for (auto & p : storage.params) save(*p, key);
    for (auto & p : storage.lookup_params) save(*p, key);
  } else {
    size_t strip_size = model.get_fullname().size();
    for (auto & p : storage.params)
      save(*p, key_ + p->name.substr(strip_size));
    for (auto & p : storage.lookup_params)
      save(*p, key_ + p->name.substr(strip_size));
  }
}

void TextFileSaver::save(const Parameter & param,
                         const string & key) {
  if (!valid_key(key))
    DYNET_INVALID_ARG("Key could not include ' ' or '#': " << key);
  save(*param.p, key);
}

void TextFileSaver::save(const LookupParameter & param,
                         const string & key) {
  if (!valid_key(key))
    DYNET_INVALID_ARG("Key could not include ' ' or '#': " << key);
  save(*param.p, key);
}

void TextFileSaver::save(const ParameterStorage & p,
                         const string & key) {
  std::ostringstream buffer;
  buffer.precision(FLOAT32_PRECISION);
  buffer << dynet::as_vector(p.values) << endl;
  buffer << dynet::as_vector(p.g) << endl;
  datastream << "#Parameter# " << (key.size() > 0 ? key : p.name) << ' '
    << p.dim << ' ' << buffer.str().size() << endl;
  datastream.write(buffer.str().c_str(), buffer.str().size());
}

void TextFileSaver::save(const LookupParameterStorage & p,
                         const string & key) {
  std::ostringstream buffer;
  buffer.precision(FLOAT32_PRECISION);
  buffer << dynet::as_vector(p.all_values) << endl;
  buffer << dynet::as_vector(p.all_grads) << endl;
  datastream << "#LookupParameter# " << (key.size() > 0 ? key : p.name) << ' ' << p.all_dim << ' ' << buffer.str().size() << endl;
  datastream.write(buffer.str().c_str(), buffer.str().size());
}

TextFileLoader::TextFileLoader(const string & filename) :
        dataname(filename) { }

void TextFileLoader::populate(ParameterCollection & model, const string & key) {
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  vector<float> values;
  Tensor *value_t, *grad_t;
  size_t param_id = 0, lookup_id = 0;
  ParameterCollectionStorage & storage = model.get_storage();
  string key_ = key;
  if (key_.back() != '/') key_ += "/";
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    // Skip ones that don't match
    if(key.size() != 0 && name.substr(0, key_.size()) != key_) {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
      continue;
    // Load a parameter
    } else if(type == "#Parameter#") {
      values.resize(dim.size());
      if(param_id >= storage.params.size())
        DYNET_RUNTIME_ERR("Too many parameters to load in populated model at " << name);
      ParameterStorage & param = *storage.params[param_id++];
      if(param.dim != dim)
        DYNET_RUNTIME_ERR("Dimensions of parameter " << name << " looked up from file (" << dim <<
                            ") do not match parameters to be populated (" << param.dim << ")");
      value_t = &param.values;
      grad_t = &param.g;
    // Load a lookup parameter
    } else if(type == "#LookupParameter#") {
      values.resize(dim.size());
      if(lookup_id >= storage.lookup_params.size())
        DYNET_RUNTIME_ERR("Too many lookup parameters in populated model at " << name);
      LookupParameterStorage & param = *storage.lookup_params[lookup_id++];
      if(param.all_dim != dim)
        DYNET_RUNTIME_ERR("Dimensions of lookup parameter " << name << " lookup up from file (" << dim <<
                            ") do not match parameters to be populated (" << param.all_dim << ")");
      value_t = &param.all_values;
      grad_t = &param.all_grads;
    } else {
      DYNET_RUNTIME_ERR("Bad parameter specification in model: " << line);
    }
    { getline(datastream, line); istringstream iss(line); iss >> values; }
    TensorTools::set_elements(*value_t, values);
    { getline(datastream, line); istringstream iss(line); iss >> values; }
    TensorTools::set_elements(*grad_t, values);
  }
  if(param_id != storage.params.size() || lookup_id != storage.lookup_params.size())
    DYNET_RUNTIME_ERR("Number of parameter/lookup parameter objects loaded from file (" <<
                      param_id << '/' << lookup_id << ") did not match number to be populated (" <<
                      storage.params.size() << '/' << storage.lookup_params.size() << ')');
}

void TextFileLoader::populate(Parameter & param,
                    const string & key) {
  if(key == "")
    DYNET_INVALID_ARG("TextFileLoader.populate() requires non-empty key");
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    if(type == "#Parameter#" && name == key) {
      if(param.p->dim != dim)
        DYNET_RUNTIME_ERR("Attempted to populate parameter where arguments don't match (" << param.p->dim << " != " << dim << ")");
      vector<float> values(dim.size());
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(param.get_storage().values, values);
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(param.get_storage().g, values);
      return;
    } else {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
    }
  }
  DYNET_RUNTIME_ERR("Could not find key " << key << " in the model file");
}

void TextFileLoader::populate(LookupParameter & lookup_param,
                              const string & key) {
  if(key == "")
    DYNET_INVALID_ARG("TextFileLoader.populate() requires non-empty key");
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    if(type == "#LookupParameter#" && name == key) {
      if(lookup_param.p->all_dim != dim)
        DYNET_RUNTIME_ERR("Attempted to populate lookup parameter where arguments don't match (" << lookup_param.p->all_dim << " != " << dim << ")");
      vector<float> values(dim.size());
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(lookup_param.get_storage().all_values, values);
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(lookup_param.get_storage().all_grads, values);
      return;
    } else {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
    }
  }
  DYNET_RUNTIME_ERR("Could not find key " << key << " in the model file");
}

Parameter TextFileLoader::load_param(ParameterCollection & model,
                                     const string & key) {
  if(key == "")
    DYNET_INVALID_ARG("TextFileLoader.load_param() requires non-empty key");
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    if(type == "#Parameter#" && name == key) {
      Parameter param = model.add_parameters(dim);
      param.get_storage().name = name;
      vector<float> values(dim.size());
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(param.get_storage().values, values);
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(param.get_storage().g, values);
      return param;
    } else {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
    }
  }
  DYNET_RUNTIME_ERR("Could not find key " << key << " in the model file");
}

LookupParameter TextFileLoader::load_lookup_param(ParameterCollection & model,
                                                  const string & key) {
  if(key == "")
    DYNET_INVALID_ARG("TextFileLoader.load_lookup_param() requires non-empty key");
  ifstream datastream(dataname);
  if(!datastream) DYNET_RUNTIME_ERR("Could not read model from " << dataname);
  string line, type, name;
  Dim dim;
  size_t byte_count = 0;
  while(getline(datastream, line)) {
    { istringstream iss(line); iss >> type >> name >> dim >> byte_count; }
    if(type == "#LookupParameter#" && name == key) {
      vector<float> values(dim.size());
      size_t size = dim[dim.nd-1]; dim.nd--;
      LookupParameter lookup_param = model.add_lookup_parameters(size, dim);
      lookup_param.get_storage().name = name;
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(lookup_param.get_storage().all_values, values);
      { getline(datastream, line); istringstream iss(line); iss >> values; }
      TensorTools::set_elements(lookup_param.get_storage().all_grads, values);
      return lookup_param;
    } else {
      size_t offset = static_cast<size_t>(datastream.tellg()) + byte_count;
      datastream.seekg(offset);
    }
  }
  DYNET_RUNTIME_ERR("Could not find key " << key << " in the model file");
}
<|endoftext|>


<commit_before>///
/// @file  S2LoadBalancer.cpp
/// @brief The S2LoadBalancer evenly distributes the work load between
///        the threads in the computation of the special leaves.
///
/// Simply parallelizing the computation of the special leaves in the
/// Lagarias-Miller-Odlyzko and Deleglise-Rivat algorithms by
/// subdividing the sieve interval by the number of threads into
/// equally sized subintervals does not scale because the distribution
/// of the special leaves is highly skewed and most special leaves are
/// in the first few segments whereas later on there are very few
/// special leaves.
///
/// Based on the above observations it is clear that we need some kind
/// of load balancing in order to scale our parallel algorithm for
/// computing special leaves. Below are the ideas I used to develop a
/// load balancing algorithm that achieves a high load balance by
/// dynamically increasing or decreasing the interval size based on
/// the relative standard deviation of the thread run-times.
///
/// 1) Start with a tiny segment size of x^(1/3) / (log x * log log x)
///    and one segment per thread. Our algorithm uses equally sized
///    intervals, for each thread the interval_size is
///    segment_size * segments_per_thread and the threads process
///    adjacent intervals i.e.
///    [base + interval_size * thread_id, base + interval_size * (thread_id + 1)].
///
/// 2) If the relative standard deviation of the thread run-times is
///    large then we know the special leaves are distributed unevenly,
///    else if the relative standard deviation is low the special
///    leaves are more evenly distributed.
///
/// 3) If the special leaves are distributed unevenly then we can
///    increase the load balance by decreasing the interval_size.
///    Contrary if the special leaves are more evenly distributed
///    we can increase the interval_size in order to improve the
///    algorithm's efficiency.
///
/// 4) We can't use a static threshold for as to when the relative
///    standard deviation is low or large as this threshold varies for
///    different PC architectures. So instead we compare the current
///    relative standard deviation to the previous one in order to
///    decide whether to increase or decrease the interval_size.
///
/// Copyright (C) 2016 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <S2LoadBalancer.hpp>
#include <primecount-internal.hpp>
#include <aligned_vector.hpp>
#include <pmath.hpp>
#include <int128.hpp>

#include <stdint.h>
#include <algorithm>
#include <cmath>

using namespace std;
using namespace primecount;

namespace {

double get_average(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double sum = 0;

  for (size_t i = 0; i < n; i++)
    sum += timings[i];

  return sum / n;
}

double relative_standard_deviation(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double average = get_average(timings);
  double sum = 0;

  if (average == 0)
    return 0;

  for (size_t i = 0; i < n; i++)
  {
    double mean = timings[i] - average;
    sum += mean * mean;
  }

  double std_dev = sqrt(sum / max(1.0, n - 1.0));
  double rsd = 100 * std_dev / average;

  return rsd;
}

} // namespace

namespace primecount {

S2LoadBalancer::S2LoadBalancer(maxint_t x,
                               int64_t y,
                               int64_t z,
                               int64_t threads) :
  x_((double) x),
  y_((double) y),
  z_((double) z),
  rsd_(40),
  avg_seconds_(0),
  count_(0),
  sqrtz_(isqrt(z))
{
  init(threads);
}

S2LoadBalancer::S2LoadBalancer(maxint_t x,
                               int64_t y,
                               int64_t z,
                               int64_t threads,
                               double rsd) :
  x_((double) x),
  y_((double) y),
  z_((double) z),
  rsd_(rsd),
  avg_seconds_(0),
  count_(0),
  sqrtz_(isqrt(z))
{
  init(threads);
}

void S2LoadBalancer::init(int64_t threads)
{
  // determined by benchmarking
  double log_threads = max(1.0, log((double) threads));
  decrease_dividend_ = max(0.5, log_threads / 3);
  min_seconds_ = 0.02 * log_threads;
  update_min_size(log(x_) * log(log(x_)));

  double alpha = get_alpha(x_, y_);
  smallest_hard_leaf_ = (int64_t) (x_ / (y_ * sqrt(alpha) * iroot<6>(x_)));
}

double S2LoadBalancer::get_rsd() const
{
  return rsd_;
}

int64_t S2LoadBalancer::get_min_segment_size() const
{
  return min_size_;
}

bool S2LoadBalancer::decrease_size(double seconds,
                                   double decrease) const
{
  return seconds > min_seconds_ &&
         rsd_ > decrease;
}

bool S2LoadBalancer::increase_size(double seconds,
                                   double decrease) const
{
  return seconds < avg_seconds_ &&
        !decrease_size(seconds, decrease);
}

/// Used to decide whether to use a smaller or larger
/// segment_size and/or segments_per_thread.
///
double S2LoadBalancer::get_decrease_threshold(double seconds) const
{
  double log_seconds = max(min_seconds_, log(seconds));
  double dont_decrease = min(decrease_dividend_ / (seconds * log_seconds), rsd_);
  return rsd_ + dont_decrease;
}

void S2LoadBalancer::update_avg_seconds(double seconds)
{
  seconds = max(seconds, min_seconds_);
  double dividend = avg_seconds_ * count_ + seconds;
  avg_seconds_ = dividend / ++count_;
}

void S2LoadBalancer::update_min_size(double divisor)
{
  int64_t size = (int64_t) (sqrtz_ / max(1.0, divisor));
  int64_t min_size = 1 << 9;
  min_size_ = max(size, min_size);
  min_size_ = next_power_of_2(min_size_);
}

/// Balance the load in the computation of the special leaves
/// by dynamically adjusting the segment_size and segments_per_thread.
/// @param timings  Timings of the threads.
///
void S2LoadBalancer::update(int64_t low,
                            int64_t threads,
                            int64_t* segment_size,
                            int64_t* segments_per_thread,
                            aligned_vector<double>& timings)
{
  double seconds = get_average(timings);
  update_avg_seconds(seconds);
  double decrease_threshold = get_decrease_threshold(seconds);
  rsd_ = max(0.1, relative_standard_deviation(timings));

  // 1 segment per thread
  if (*segment_size < sqrtz_)
  {
    if (increase_size(seconds, decrease_threshold))
      *segment_size <<= 1;
    else if (decrease_size(seconds, decrease_threshold))
      if (*segment_size > min_size_)
        *segment_size >>= 1;
  }
  // many segments per thread
  else if (low > smallest_hard_leaf_)
  {
    double factor = decrease_threshold / rsd_;
    factor = in_between(0.5, factor, 2);
    double n = *segments_per_thread * factor;
    n = max(1.0, n);

    if ((n < *segments_per_thread && seconds > min_seconds_) ||
        (n > *segments_per_thread && seconds < avg_seconds_))
    {
      *segments_per_thread = (int) n;
    }
  }

  int64_t interval_per_thread = *segment_size * *segments_per_thread;
  int64_t high = low + interval_per_thread * threads;

  // near smallest_hard_leaf_ the hard special leaves
  // are distributed unevenly so use min_size_
  if (low <= smallest_hard_leaf_ &&
      high > smallest_hard_leaf_)
  {
    *segment_size = min_size_;
    interval_per_thread = *segment_size * *segments_per_thread;
    high = low + interval_per_thread * threads;
  }

  // slightly increase min_size_
  if (high >= smallest_hard_leaf_)
  {
    update_min_size(log(y_));
    *segment_size = max(min_size_, *segment_size);
  }
}

} // namespace
<commit_msg>Refactoring<commit_after>///
/// @file  S2LoadBalancer.cpp
/// @brief The S2LoadBalancer evenly distributes the work load between
///        the threads in the computation of the special leaves.
///
/// Simply parallelizing the computation of the special leaves in the
/// Lagarias-Miller-Odlyzko and Deleglise-Rivat algorithms by
/// subdividing the sieve interval by the number of threads into
/// equally sized subintervals does not scale because the distribution
/// of the special leaves is highly skewed and most special leaves are
/// in the first few segments whereas later on there are very few
/// special leaves.
///
/// Based on the above observations it is clear that we need some kind
/// of load balancing in order to scale our parallel algorithm for
/// computing special leaves. Below are the ideas I used to develop a
/// load balancing algorithm that achieves a high load balance by
/// dynamically increasing or decreasing the interval size based on
/// the relative standard deviation of the thread run-times.
///
/// 1) Start with a tiny segment size of x^(1/3) / (log x * log log x)
///    and one segment per thread. Our algorithm uses equally sized
///    intervals, for each thread the interval_size is
///    segment_size * segments_per_thread and the threads process
///    adjacent intervals i.e.
///    [base + interval_size * thread_id, base + interval_size * (thread_id + 1)].
///
/// 2) If the relative standard deviation of the thread run-times is
///    large then we know the special leaves are distributed unevenly,
///    else if the relative standard deviation is low the special
///    leaves are more evenly distributed.
///
/// 3) If the special leaves are distributed unevenly then we can
///    increase the load balance by decreasing the interval_size.
///    Contrary if the special leaves are more evenly distributed
///    we can increase the interval_size in order to improve the
///    algorithm's efficiency.
///
/// 4) We can't use a static threshold for as to when the relative
///    standard deviation is low or large as this threshold varies for
///    different PC architectures. So instead we compare the current
///    relative standard deviation to the previous one in order to
///    decide whether to increase or decrease the interval_size.
///
/// Copyright (C) 2016 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <S2LoadBalancer.hpp>
#include <primecount-internal.hpp>
#include <aligned_vector.hpp>
#include <pmath.hpp>
#include <int128.hpp>

#include <stdint.h>
#include <algorithm>
#include <cmath>

using namespace std;
using namespace primecount;

namespace {

double get_average(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double sum = 0;

  for (size_t i = 0; i < n; i++)
    sum += timings[i];

  return sum / n;
}

double relative_standard_deviation(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double average = get_average(timings);
  double sum = 0;

  if (average == 0)
    return 0;

  for (size_t i = 0; i < n; i++)
  {
    double mean = timings[i] - average;
    sum += mean * mean;
  }

  double std_dev = sqrt(sum / max(1.0, n - 1.0));
  double rsd = 100 * std_dev / average;

  return rsd;
}

} // namespace

namespace primecount {

S2LoadBalancer::S2LoadBalancer(maxint_t x,
                               int64_t y,
                               int64_t z,
                               int64_t threads) :
  x_((double) x),
  y_((double) y),
  z_((double) z),
  rsd_(40),
  avg_seconds_(0),
  count_(0),
  sqrtz_(isqrt(z))
{
  init(threads);
}

S2LoadBalancer::S2LoadBalancer(maxint_t x,
                               int64_t y,
                               int64_t z,
                               int64_t threads,
                               double rsd) :
  x_((double) x),
  y_((double) y),
  z_((double) z),
  rsd_(rsd),
  avg_seconds_(0),
  count_(0),
  sqrtz_(isqrt(z))
{
  init(threads);
}

void S2LoadBalancer::init(int64_t threads)
{
  // determined by benchmarking
  double log_threads = max(1.0, log((double) threads));
  decrease_dividend_ = max(0.5, log_threads / 3);
  min_seconds_ = 0.02 * log_threads;
  update_min_size(log(x_) * log(log(x_)));

  double alpha = get_alpha(x_, y_);
  smallest_hard_leaf_ = (int64_t) (x_ / (y_ * sqrt(alpha) * iroot<6>(x_)));
}

double S2LoadBalancer::get_rsd() const
{
  return rsd_;
}

int64_t S2LoadBalancer::get_min_segment_size() const
{
  return min_size_;
}

bool S2LoadBalancer::decrease_size(double seconds,
                                   double decrease) const
{
  return seconds > min_seconds_ &&
         rsd_ > decrease;
}

bool S2LoadBalancer::increase_size(double seconds,
                                   double decrease) const
{
  return seconds < avg_seconds_ &&
        !decrease_size(seconds, decrease);
}

/// Used to decide whether to use a smaller or larger
/// segment_size and/or segments_per_thread.
///
double S2LoadBalancer::get_decrease_threshold(double seconds) const
{
  double log_seconds = max(min_seconds_, log(seconds));
  double dont_decrease = min(decrease_dividend_ / (seconds * log_seconds), rsd_);
  return rsd_ + dont_decrease;
}

void S2LoadBalancer::update_avg_seconds(double seconds)
{
  seconds = max(seconds, min_seconds_);
  double dividend = avg_seconds_ * count_ + seconds;
  avg_seconds_ = dividend / ++count_;
}

void S2LoadBalancer::update_min_size(double divisor)
{
  int64_t size = (int64_t) (sqrtz_ / max(1.0, divisor));
  int64_t min_size = 1 << 9;
  min_size_ = max(size, min_size);
  min_size_ = next_power_of_2(min_size_);
}

/// Balance the load in the computation of the special leaves
/// by dynamically adjusting the segment_size and segments_per_thread.
/// @param timings  Timings of the threads.
///
void S2LoadBalancer::update(int64_t low,
                            int64_t threads,
                            int64_t* segment_size,
                            int64_t* segments_per_thread,
                            aligned_vector<double>& timings)
{
  double seconds = get_average(timings);
  update_avg_seconds(seconds);
  double decrease_threshold = get_decrease_threshold(seconds);
  rsd_ = max(0.1, relative_standard_deviation(timings));

  // 1 segment per thread
  if (*segment_size < sqrtz_)
  {
    if (increase_size(seconds, decrease_threshold))
      *segment_size <<= 1;
    else if (decrease_size(seconds, decrease_threshold))
      if (*segment_size > min_size_)
        *segment_size >>= 1;
  }
  // many segments per thread
  else if (low > smallest_hard_leaf_)
    update(segments_per_thread, decrease_threshold, seconds);

  int64_t interval_per_thread = *segment_size * *segments_per_thread;
  int64_t high = low + interval_per_thread * threads;

  // near smallest_hard_leaf_ the hard special leaves
  // are distributed unevenly so use min_size_
  if (low <= smallest_hard_leaf_ &&
      high > smallest_hard_leaf_)
  {
    *segment_size = min_size_;
    interval_per_thread = *segment_size * *segments_per_thread;
    high = low + interval_per_thread * threads;
  }

  // slightly increase min_size_
  if (high >= smallest_hard_leaf_)
  {
    update_min_size(log(y_));
    *segment_size = max(min_size_, *segment_size);
  }
}

/// Increase the number of segments per thread if the previous
/// thread run-times are close, otherwise decrease the
/// number of segments per thread.
///
void S2LoadBalancer::update(int64_t* segments_per_thread,
                            double decrease_threshold,
                            double seconds)
{
  double factor = decrease_threshold / rsd_;
  factor = in_between(0.5, factor, 2);
  double n = *segments_per_thread * factor;
  n = max(1.0, n);

  if ((n < *segments_per_thread && seconds > min_seconds_) ||
      (n > *segments_per_thread && seconds < avg_seconds_))
  {
    *segments_per_thread = (int64_t) n;
  }
}

} // namespace
<|endoftext|>


<commit_before>#include "SensorDataReceiver.h"

#include <OpenALRF/Communication/SensorBusTypes.h>
#include <Jumpropes/Functions.h>
#include "../System/Configuration.h"
#include "../System/Modules.h"

AudacityRover::SensorDataReceiver::SensorDataReceiver(const string AIPAddress)
{
   Address.setValue(AIPAddress);
}

bool AudacityRover::SensorDataReceiver::Connect()
{
   Disconnect();

   Jumpropes::LookupObject Lookup;
   Jumpropes::JRresolveIP(&Address, &Lookup);

   Connection.getRemoteAddress()->setValue(Lookup.getAddress(0));
   Connection.remotePort.set(Configuration::Instance()->SensorServerPort);

   if (Connection.connect())
   {
      Thread = new SensorDataConnection(&Connection);
      Thread->start();

      return true;
   }
   else
   {
      return false;
   }
}

void AudacityRover::SensorDataReceiver::Disconnect()
{
   if (Thread != nullptr)
   {
      LOGFUNCTION();
      Thread->setSocket(nullptr);

      Connection.disconnect();
      delete Thread;
      Thread = nullptr;
   }
}

bool AudacityRover::SensorDataReceiver::IsConnected()
{
   return ((Thread != nullptr) && Connection.isConnected());
}

AudacityRover::SensorDataConnection::SensorDataConnection(Jumpropes::BaseSocket * aSocket) : Jumpropes::ThreadedConnection(aSocket)
{
}

void AudacityRover::SensorDataConnection::newMessageReceived(const String * sMessage)
{
   if (sMessage->getLength() == 37)
   {
      char *Data = sMessage->getValue();
      if ((Data[0] == 1) && (Data[36] == 23))
      {
         // only allow valid sensor packets
         OpenALRF::SensorBusData3D SensorData;
         SensorData.Origin = OpenALRF::sensoriRemote;
         SensorData.ID = Data[1];
         SensorData.Type = static_cast<OpenALRF::sensor_t>(Data[2]);
         SensorData.UnitUsed = static_cast<OpenALRF::sensorunit_t>(Data[3]);

         memcpy(&SensorData.Data.Timestamp, Data + 4, 8);
         memcpy(&SensorData.Data.Data1, Data + 12, 8);
         memcpy(&SensorData.Data.Data2, Data + 20, 8);
         memcpy(&SensorData.Data.Data3, Data + 28, 8);

         Modules::Instance()->SensorBus->Broadcast(SensorData);
      }
   }
}
<commit_msg>fix to resolve hostnames better under linux<commit_after>#include "SensorDataReceiver.h"

#include <OpenALRF/Communication/SensorBusTypes.h>
#include <Jumpropes/Functions.h>
#include "../System/Configuration.h"
#include "../System/Modules.h"

AudacityRover::SensorDataReceiver::SensorDataReceiver(const string AIPAddress)
{
   if (AIPAddress.find('.'))
   {
      Address.setValue(AIPAddress);
   }
   else
   {
      Address.setValue(AIPAddress + ".local");
   }
}

bool AudacityRover::SensorDataReceiver::Connect()
{
   Disconnect();

   Jumpropes::LookupObject Lookup;
   Jumpropes::JRresolveIP(&Address, &Lookup);

   Connection.getRemoteAddress()->setValue(Lookup.getAddress(0));
   Connection.remotePort.set(Configuration::Instance()->SensorServerPort);

   if (Connection.connect())
   {
      Thread = new SensorDataConnection(&Connection);
      Thread->start();

      return true;
   }
   else
   {
      return false;
   }
}

void AudacityRover::SensorDataReceiver::Disconnect()
{
   if (Thread != nullptr)
   {
      LOGFUNCTION();
      Thread->setSocket(nullptr);

      Connection.disconnect();
      delete Thread;
      Thread = nullptr;
   }
}

bool AudacityRover::SensorDataReceiver::IsConnected()
{
   return ((Thread != nullptr) && Connection.isConnected());
}

AudacityRover::SensorDataConnection::SensorDataConnection(Jumpropes::BaseSocket * aSocket) : Jumpropes::ThreadedConnection(aSocket)
{
}

void AudacityRover::SensorDataConnection::newMessageReceived(const String * sMessage)
{
   if (sMessage->getLength() == 37)
   {
      char *Data = sMessage->getValue();
      if ((Data[0] == 1) && (Data[36] == 23))
      {
         // only allow valid sensor packets
         OpenALRF::SensorBusData3D SensorData;
         SensorData.Origin = OpenALRF::sensoriRemote;
         SensorData.ID = Data[1];
         SensorData.Type = static_cast<OpenALRF::sensor_t>(Data[2]);
         SensorData.UnitUsed = static_cast<OpenALRF::sensorunit_t>(Data[3]);

         memcpy(&SensorData.Data.Timestamp, Data + 4, 8);
         memcpy(&SensorData.Data.Data1, Data + 12, 8);
         memcpy(&SensorData.Data.Data2, Data + 20, 8);
         memcpy(&SensorData.Data.Data3, Data + 28, 8);

         Modules::Instance()->SensorBus->Broadcast(SensorData);
      }
   }
}
<|endoftext|>


<commit_before>#include <iostream>

#include <gtest/gtest.h>

#include "drake/systems/plants/RigidBodyTree.h"

namespace drake {
namespace systems {
namespace plants {
namespace test {
namespace {

class RigidBodyTreeTest : public ::testing::Test {
 protected:
  virtual void SetUp() {
    // NOTE: The code below is inefficient because it requires two heap
    // allocations, one to create the object and another to save it
    // in a shared_ptr. We chose to do it this way instead of using
    // std::allocate_shared, which would only require one heap allocation, to
    // avoid needing a custom allocator for RigidBodyTree and RigidBody. We feel
    // that the additional heap allocation overhead is acceptable since the
    // allocations are being done at startup only.
    //
    // For additional information, see:
    // https://github.com/RobotLocomotion/drake/issues/1854

    // Instantiates the rigid body tree.
    tree = std::shared_ptr<RigidBodyTree>(new RigidBodyTree());

    // Defines a four rigid bodies.
    r1b1 = std::shared_ptr<RigidBody>(new RigidBody());
    r1b1->model_name = "robot1";
    r1b1->linkname = "body1";

    r2b1 = std::shared_ptr<RigidBody>(new RigidBody());
    r2b1->model_name = "robot2";
    r2b1->linkname = "body1";

    r3b1 = std::shared_ptr<RigidBody>(new RigidBody());
    r3b1->model_name = "robot3";
    r3b1->linkname = "body1";

    r4b1 = std::shared_ptr<RigidBody>(new RigidBody());
    r4b1->model_name = "robot4";
    r4b1->linkname = "body1";
  }

 public:
  std::shared_ptr<RigidBodyTree> tree;
  std::shared_ptr<RigidBody> r1b1;
  std::shared_ptr<RigidBody> r2b1;
  std::shared_ptr<RigidBody> r3b1;
  std::shared_ptr<RigidBody> r4b1;
};

TEST_F(RigidBodyTreeTest, TestAddFloatingJointNoOffset) {
  // Adds rigid bodies r1b1 and r2b1 to the rigid body tree and verify they can
  // be found.
  tree->add_rigid_body(r1b1);
  tree->add_rigid_body(r2b1);

  EXPECT_TRUE(tree->findLink("body1", "robot1") != nullptr);
  EXPECT_TRUE(tree->findLink("body1", "robot2") != nullptr);
  EXPECT_TRUE(tree->findLink("body2", "robot1") == nullptr);
  EXPECT_TRUE(tree->findLink("body2", "robot2") == nullptr);

  // Adds floating joints that connect r1b1 and r2b1 to the rigid body tree's
  // world link at zero offset.
  tree->AddFloatingJoint(DrakeJoint::QUATERNION,
                        {r1b1->body_index, r2b1->body_index});

  // Verfies that the two rigid bodies are located in the correct place.
  const DrakeJoint& jointR1B1 = tree->findLink("body1", "robot1")->getJoint();
  EXPECT_TRUE(jointR1B1.isFloating());
  EXPECT_TRUE(jointR1B1.getTransformToParentBody().matrix() ==
              Eigen::Isometry3d::Identity().matrix());

  const DrakeJoint& jointR2B1 = tree->findLink("body1", "robot2")->getJoint();
  EXPECT_TRUE(jointR2B1.isFloating());
  EXPECT_TRUE(jointR2B1.getTransformToParentBody().matrix() ==
              Eigen::Isometry3d::Identity().matrix());
}

TEST_F(RigidBodyTreeTest, TestAddFloatingJointWithOffset) {
  // Adds rigid bodies r1b1 and r2b1 to the rigid body tree.
  tree->add_rigid_body(r1b1);
  tree->add_rigid_body(r2b1);

  // Adds floating joints that connect r1b1 and r2b1 to the rigid body tree's
  // world link at offset x = 1, y = 1, z = 1.
  Eigen::Isometry3d T_r1and2_to_world;
  {
    Eigen::Vector3d xyz, rpy;
    xyz << 1, 1, 1;
    rpy = Eigen::Vector3d::Zero();
    T_r1and2_to_world.matrix() << rpy2rotmat(rpy), xyz, 0, 0, 0, 1;
  }

  auto weld_to_frame = std::allocate_shared<RigidBodyFrame>(
      Eigen::aligned_allocator<RigidBodyFrame>(), "world", nullptr,
      T_r1and2_to_world);

  tree->AddFloatingJoint(DrakeJoint::QUATERNION,
                        {r1b1->body_index, r2b1->body_index}, weld_to_frame);

  // Verfies that the two rigid bodies are located in the correct place.
  const DrakeJoint& jointR1B1 = tree->findLink("body1", "robot1")->getJoint();
  EXPECT_TRUE(jointR1B1.isFloating());
  EXPECT_TRUE(jointR1B1.getTransformToParentBody().matrix() ==
              T_r1and2_to_world.matrix());

  const DrakeJoint& jointR2B1 = tree->findLink("body1", "robot2")->getJoint();
  EXPECT_TRUE(jointR2B1.isFloating());
  EXPECT_TRUE(jointR2B1.getTransformToParentBody().matrix() ==
              T_r1and2_to_world.matrix());
}

TEST_F(RigidBodyTreeTest, TestAddFloatingJointWeldToLink) {
  // Adds rigid body r1b1 to the rigid body tree and welds it to the world with
  // zero offset. Verifies that it is in the correct place.
  tree->add_rigid_body(r1b1);

  tree->AddFloatingJoint(DrakeJoint::QUATERNION, {r1b1->body_index});

  // Adds rigid body r2b1 to the rigid body tree and welds it to r1b1 with
  // offset x = 1, y = 1, z = 1. Verifies that it is in the correct place.
  tree->add_rigid_body(r2b1);

  Eigen::Isometry3d T_r2_to_r1;
  {
    Eigen::Vector3d xyz, rpy;
    xyz << 1, 1, 1;
    rpy = Eigen::Vector3d::Zero();
    T_r2_to_r1.matrix() << rpy2rotmat(rpy), xyz, 0, 0, 0, 1;
  }

  auto r2b1_weld = std::allocate_shared<RigidBodyFrame>(
      Eigen::aligned_allocator<RigidBodyFrame>(), "body1",
      tree->findLink("body1", "robot1"), T_r2_to_r1);

  tree->AddFloatingJoint(DrakeJoint::QUATERNION, {r2b1->body_index}, r2b1_weld);

  // Adds rigid body r3b1 and r4b1 to the rigid body tree and welds it to r2b1
  // with offset x = 2, y = 2, z = 2. Verifies that it is in the correct place.
  tree->add_rigid_body(r3b1);
  tree->add_rigid_body(r4b1);

  Eigen::Isometry3d T_r3_and_r4_to_r2;
  {
    Eigen::Vector3d xyz, rpy;
    xyz << 2, 2, 2;
    rpy = Eigen::Vector3d::Zero();
    T_r3_and_r4_to_r2.matrix() << rpy2rotmat(rpy), xyz, 0, 0, 0, 1;
  }

  auto r3b1_and_r4b1_weld = std::allocate_shared<RigidBodyFrame>(
      Eigen::aligned_allocator<RigidBodyFrame>(), "body1",
      tree->findLink("body1", "robot2"), T_r3_and_r4_to_r2);

  tree->AddFloatingJoint(DrakeJoint::QUATERNION,
                        {r3b1->body_index, r4b1->body_index},
                        r3b1_and_r4b1_weld);

  EXPECT_TRUE(tree->findLink("body1", "robot1")
                  ->getJoint()
                  .getTransformToParentBody()
                  .matrix() == Eigen::Isometry3d::Identity().matrix());

  EXPECT_TRUE(tree->findLink("body1", "robot2")
                  ->getJoint()
                  .getTransformToParentBody()
                  .matrix() == T_r2_to_r1.matrix());

  EXPECT_TRUE(tree->findLink("body1", "robot3")
                  ->getJoint()
                  .getTransformToParentBody()
                  .matrix() == T_r3_and_r4_to_r2.matrix());

  EXPECT_TRUE(tree->findLink("body1", "robot4")
                  ->getJoint()
                  .getTransformToParentBody()
                  .matrix() == T_r3_and_r4_to_r2.matrix());
}

}  // namespace
}  // namespace test
}  // namespace plants
}  // namespace systems
}  // namespace drake
<commit_msg>Updated comment to be more accurate.<commit_after>#include <iostream>

#include <gtest/gtest.h>

#include "drake/systems/plants/RigidBodyTree.h"

namespace drake {
namespace systems {
namespace plants {
namespace test {
namespace {

class RigidBodyTreeTest : public ::testing::Test {
 protected:
  virtual void SetUp() {
    // NOTE: The code below is inefficient because it requires two heap
    // allocations, one to create the object and another to create the
    // shared_ptr. We chose to do it this way instead of using
    // std::allocate_shared, which would only require one heap allocation, to
    // avoid needing a custom allocator for RigidBodyTree and RigidBody. We feel
    // that the additional heap allocation overhead is acceptable since the
    // allocations are being done at startup only.
    //
    // For additional information, see:
    // https://github.com/RobotLocomotion/drake/issues/1854

    // Instantiates the rigid body tree.
    tree = std::shared_ptr<RigidBodyTree>(new RigidBodyTree());

    // Defines a four rigid bodies.
    r1b1 = std::shared_ptr<RigidBody>(new RigidBody());
    r1b1->model_name = "robot1";
    r1b1->linkname = "body1";

    r2b1 = std::shared_ptr<RigidBody>(new RigidBody());
    r2b1->model_name = "robot2";
    r2b1->linkname = "body1";

    r3b1 = std::shared_ptr<RigidBody>(new RigidBody());
    r3b1->model_name = "robot3";
    r3b1->linkname = "body1";

    r4b1 = std::shared_ptr<RigidBody>(new RigidBody());
    r4b1->model_name = "robot4";
    r4b1->linkname = "body1";
  }

 public:
  std::shared_ptr<RigidBodyTree> tree;
  std::shared_ptr<RigidBody> r1b1;
  std::shared_ptr<RigidBody> r2b1;
  std::shared_ptr<RigidBody> r3b1;
  std::shared_ptr<RigidBody> r4b1;
};

TEST_F(RigidBodyTreeTest, TestAddFloatingJointNoOffset) {
  // Adds rigid bodies r1b1 and r2b1 to the rigid body tree and verify they can
  // be found.
  tree->add_rigid_body(r1b1);
  tree->add_rigid_body(r2b1);

  EXPECT_TRUE(tree->findLink("body1", "robot1") != nullptr);
  EXPECT_TRUE(tree->findLink("body1", "robot2") != nullptr);
  EXPECT_TRUE(tree->findLink("body2", "robot1") == nullptr);
  EXPECT_TRUE(tree->findLink("body2", "robot2") == nullptr);

  // Adds floating joints that connect r1b1 and r2b1 to the rigid body tree's
  // world link at zero offset.
  tree->AddFloatingJoint(DrakeJoint::QUATERNION,
                        {r1b1->body_index, r2b1->body_index});

  // Verfies that the two rigid bodies are located in the correct place.
  const DrakeJoint& jointR1B1 = tree->findLink("body1", "robot1")->getJoint();
  EXPECT_TRUE(jointR1B1.isFloating());
  EXPECT_TRUE(jointR1B1.getTransformToParentBody().matrix() ==
              Eigen::Isometry3d::Identity().matrix());

  const DrakeJoint& jointR2B1 = tree->findLink("body1", "robot2")->getJoint();
  EXPECT_TRUE(jointR2B1.isFloating());
  EXPECT_TRUE(jointR2B1.getTransformToParentBody().matrix() ==
              Eigen::Isometry3d::Identity().matrix());
}

TEST_F(RigidBodyTreeTest, TestAddFloatingJointWithOffset) {
  // Adds rigid bodies r1b1 and r2b1 to the rigid body tree.
  tree->add_rigid_body(r1b1);
  tree->add_rigid_body(r2b1);

  // Adds floating joints that connect r1b1 and r2b1 to the rigid body tree's
  // world link at offset x = 1, y = 1, z = 1.
  Eigen::Isometry3d T_r1and2_to_world;
  {
    Eigen::Vector3d xyz, rpy;
    xyz << 1, 1, 1;
    rpy = Eigen::Vector3d::Zero();
    T_r1and2_to_world.matrix() << rpy2rotmat(rpy), xyz, 0, 0, 0, 1;
  }

  auto weld_to_frame = std::allocate_shared<RigidBodyFrame>(
      Eigen::aligned_allocator<RigidBodyFrame>(), "world", nullptr,
      T_r1and2_to_world);

  tree->AddFloatingJoint(DrakeJoint::QUATERNION,
                        {r1b1->body_index, r2b1->body_index}, weld_to_frame);

  // Verfies that the two rigid bodies are located in the correct place.
  const DrakeJoint& jointR1B1 = tree->findLink("body1", "robot1")->getJoint();
  EXPECT_TRUE(jointR1B1.isFloating());
  EXPECT_TRUE(jointR1B1.getTransformToParentBody().matrix() ==
              T_r1and2_to_world.matrix());

  const DrakeJoint& jointR2B1 = tree->findLink("body1", "robot2")->getJoint();
  EXPECT_TRUE(jointR2B1.isFloating());
  EXPECT_TRUE(jointR2B1.getTransformToParentBody().matrix() ==
              T_r1and2_to_world.matrix());
}

TEST_F(RigidBodyTreeTest, TestAddFloatingJointWeldToLink) {
  // Adds rigid body r1b1 to the rigid body tree and welds it to the world with
  // zero offset. Verifies that it is in the correct place.
  tree->add_rigid_body(r1b1);

  tree->AddFloatingJoint(DrakeJoint::QUATERNION, {r1b1->body_index});

  // Adds rigid body r2b1 to the rigid body tree and welds it to r1b1 with
  // offset x = 1, y = 1, z = 1. Verifies that it is in the correct place.
  tree->add_rigid_body(r2b1);

  Eigen::Isometry3d T_r2_to_r1;
  {
    Eigen::Vector3d xyz, rpy;
    xyz << 1, 1, 1;
    rpy = Eigen::Vector3d::Zero();
    T_r2_to_r1.matrix() << rpy2rotmat(rpy), xyz, 0, 0, 0, 1;
  }

  auto r2b1_weld = std::allocate_shared<RigidBodyFrame>(
      Eigen::aligned_allocator<RigidBodyFrame>(), "body1",
      tree->findLink("body1", "robot1"), T_r2_to_r1);

  tree->AddFloatingJoint(DrakeJoint::QUATERNION, {r2b1->body_index}, r2b1_weld);

  // Adds rigid body r3b1 and r4b1 to the rigid body tree and welds it to r2b1
  // with offset x = 2, y = 2, z = 2. Verifies that it is in the correct place.
  tree->add_rigid_body(r3b1);
  tree->add_rigid_body(r4b1);

  Eigen::Isometry3d T_r3_and_r4_to_r2;
  {
    Eigen::Vector3d xyz, rpy;
    xyz << 2, 2, 2;
    rpy = Eigen::Vector3d::Zero();
    T_r3_and_r4_to_r2.matrix() << rpy2rotmat(rpy), xyz, 0, 0, 0, 1;
  }

  auto r3b1_and_r4b1_weld = std::allocate_shared<RigidBodyFrame>(
      Eigen::aligned_allocator<RigidBodyFrame>(), "body1",
      tree->findLink("body1", "robot2"), T_r3_and_r4_to_r2);

  tree->AddFloatingJoint(DrakeJoint::QUATERNION,
                        {r3b1->body_index, r4b1->body_index},
                        r3b1_and_r4b1_weld);

  EXPECT_TRUE(tree->findLink("body1", "robot1")
                  ->getJoint()
                  .getTransformToParentBody()
                  .matrix() == Eigen::Isometry3d::Identity().matrix());

  EXPECT_TRUE(tree->findLink("body1", "robot2")
                  ->getJoint()
                  .getTransformToParentBody()
                  .matrix() == T_r2_to_r1.matrix());

  EXPECT_TRUE(tree->findLink("body1", "robot3")
                  ->getJoint()
                  .getTransformToParentBody()
                  .matrix() == T_r3_and_r4_to_r2.matrix());

  EXPECT_TRUE(tree->findLink("body1", "robot4")
                  ->getJoint()
                  .getTransformToParentBody()
                  .matrix() == T_r3_and_r4_to_r2.matrix());
}

}  // namespace
}  // namespace test
}  // namespace plants
}  // namespace systems
}  // namespace drake
<|endoftext|>


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

#include <fstream>
#include <phosphor-logging/log.hpp>
#include <xyz/openbmc_project/Common/File/error.hpp>

namespace openpower
{
namespace pels
{

namespace fs = std::filesystem;
using namespace phosphor::logging;
namespace file_error = sdbusplus::xyz::openbmc_project::Common::File::Error;

Repository::Repository(const std::filesystem::path& basePath) :
    _logPath(basePath / "logs")
{
    if (!fs::exists(_logPath))
    {
        fs::create_directories(_logPath);
    }

    restore();
}

void Repository::restore()
{
    for (auto& dirEntry : fs::directory_iterator(_logPath))
    {
        try
        {
            if (!fs::is_regular_file(dirEntry.path()))
            {
                continue;
            }

            std::ifstream file{dirEntry.path()};
            std::vector<uint8_t> data{std::istreambuf_iterator<char>(file),
                                      std::istreambuf_iterator<char>()};
            file.close();

            PEL pel{data};
            if (pel.valid())
            {
                PELAttributes attributes{
                    dirEntry.path(), pel.userHeader().actionFlags(),
                    pel.hostTransmissionState(), pel.hmcTransmissionState()};

                using pelID = LogID::Pel;
                using obmcID = LogID::Obmc;
                _pelAttributes.emplace(
                    LogID(pelID(pel.id()), obmcID(pel.obmcLogID())),
                    attributes);
            }
            else
            {
                log<level::ERR>(
                    "Found invalid PEL file while restoring.  Removing.",
                    entry("FILENAME=%s", dirEntry.path().c_str()));
                fs::remove(dirEntry.path());
            }
        }
        catch (std::exception& e)
        {
            log<level::ERR>("Hit exception while restoring PEL File",
                            entry("FILENAME=%s", dirEntry.path().c_str()),
                            entry("ERROR=%s", e.what()));
        }
    }
}

std::string Repository::getPELFilename(uint32_t pelID, const BCDTime& time)
{
    char name[50];
    sprintf(name, "%.2X%.2X%.2X%.2X%.2X%.2X%.2X%.2X_%.8X", time.yearMSB,
            time.yearLSB, time.month, time.day, time.hour, time.minutes,
            time.seconds, time.hundredths, pelID);
    return std::string{name};
}

void Repository::add(std::unique_ptr<PEL>& pel)
{
    pel->setHostTransmissionState(TransmissionState::newPEL);
    pel->setHMCTransmissionState(TransmissionState::newPEL);

    auto path = _logPath / getPELFilename(pel->id(), pel->commitTime());

    write(*(pel.get()), path);

    PELAttributes attributes{path, pel->userHeader().actionFlags(),
                             pel->hostTransmissionState(),
                             pel->hmcTransmissionState()};

    using pelID = LogID::Pel;
    using obmcID = LogID::Obmc;
    _pelAttributes.emplace(LogID(pelID(pel->id()), obmcID(pel->obmcLogID())),
                           attributes);

    processAddCallbacks(*pel);
}

void Repository::write(const PEL& pel, const fs::path& path)
{
    std::ofstream file{path, std::ios::binary};

    if (!file.good())
    {
        // If this fails, the filesystem is probably full so it isn't like
        // we could successfully create yet another error log here.
        auto e = errno;
        fs::remove(path);
        log<level::ERR>("Unable to open PEL file for writing",
                        entry("ERRNO=%d", e), entry("PATH=%s", path.c_str()));
        throw file_error::Open();
    }

    auto data = pel.data();
    file.write(reinterpret_cast<const char*>(data.data()), data.size());

    if (file.fail())
    {
        // Same note as above about not being able to create an error log
        // for this case even if we wanted.
        auto e = errno;
        file.close();
        fs::remove(path);
        log<level::ERR>("Unable to write PEL file", entry("ERRNO=%d", e),
                        entry("PATH=%s", path.c_str()));
        throw file_error::Write();
    }
}

void Repository::remove(const LogID& id)
{
    auto pel = findPEL(id);
    if (pel != _pelAttributes.end())
    {
        fs::remove(pel->second.path);
        _pelAttributes.erase(pel);

        processDeleteCallbacks(id.pelID.id);
    }
}

std::optional<std::vector<uint8_t>> Repository::getPELData(const LogID& id)
{
    auto pel = findPEL(id);
    if (pel != _pelAttributes.end())
    {
        std::ifstream file{pel->second.path.c_str()};
        if (!file.good())
        {
            auto e = errno;
            log<level::ERR>("Unable to open PEL file", entry("ERRNO=%d", e),
                            entry("PATH=%s", pel->second.path.c_str()));
            throw file_error::Open();
        }

        std::vector<uint8_t> data{std::istreambuf_iterator<char>(file),
                                  std::istreambuf_iterator<char>()};
        return data;
    }

    return std::nullopt;
}

void Repository::for_each(ForEachFunc func) const
{
    for (const auto& [id, attributes] : _pelAttributes)
    {
        std::ifstream file{attributes.path};

        if (!file.good())
        {
            auto e = errno;
            log<level::ERR>("Repository::for_each: Unable to open PEL file",
                            entry("ERRNO=%d", e),
                            entry("PATH=%s", attributes.path.c_str()));
            continue;
        }

        std::vector<uint8_t> data{std::istreambuf_iterator<char>(file),
                                  std::istreambuf_iterator<char>()};
        file.close();

        PEL pel{data};

        try
        {
            if (func(pel))
            {
                break;
            }
        }
        catch (std::exception& e)
        {
            log<level::ERR>("Repository::for_each function exception",
                            entry("ERROR=%s", e.what()));
        }
    }
}

void Repository::processAddCallbacks(const PEL& pel) const
{
    for (auto& [name, func] : _addSubscriptions)
    {
        try
        {
            func(pel);
        }
        catch (std::exception& e)
        {
            log<level::ERR>("PEL Repository add callback exception",
                            entry("NAME=%s", name.c_str()),
                            entry("ERROR=%s", e.what()));
        }
    }
}

void Repository::processDeleteCallbacks(uint32_t id) const
{
    for (auto& [name, func] : _deleteSubscriptions)
    {
        try
        {
            func(id);
        }
        catch (std::exception& e)
        {
            log<level::ERR>("PEL Repository delete callback exception",
                            entry("NAME=%s", name.c_str()),
                            entry("ERROR=%s", e.what()));
        }
    }
}

std::optional<std::reference_wrapper<const Repository::PELAttributes>>
    Repository::getPELAttributes(const LogID& id) const
{
    auto pel = findPEL(id);
    if (pel != _pelAttributes.end())
    {
        return pel->second;
    }

    return std::nullopt;
}

} // namespace pels
} // namespace openpower
<commit_msg>PEL: Reset host transmission state on restart<commit_after>/**
 * Copyright © 2019 IBM Corporation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "repository.hpp"

#include <fstream>
#include <phosphor-logging/log.hpp>
#include <xyz/openbmc_project/Common/File/error.hpp>

namespace openpower
{
namespace pels
{

namespace fs = std::filesystem;
using namespace phosphor::logging;
namespace file_error = sdbusplus::xyz::openbmc_project::Common::File::Error;

Repository::Repository(const std::filesystem::path& basePath) :
    _logPath(basePath / "logs")
{
    if (!fs::exists(_logPath))
    {
        fs::create_directories(_logPath);
    }

    restore();
}

void Repository::restore()
{
    for (auto& dirEntry : fs::directory_iterator(_logPath))
    {
        try
        {
            if (!fs::is_regular_file(dirEntry.path()))
            {
                continue;
            }

            std::ifstream file{dirEntry.path()};
            std::vector<uint8_t> data{std::istreambuf_iterator<char>(file),
                                      std::istreambuf_iterator<char>()};
            file.close();

            PEL pel{data};
            if (pel.valid())
            {
                // If the host hasn't acked it, reset the host state so
                // it will get sent up again.
                if (pel.hostTransmissionState() == TransmissionState::sent)
                {
                    pel.setHostTransmissionState(TransmissionState::newPEL);
                    try
                    {
                        write(pel, dirEntry.path());
                    }
                    catch (std::exception& e)
                    {
                        log<level::ERR>(
                            "Failed to save PEL after updating host state",
                            entry("PELID=0x%X", pel.id()));
                    }
                }

                PELAttributes attributes{
                    dirEntry.path(), pel.userHeader().actionFlags(),
                    pel.hostTransmissionState(), pel.hmcTransmissionState()};

                using pelID = LogID::Pel;
                using obmcID = LogID::Obmc;
                _pelAttributes.emplace(
                    LogID(pelID(pel.id()), obmcID(pel.obmcLogID())),
                    attributes);
            }
            else
            {
                log<level::ERR>(
                    "Found invalid PEL file while restoring.  Removing.",
                    entry("FILENAME=%s", dirEntry.path().c_str()));
                fs::remove(dirEntry.path());
            }
        }
        catch (std::exception& e)
        {
            log<level::ERR>("Hit exception while restoring PEL File",
                            entry("FILENAME=%s", dirEntry.path().c_str()),
                            entry("ERROR=%s", e.what()));
        }
    }
}

std::string Repository::getPELFilename(uint32_t pelID, const BCDTime& time)
{
    char name[50];
    sprintf(name, "%.2X%.2X%.2X%.2X%.2X%.2X%.2X%.2X_%.8X", time.yearMSB,
            time.yearLSB, time.month, time.day, time.hour, time.minutes,
            time.seconds, time.hundredths, pelID);
    return std::string{name};
}

void Repository::add(std::unique_ptr<PEL>& pel)
{
    pel->setHostTransmissionState(TransmissionState::newPEL);
    pel->setHMCTransmissionState(TransmissionState::newPEL);

    auto path = _logPath / getPELFilename(pel->id(), pel->commitTime());

    write(*(pel.get()), path);

    PELAttributes attributes{path, pel->userHeader().actionFlags(),
                             pel->hostTransmissionState(),
                             pel->hmcTransmissionState()};

    using pelID = LogID::Pel;
    using obmcID = LogID::Obmc;
    _pelAttributes.emplace(LogID(pelID(pel->id()), obmcID(pel->obmcLogID())),
                           attributes);

    processAddCallbacks(*pel);
}

void Repository::write(const PEL& pel, const fs::path& path)
{
    std::ofstream file{path, std::ios::binary};

    if (!file.good())
    {
        // If this fails, the filesystem is probably full so it isn't like
        // we could successfully create yet another error log here.
        auto e = errno;
        fs::remove(path);
        log<level::ERR>("Unable to open PEL file for writing",
                        entry("ERRNO=%d", e), entry("PATH=%s", path.c_str()));
        throw file_error::Open();
    }

    auto data = pel.data();
    file.write(reinterpret_cast<const char*>(data.data()), data.size());

    if (file.fail())
    {
        // Same note as above about not being able to create an error log
        // for this case even if we wanted.
        auto e = errno;
        file.close();
        fs::remove(path);
        log<level::ERR>("Unable to write PEL file", entry("ERRNO=%d", e),
                        entry("PATH=%s", path.c_str()));
        throw file_error::Write();
    }
}

void Repository::remove(const LogID& id)
{
    auto pel = findPEL(id);
    if (pel != _pelAttributes.end())
    {
        fs::remove(pel->second.path);
        _pelAttributes.erase(pel);

        processDeleteCallbacks(id.pelID.id);
    }
}

std::optional<std::vector<uint8_t>> Repository::getPELData(const LogID& id)
{
    auto pel = findPEL(id);
    if (pel != _pelAttributes.end())
    {
        std::ifstream file{pel->second.path.c_str()};
        if (!file.good())
        {
            auto e = errno;
            log<level::ERR>("Unable to open PEL file", entry("ERRNO=%d", e),
                            entry("PATH=%s", pel->second.path.c_str()));
            throw file_error::Open();
        }

        std::vector<uint8_t> data{std::istreambuf_iterator<char>(file),
                                  std::istreambuf_iterator<char>()};
        return data;
    }

    return std::nullopt;
}

void Repository::for_each(ForEachFunc func) const
{
    for (const auto& [id, attributes] : _pelAttributes)
    {
        std::ifstream file{attributes.path};

        if (!file.good())
        {
            auto e = errno;
            log<level::ERR>("Repository::for_each: Unable to open PEL file",
                            entry("ERRNO=%d", e),
                            entry("PATH=%s", attributes.path.c_str()));
            continue;
        }

        std::vector<uint8_t> data{std::istreambuf_iterator<char>(file),
                                  std::istreambuf_iterator<char>()};
        file.close();

        PEL pel{data};

        try
        {
            if (func(pel))
            {
                break;
            }
        }
        catch (std::exception& e)
        {
            log<level::ERR>("Repository::for_each function exception",
                            entry("ERROR=%s", e.what()));
        }
    }
}

void Repository::processAddCallbacks(const PEL& pel) const
{
    for (auto& [name, func] : _addSubscriptions)
    {
        try
        {
            func(pel);
        }
        catch (std::exception& e)
        {
            log<level::ERR>("PEL Repository add callback exception",
                            entry("NAME=%s", name.c_str()),
                            entry("ERROR=%s", e.what()));
        }
    }
}

void Repository::processDeleteCallbacks(uint32_t id) const
{
    for (auto& [name, func] : _deleteSubscriptions)
    {
        try
        {
            func(id);
        }
        catch (std::exception& e)
        {
            log<level::ERR>("PEL Repository delete callback exception",
                            entry("NAME=%s", name.c_str()),
                            entry("ERROR=%s", e.what()));
        }
    }
}

std::optional<std::reference_wrapper<const Repository::PELAttributes>>
    Repository::getPELAttributes(const LogID& id) const
{
    auto pel = findPEL(id);
    if (pel != _pelAttributes.end())
    {
        return pel->second;
    }

    return std::nullopt;
}

} // namespace pels
} // namespace openpower
<|endoftext|>