<commit_before>/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.osgeo.org
 *
 * Copyright (C) 2009-2010  Sandro Santilli <strk@keybit.net>
 * Copyright (C) 2006 Refractions Research Inc.
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation.
 * See the COPYING file for more information.
 *
 ***********************************************************************
 *
 * Last port: operation/overlay/snap/LineStringSnapper.java r320 (JTS-1.12)
 *
 **********************************************************************/

#include <geos/operation/overlay/snap/LineStringSnapper.h>
#include <geos/geom/CoordinateSequence.h>
#include <geos/geom/Coordinate.h>
#include <geos/geom/CoordinateList.h>
#include <geos/util/UniqueCoordinateArrayFilter.h>
#include <geos/geom/LineSegment.h>

#include <vector>
#include <memory>

#ifndef GEOS_DEBUG
#define GEOS_DEBUG 0
#endif

#if GEOS_DEBUG
#include <iostream>
using std::cerr;
using std::endl;
#endif

//using namespace std;
using namespace geos::geom;

namespace geos {
namespace operation { // geos.operation
namespace overlay { // geos.operation.overlay
namespace snap { // geos.operation.overlay.snap

/*public*/
std::auto_ptr<Coordinate::Vect>
LineStringSnapper::snapTo(const geom::Coordinate::ConstVect& snapPts)
{
	geom::CoordinateList coordList(srcPts);

	snapVertices(coordList, snapPts);
	snapSegments(coordList, snapPts);

	return coordList.toCoordinateArray();
}

/*private*/
void
LineStringSnapper::snapVertices(geom::CoordinateList& srcCoords,
			const geom::Coordinate::ConstVect& snapPts)
{
  if ( srcCoords.empty() ) return;

#if GEOS_DEBUG
cerr << "Snapping vertices of: " << srcCoords << endl;
#endif

	using geom::CoordinateList;

	geom::Coordinate::ConstVect::const_iterator not_found = snapPts.end();

	// try snapping vertices
	// if src is a ring then don't snap final vertex
	CoordinateList::iterator it = srcCoords.begin();
	CoordinateList::iterator end = srcCoords.end();
	CoordinateList::iterator last = end; --last;
  if ( isClosed ) --end;
	for ( ; it != end; ++it )
	{
		Coordinate& srcPt = *it;

#if GEOS_DEBUG
cerr << "Checking for a snap for source coordinate " << srcPt << endl;
#endif

		geom::Coordinate::ConstVect::const_iterator found = findSnapForVertex(srcPt, snapPts);
		if ( found == not_found )
		{	// no snaps found (or no need to snap)
#if GEOS_DEBUG
cerr << " no snap found" << endl;
#endif
			continue;
		}

		assert(*found);
		const Coordinate& snapPt = *(*found);

#if GEOS_DEBUG
cerr << " found snap point " << snapPt << endl;
#endif

		// update src with snap pt
		*it = snapPt;

#if GEOS_DEBUG
cerr << " source point became " << srcPt << endl;
#endif

		// keep final closing point in synch (rings only)
		if (it == srcCoords.begin() && isClosed)
		{
			*last = snapPt;
		}
	}
}

/*private*/
Coordinate::ConstVect::const_iterator
LineStringSnapper::findSnapForVertex(const Coordinate& pt,
			const Coordinate::ConstVect& snapPts)
{

	// TODO: use std::find_if
	Coordinate::ConstVect::const_iterator end=snapPts.end();
	for ( Coordinate::ConstVect::const_iterator
			it=snapPts.begin();
			it != end;
			++it)
	{
		assert(*it);
		const Coordinate& snapPt = *(*it);

		// shouldn't we look for *all* segments to be snapped rather then a single one?
		if ( snapPt.equals2D(pt) )
		{
#if GEOS_DEBUG
cerr << " points are equal, returning not-found " << endl;
#endif
			return end;
			//continue;
		}

		double dist = snapPt.distance(pt);
#if GEOS_DEBUG
cerr << " distance from snap point " << snapPt << ": " << dist << endl;
#endif

		if ( dist < snapTolerance )
		{
#if GEOS_DEBUG
cerr << " snap point within tolerance, returning iterator to it" << endl;
#endif
			return it;
		}
	}

#if GEOS_DEBUG
cerr << " no snap point within distance, returning not-found" << endl;
#endif

	return end;
}

/*private*/
void
LineStringSnapper::snapSegments(geom::CoordinateList& srcCoords,
			const geom::Coordinate::ConstVect& snapPts)
{

  // nothing to do if there are no source coords..
  if ( srcCoords.empty() ) return;

#if GEOS_DEBUG
cerr << "Snapping segments of: " << srcCoords << endl;
#endif

	for ( Coordinate::ConstVect::const_iterator
			it=snapPts.begin(), end=snapPts.end();
			it != end;
			++it)
	{
		assert(*it);
		const Coordinate& snapPt = *(*it);

#if GEOS_DEBUG
cerr << "Checking for a segment to snap to snapPt " << snapPt << endl;
#endif

		// shouldn't we look for *all* segments to be snapped
		// rather then a single one?
		CoordinateList::iterator too_far = srcCoords.end(); --too_far;
		CoordinateList::iterator segpos =
			findSegmentToSnap(snapPt, srcCoords.begin(), too_far);
		if ( segpos == too_far)
		{
#if GEOS_DEBUG
cerr << " No segment to snap" << endl;
#endif
			continue;
		}
#if GEOS_DEBUG
cerr << " Segment to be snapped found, inserting point" << endl;
#endif
		// insert must happen one-past first point (before next point)
		++segpos;
		srcCoords.insert(segpos, snapPt);
	}

#if GEOS_DEBUG
cerr << " After segment snapping, srcCoors are: " << srcCoords << endl;
#endif

}

/*private*/
/* NOTE: this is called findSegmentIndexToSnap in JTS */
CoordinateList::iterator
LineStringSnapper::findSegmentToSnap(
			const Coordinate& snapPt,
			CoordinateList::iterator from,
			CoordinateList::iterator too_far)
{
	LineSegment seg;
	double minDist = snapTolerance+1; // make sure the first closer then
	                                  // snapTolerance is accepted
	CoordinateList::iterator match=too_far;

	// TODO: use std::find_if
	for ( ; from != too_far; ++from)
	{
		seg.p0 = *from;
		CoordinateList::iterator to = from;
		++to;
		seg.p1 = *to;

#if GEOS_DEBUG
cerr << " Checking segment " << seg << endl;
#endif

		/**                                                                              * Check if the snap pt is equal to one of
		 * the segment endpoints.
		 *
		 * If the snap pt is already in the src list,
		 * don't snap at all (unless allowSnappingToSourceVertices
		 * is set to true)
		 */
		if ( seg.p0.equals2D(snapPt) || seg.p1.equals2D(snapPt) )
		{

			if (allowSnappingToSourceVertices) {
#if GEOS_DEBUG
cerr << "   snap point matches a segment endpoint, checking next segment"
     << endl;
#endif
				continue;
			} else {
#if GEOS_DEBUG
cerr << "   snap point matches a segment endpoint, giving up seek" << endl;
#endif
				return too_far;
			}
		}

		double dist = seg.distance(snapPt);
		if ( dist < snapTolerance ) {
      if ( dist < minDist ) {
#if GEOS_DEBUG
cerr << "   snap point distance " << dist << " within tolerance "
     << snapTolerance << " and closer than previous candidate " << minDist
     << endl;
#endif
        match = from;
        minDist = dist;
      }
#if GEOS_DEBUG
      else {
cerr << "   snap point distance " << dist << " within tolerance "
     << snapTolerance << " but not closer than previous candidate " << minDist
     << endl;
      }
#endif
    }
#if GEOS_DEBUG
    else {
cerr << "   snap point distance " << dist << " bigger than tolerance "
     << snapTolerance << endl;
    }
#endif
	}

	return match;
}

} // namespace geos.operation.snap
} // namespace geos.operation.overlay
} // namespace geos.operation
} // namespace geos

<commit_msg>Indent..<commit_after>/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.osgeo.org
 *
 * Copyright (C) 2009-2010  Sandro Santilli <strk@keybit.net>
 * Copyright (C) 2006 Refractions Research Inc.
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation.
 * See the COPYING file for more information.
 *
 ***********************************************************************
 *
 * Last port: operation/overlay/snap/LineStringSnapper.java r320 (JTS-1.12)
 *
 **********************************************************************/

#include <geos/operation/overlay/snap/LineStringSnapper.h>
#include <geos/geom/CoordinateSequence.h>
#include <geos/geom/Coordinate.h>
#include <geos/geom/CoordinateList.h>
#include <geos/util/UniqueCoordinateArrayFilter.h>
#include <geos/geom/LineSegment.h>

#include <vector>
#include <memory>

#ifndef GEOS_DEBUG
#define GEOS_DEBUG 0
#endif

#if GEOS_DEBUG
#include <iostream>
using std::cerr;
using std::endl;
#endif

//using namespace std;
using namespace geos::geom;

namespace geos {
namespace operation { // geos.operation
namespace overlay { // geos.operation.overlay
namespace snap { // geos.operation.overlay.snap

/*public*/
std::auto_ptr<Coordinate::Vect>
LineStringSnapper::snapTo(const geom::Coordinate::ConstVect& snapPts)
{
	geom::CoordinateList coordList(srcPts);

	snapVertices(coordList, snapPts);
	snapSegments(coordList, snapPts);

	return coordList.toCoordinateArray();
}

/*private*/
void
LineStringSnapper::snapVertices(geom::CoordinateList& srcCoords,
			const geom::Coordinate::ConstVect& snapPts)
{
  if ( srcCoords.empty() ) return;

#if GEOS_DEBUG
cerr << "Snapping vertices of: " << srcCoords << endl;
#endif

	using geom::CoordinateList;

	geom::Coordinate::ConstVect::const_iterator not_found = snapPts.end();

	// try snapping vertices
	// if src is a ring then don't snap final vertex
	CoordinateList::iterator it = srcCoords.begin();
	CoordinateList::iterator end = srcCoords.end();
	CoordinateList::iterator last = end; --last;
  if ( isClosed ) --end;
	for ( ; it != end; ++it )
	{
		Coordinate& srcPt = *it;

#if GEOS_DEBUG
cerr << "Checking for a snap for source coordinate " << srcPt << endl;
#endif

		geom::Coordinate::ConstVect::const_iterator found = findSnapForVertex(srcPt, snapPts);
		if ( found == not_found )
		{	// no snaps found (or no need to snap)
#if GEOS_DEBUG
cerr << " no snap found" << endl;
#endif
			continue;
		}

		assert(*found);
		const Coordinate& snapPt = *(*found);

#if GEOS_DEBUG
cerr << " found snap point " << snapPt << endl;
#endif

		// update src with snap pt
		*it = snapPt;

#if GEOS_DEBUG
cerr << " source point became " << srcPt << endl;
#endif

		// keep final closing point in synch (rings only)
		if (it == srcCoords.begin() && isClosed)
		{
			*last = snapPt;
		}
	}
}

/*private*/
Coordinate::ConstVect::const_iterator
LineStringSnapper::findSnapForVertex(const Coordinate& pt,
			const Coordinate::ConstVect& snapPts)
{

	// TODO: use std::find_if
	Coordinate::ConstVect::const_iterator end=snapPts.end();
	for ( Coordinate::ConstVect::const_iterator
			it=snapPts.begin();
			it != end;
			++it)
	{
		assert(*it);
		const Coordinate& snapPt = *(*it);

		// shouldn't we look for *all* segments to be snapped rather then a single one?
		if ( snapPt.equals2D(pt) )
		{
#if GEOS_DEBUG
cerr << " points are equal, returning not-found " << endl;
#endif
			return end;
			//continue;
		}

		double dist = snapPt.distance(pt);
#if GEOS_DEBUG
cerr << " distance from snap point " << snapPt << ": " << dist << endl;
#endif

		if ( dist < snapTolerance )
		{
#if GEOS_DEBUG
cerr << " snap point within tolerance, returning iterator to it" << endl;
#endif
			return it;
		}
	}

#if GEOS_DEBUG
cerr << " no snap point within distance, returning not-found" << endl;
#endif

	return end;
}

/*private*/
void
LineStringSnapper::snapSegments(geom::CoordinateList& srcCoords,
			const geom::Coordinate::ConstVect& snapPts)
{

  // nothing to do if there are no source coords..
  if ( srcCoords.empty() ) return;

#if GEOS_DEBUG
cerr << "Snapping segments of: " << srcCoords << endl;
#endif

	for ( Coordinate::ConstVect::const_iterator
			it=snapPts.begin(), end=snapPts.end();
			it != end;
			++it)
	{
		assert(*it);
		const Coordinate& snapPt = *(*it);

#if GEOS_DEBUG
cerr << "Checking for a segment to snap to snapPt " << snapPt << endl;
#endif

		// shouldn't we look for *all* segments to be snapped
		// rather then a single one?
		CoordinateList::iterator too_far = srcCoords.end(); --too_far;
		CoordinateList::iterator segpos =
			findSegmentToSnap(snapPt, srcCoords.begin(), too_far);
		if ( segpos == too_far)
		{
#if GEOS_DEBUG
cerr << " No segment to snap" << endl;
#endif
			continue;
		}
#if GEOS_DEBUG
cerr << " Segment to be snapped found, inserting point" << endl;
#endif
		// insert must happen one-past first point (before next point)
		++segpos;
		srcCoords.insert(segpos, snapPt);
	}

#if GEOS_DEBUG
cerr << " After segment snapping, srcCoors are: " << srcCoords << endl;
#endif

}

/*private*/
/* NOTE: this is called findSegmentIndexToSnap in JTS */
CoordinateList::iterator
LineStringSnapper::findSegmentToSnap(
			const Coordinate& snapPt,
			CoordinateList::iterator from,
			CoordinateList::iterator too_far)
{
	LineSegment seg;
	double minDist = snapTolerance+1; // make sure the first closer then
	                                  // snapTolerance is accepted
	CoordinateList::iterator match=too_far;

	// TODO: use std::find_if
	for ( ; from != too_far; ++from)
	{
		seg.p0 = *from;
		CoordinateList::iterator to = from;
		++to;
		seg.p1 = *to;

#if GEOS_DEBUG
cerr << " Checking segment " << seg << endl;
#endif

		/**
		 * Check if the snap pt is equal to one of
		 * the segment endpoints.
		 *
		 * If the snap pt is already in the src list,
		 * don't snap at all (unless allowSnappingToSourceVertices
		 * is set to true)
		 */
		if ( seg.p0.equals2D(snapPt) || seg.p1.equals2D(snapPt) )
		{

			if (allowSnappingToSourceVertices) {
#if GEOS_DEBUG
cerr << "   snap point matches a segment endpoint, checking next segment"
     << endl;
#endif
				continue;
			} else {
#if GEOS_DEBUG
cerr << "   snap point matches a segment endpoint, giving up seek" << endl;
#endif
				return too_far;
			}
		}

		double dist = seg.distance(snapPt);
		if ( dist < snapTolerance ) {
      if ( dist < minDist ) {
#if GEOS_DEBUG
cerr << "   snap point distance " << dist << " within tolerance "
     << snapTolerance << " and closer than previous candidate " << minDist
     << endl;
#endif
        match = from;
        minDist = dist;
      }
#if GEOS_DEBUG
      else {
cerr << "   snap point distance " << dist << " within tolerance "
     << snapTolerance << " but not closer than previous candidate " << minDist
     << endl;
      }
#endif
    }
#if GEOS_DEBUG
    else {
cerr << "   snap point distance " << dist << " bigger than tolerance "
     << snapTolerance << endl;
    }
#endif
	}

	return match;
}

} // namespace geos.operation.snap
} // namespace geos.operation.overlay
} // namespace geos.operation
} // namespace geos

<|endoftext|>


<commit_before>// rm -rf %t
// RUN: %clang_cc1 -fmodules -fimplicit-module-maps -x c++ -Rmodule-build -I%S/Inputs/module-map-path-hash -fmodules-cache-path=%t -fsyntax-only %s
// xUN: %clang_cc1 -fmodules -fimplicit-module-maps -x c++ -Rmodule-build -I%S/Inputs//module-map-path-hash -fmodules-cache-path=%t -fsyntax-only %s 2>&1 | FileCheck -allow-empty %s
// xUN: %clang_cc1 -fmodules -fimplicit-module-maps -x c++ -Rmodule-build -I%S/Inputs/./module-map-path-hash -fmodules-cache-path=%t -fsyntax-only %s 2>&1 | FileCheck -allow-empty %s
// RUN: %clang_cc1 -fmodules -fimplicit-module-maps -x c++ -Rmodule-build -I%S/Inputs/../Inputs/module-map-path-hash -fmodules-cache-path=%t -fsyntax-only %s 2>&1 | FileCheck -allow-empty %s

#include "a.h"

// CHECK-NOT: remark: building module
<commit_msg>Enable accidentally-disabled tests.<commit_after>// rm -rf %t
// RUN: %clang_cc1 -fmodules -fimplicit-module-maps -x c++ -Rmodule-build -I%S/Inputs/module-map-path-hash -fmodules-cache-path=%t -fsyntax-only %s
// RUN: %clang_cc1 -fmodules -fimplicit-module-maps -x c++ -Rmodule-build -I%S/Inputs//module-map-path-hash -fmodules-cache-path=%t -fsyntax-only %s 2>&1 | FileCheck -allow-empty %s
// RUN: %clang_cc1 -fmodules -fimplicit-module-maps -x c++ -Rmodule-build -I%S/Inputs/./module-map-path-hash -fmodules-cache-path=%t -fsyntax-only %s 2>&1 | FileCheck -allow-empty %s
// RUN: %clang_cc1 -fmodules -fimplicit-module-maps -x c++ -Rmodule-build -I%S/Inputs/../Inputs/module-map-path-hash -fmodules-cache-path=%t -fsyntax-only %s 2>&1 | FileCheck -allow-empty %s

#include "a.h"

// CHECK-NOT: remark: building module
<|endoftext|>


<commit_before>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int high_right = 600, high_left = 600, period = 20000; //PWM high time in us
int top_edge = 50, servo_offset = 0, road_offset = 0;

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;
int settings_servo_offset = 500;
int settings_road_approx = 5;
int settings_middle_line = 240;

// Graphics
std::vector<std::vector<cv::Point>> contours;

//Functions declaration
void pwm_servo_right(int);
void pwm_servo_left(int);
int car_position(int);
int obstacle_position(int);

int main(int argc, char** argv)
{
    pthread_t processing_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Road Approx ", settings_window_name, &settings_road_approx, 30);
	cv::createTrackbar("Middle Line ", settings_window_name, &settings_middle_line, 320);
	cv::createTrackbar("Servo Offset", settings_window_name, &settings_servo_offset, 1000);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 1500);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 1500);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				led_front.low();
				led_R.low();
				running = false;
			} else if((gui_key == 48) || (gui_key == 32)) {
				start.high(); // 1 = start/stop
				usleep(50000);
				start.low();
			} else if((gui_key == 50) || (gui_key == 66)) {
				led_front.toggle(); // 2 = faruri
			} else if((gui_key == 51) || (gui_key == 67)) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(int a, int b){
	return cv::contourArea(contours[a]) > cv::contourArea(contours[b]);
}

bool get_obstacle(
	int parent,
	std::vector<std::vector<cv::Point>> contours,
	std::vector<cv::Vec4i> hierarchy,
	cv::Rect &obstacle){

	int start = hierarchy[parent][2];
	int closest = -1;
	bool found = false;

	while(start > 0){

		if(closest == -1){
			closest = start;
			found = true;
		} else if(cv::boundingRect(contours[start]).y > cv::boundingRect(contours[closest]).y){
			closest = start;
		}

		start = hierarchy[start][0];
	}

	if(found){
		obstacle = cv::boundingRect(contours[closest]);
	}

	return found;
}

void draw_obstacles(cv::Mat &threshold_frame, cv::Mat &cam_frame){
	std::vector<std::vector<cv::Point>> road(2);
	std::vector<cv::Vec4i> hierarchy;

	cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
	if(contours.size() > 1){

		// create index vector
		std::vector<int> contour_indexes(contours.size());

		for(unsigned int i = 0; i < contours.size(); i++){
			contour_indexes[i] = i;
		}

		// sorting index vector acording to controur area
		std::sort(contour_indexes.begin(), contour_indexes.end(), contour_area);

		cv::approxPolyDP(cv::Mat(contours[contour_indexes[0]]), road[0], settings_road_approx, true);
		cv::drawContours(cam_frame, road, 0, cv::Scalar(0, 255 ,0), 2);

		cv::approxPolyDP(cv::Mat(contours[contour_indexes[1]]), road[1], settings_road_approx, true);
		cv::drawContours(cam_frame, road, 1, cv::Scalar(255, 0, 0), 2);

		// validate road detection
		int road_edge = -1;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y > 235) && (road_edge == -1)){
				road_edge = road[0][i].x;

			} else if((road[0][i].y > 235) && (road[0][i].x < road_edge)){

				road_edge = road[0][i].x;
			}
		}

		if(road_edge < 0)
			return ;

		for(unsigned int i = 0; i < road[1].size(); ++i){
			if((road[1][i].y > 235) && (road[1][i].x > road_edge)){

				// wrong detection
				return;
			}
		}

		// get road offset
		// reference point is the left most point from the top edge of road contour
		int new_road_offset = 600;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y == top_edge) && (road[0][i].x < new_road_offset)){
				new_road_offset = road[0][i].x;
			}
		}

		if((new_road_offset > 50) && (new_road_offset < 250)){
			road_offset = new_road_offset;
		}

		std::cout << road_offset << std::endl;

		cv::Rect obstacle;
		if(get_obstacle(contour_indexes[0], contours, hierarchy, obstacle)){
			cv::rectangle(cam_frame, obstacle, cv::Scalar(0, 0, 255));
			std::cout << cv::Point(obstacle.x + (obstacle.width / 2), obstacle.y + (obstacle.height / 2)) << std::endl;
		}

		if(get_obstacle(contour_indexes[1], contours, hierarchy, obstacle)){
			cv::rectangle(cam_frame, obstacle, cv::Scalar(255, 0, 255));
		}

		cv::line(cam_frame, cv::Point(settings_middle_line, 320), cv::Point(settings_middle_line, top_edge), cv::Scalar(0, 255, 255));
	}
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;

    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1, h_r, h_l;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {
		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		// Disable image top from detection to remove false edges
		cv::rectangle(threshold_frame, cv::Rect(0, 0, 320, top_edge), cv::Scalar(0), CV_FILLED);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours
		draw_obstacles(threshold_frame, cam_frame);
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);

		h_r = high_right + servo_offset + road_offset + car_position(0);//!!!!
		h_l = high_left + servo_offset + road_offset + obstacle_position(0);//!!!!

   		pwm_servo_right(h_r);
   		pwm_servo_left(h_l);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

void pwm_servo_right(int h_r){
	int static local_hr = 0;
	if(abs(h_r - local_hr) > 5){
		local_hr = h_r;
		pwm_right.high();
		usleep(local_hr);
		pwm_right.low();
		usleep(period - local_hr);
	}
}

void pwm_servo_left(int h_l){
	int static local_hl = 0;
	if(abs(h_l - local_hl) > 5){
		local_hl = h_l;
		pwm_left.high();
		usleep(local_hl);
		pwm_left.low();
		usleep(period - local_hl);
	}
}

int obstacle_position(int y_position){
	if(y_position > top_edge){
		return int ( 0.9 * y_position + 15);
	} else {
		return 0;
	}
}

int car_position(int y_position){
	if(y_position > top_edge){
		return 1;
	} else {
		return 0;
	}
}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<commit_msg>Resolved bug, finnaly<commit_after>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int high_right = 600, high_left = 600, period = 20000; //PWM high time in us
int top_edge = 50, servo_offset = 0, road_offset = 0;

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;
int settings_servo_offset = 500;
int settings_road_approx = 5;
int settings_middle_line = 240;

// Graphics
std::vector<std::vector<cv::Point>> contours;

//Functions declaration
void pwm_servo_right(int);
void pwm_servo_left(int);
int car_position(int);
int obstacle_position(int);

int main(int argc, char** argv)
{
    pthread_t processing_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Road Approx ", settings_window_name, &settings_road_approx, 30);
	cv::createTrackbar("Middle Line ", settings_window_name, &settings_middle_line, 320);
	cv::createTrackbar("Servo Offset", settings_window_name, &settings_servo_offset, 1000);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 1500);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 1500);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				led_front.low();
				led_R.low();
				running = false;
			} else if((gui_key == 48) || (gui_key == 32)) {
				start.high(); // 1 = start/stop
				usleep(50000);
				start.low();
			} else if((gui_key == 50) || (gui_key == 66)) {
				led_front.toggle(); // 2 = faruri
			} else if((gui_key == 51) || (gui_key == 67)) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(int a, int b){
	return cv::contourArea(contours[a]) > cv::contourArea(contours[b]);
}

bool get_obstacle(
	int parent,
	std::vector<std::vector<cv::Point>> contours,
	std::vector<cv::Vec4i> hierarchy,
	cv::Rect &obstacle){

	int start = hierarchy[parent][2];
	int closest = -1;
	bool found = false;

	while(start > 0){

		if(closest == -1){
			closest = start;
			found = true;
		} else if(cv::boundingRect(contours[start]).y > cv::boundingRect(contours[closest]).y){
			closest = start;
		}

		start = hierarchy[start][0];
	}

	if(found){
		obstacle = cv::boundingRect(contours[closest]);
	}

	return found;
}

void draw_obstacles(cv::Mat &threshold_frame, cv::Mat &cam_frame){
	std::vector<std::vector<cv::Point>> road(2);
	std::vector<cv::Vec4i> hierarchy;

	cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
	if(contours.size() > 1){

		// create index vector
		std::vector<int> contour_indexes(contours.size());

		for(unsigned int i = 0; i < contours.size(); i++){
			contour_indexes[i] = i;
		}

		// sorting index vector acording to controur area
		std::sort(contour_indexes.begin(), contour_indexes.end(), contour_area);

		cv::approxPolyDP(cv::Mat(contours[contour_indexes[0]]), road[0], settings_road_approx, true);
		cv::approxPolyDP(cv::Mat(contours[contour_indexes[1]]), road[1], settings_road_approx, true);

		// validate road detection
		int road_edge = -1;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y > 235) && (road_edge == -1)){
				road_edge = road[0][i].x;

			} else if((road[0][i].y > 235) && (road[0][i].x < road_edge)){

				road_edge = road[0][i].x;
			}
		}

		if(road_edge < 0)
			return ;

		for(unsigned int i = 0; i < road[1].size(); ++i){
			if((road[1][i].y > 235) && (road[1][i].x > road_edge)){

				// wrong detection
				return;
			}
		}

		// paint road area
		cv::drawContours(cam_frame, road, 0, cv::Scalar(0, 255 ,0), 2);
		cv::drawContours(cam_frame, road, 1, cv::Scalar(255, 0, 0), 2);

		// get road offset
		// reference point is the left most point from the top edge of road contour
		int new_road_offset = 600;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y == top_edge) && (road[0][i].x < new_road_offset)){
				new_road_offset = road[0][i].x;
			}
		}

		if((new_road_offset > 50) && (new_road_offset < 250)){
			road_offset = new_road_offset;
		}

		std::cout << road_offset << std::endl;

		cv::Rect obstacle;
		if(get_obstacle(contour_indexes[0], contours, hierarchy, obstacle)){
			cv::rectangle(cam_frame, obstacle, cv::Scalar(0, 0, 255));
			std::cout << cv::Point(obstacle.x + (obstacle.width / 2), obstacle.y + (obstacle.height / 2)) << std::endl;
		}

		if(get_obstacle(contour_indexes[1], contours, hierarchy, obstacle)){
			cv::rectangle(cam_frame, obstacle, cv::Scalar(255, 0, 255));
		}

		cv::line(cam_frame, cv::Point(settings_middle_line, 320), cv::Point(settings_middle_line, top_edge), cv::Scalar(0, 255, 255));
	}
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;

    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1, h_r, h_l;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {
		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		// Disable image top from detection to remove false edges
		cv::rectangle(threshold_frame, cv::Rect(0, 0, 320, top_edge), cv::Scalar(0), CV_FILLED);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours
		draw_obstacles(threshold_frame, cam_frame);
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);

		h_r = high_right + servo_offset + road_offset + car_position(0);//!!!!
		h_l = high_left + servo_offset + road_offset + obstacle_position(0);//!!!!

   		pwm_servo_right(h_r);
   		pwm_servo_left(h_l);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

void pwm_servo_right(int h_r){
	int static local_hr = 0;
	if(abs(h_r - local_hr) > 5){
		local_hr = h_r;
		pwm_right.high();
		usleep(local_hr);
		pwm_right.low();
		usleep(period - local_hr);
	}
}

void pwm_servo_left(int h_l){
	int static local_hl = 0;
	if(abs(h_l - local_hl) > 5){
		local_hl = h_l;
		pwm_left.high();
		usleep(local_hl);
		pwm_left.low();
		usleep(period - local_hl);
	}
}

int obstacle_position(int y_position){
	if(y_position > top_edge){
		return int ( 0.9 * y_position + 15);
	} else {
		return 0;
	}
}

int car_position(int y_position){
	if(y_position > top_edge){
		return 1;
	} else {
		return 0;
	}
}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<|endoftext|>


<commit_before>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(4, "out"), pwm_left(7, "out"); // led_right(2 , "out");
GPIO led_front(1, "out"), led_R(5, "out"), start(3 ,"out"); // 0,2,6 bulit
int high_right = 600, high_left = 600, period = 20000; //PWM high time in us

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;

//Functions declaration
void pwm_control(int, int);

int main(int argc, char** argv)
{
    pthread_t processing_thread, pwm_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 3000);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 3000);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				running = false;
			} else if(gui_key == 49) {
				start.high(); // 1 = start/stop
				usleep(10000);
				start.low();
			} else if(gui_key == 50) {
				led_front.toggle(); // 2 = faruri
			} else if(gui_key == 51) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(std::vector<cv::Point> a, std::vector<cv::Point> b){
	return cv::contourArea(a) > cv::contourArea(b);
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;
    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1;
	int local_hr = 0, local_hl = 0; //local variables for pwm control

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {

   		if((high_right != local_hr)||(high_left != local_hl)){
   			local_hr = high_right;
   			local_hl = high_left;
   			pwm_control(local_hr, local_hl);
   		}

		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours

		std::vector<std::vector<cv::Point>> contours, road(1);
		std::vector<cv::Vec4i> hierarchy;
		contour_frame = blur_frame;

		cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
		if(contours.size() > 1){
			std::sort(contours.begin(), contours.end(), contour_area);
			cv::approxPolyDP(cv::Mat(contours[0]), road[0], 20, true);
			cv::drawContours(cam_frame, road, -1, cv::Scalar(0, 255 ,0), 2);

			cv::approxPolyDP(cv::Mat(contours[1]), road[0], 20, true);
			cv::drawContours(cam_frame, road, -1, cv::Scalar(255, 0, 0), 2);
		}
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

	void pwm_control(int h_r, int h_l){
		for(int i = 0; i < 2; i++){
			pwm_right.high();
			pwm_left.high();
			usleep(std::min(h_r, h_l));
			if(h_r > h_l)
				pwm_left.low();
			else
				pwm_right.low();
			usleep(abs(h_r - h_l));
			if(h_r > h_l)
				pwm_right.low();
			else
				pwm_left.low();
			usleep(period - std::max(h_r, h_l));
		}
	}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<commit_msg>modified gpio position<commit_after>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int high_right = 600, high_left = 600, period = 20000; //PWM high time in us

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;

//Functions declaration
void pwm_control(int, int);

int main(int argc, char** argv)
{
    pthread_t processing_thread, pwm_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 3000);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 3000);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				running = false;
			} else if(gui_key == 49) {
				start.high(); // 1 = start/stop
				usleep(10000);
				start.low();
			} else if(gui_key == 50) {
				led_front.toggle(); // 2 = faruri
			} else if(gui_key == 51) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(std::vector<cv::Point> a, std::vector<cv::Point> b){
	return cv::contourArea(a) > cv::contourArea(b);
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;
    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1;
	int local_hr = 0, local_hl = 0; //local variables for pwm control

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {

   		if((high_right != local_hr)||(high_left != local_hl)){
   			local_hr = high_right;
   			local_hl = high_left;
   			pwm_control(local_hr, local_hl);
   		}

		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours

		std::vector<std::vector<cv::Point>> contours, road(1);
		std::vector<cv::Vec4i> hierarchy;
		contour_frame = blur_frame;

		cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
		if(contours.size() > 1){
			std::sort(contours.begin(), contours.end(), contour_area);
			cv::approxPolyDP(cv::Mat(contours[0]), road[0], 20, true);
			cv::drawContours(cam_frame, road, -1, cv::Scalar(0, 255 ,0), 2);

			cv::approxPolyDP(cv::Mat(contours[1]), road[0], 20, true);
			cv::drawContours(cam_frame, road, -1, cv::Scalar(255, 0, 0), 2);
		}
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

	void pwm_control(int h_r, int h_l){
		for(int i = 0; i < 2; i++){
			pwm_right.high();
			pwm_left.high();
			usleep(std::min(h_r, h_l));
			if(h_r > h_l)
				pwm_left.low();
			else
				pwm_right.low();
			usleep(abs(h_r - h_l));
			if(h_r > h_l)
				pwm_right.low();
			else
				pwm_left.low();
			usleep(period - std::max(h_r, h_l));
		}
	}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<|endoftext|>


<commit_before>#include "aghInclude.h"
#include <iostream>

using namespace std;
// ---------------------------------------------------------

void showTestResult(int, bool);
// ---------------------------------------------------------

int main(void)
{
    cout << "main by kk. modified by viters. Last updated 21.05.2016\n";

    aghDlist<aghDlist<int> > a;
    aghContainer<int> *c1 = new aghDlist<int>;
    aghContainer<int> *c2;
    a << *((aghDlist<int> *)c1);

    // 1st test - dodawanie do pojemnika stalych, zmiennych, tymczasowych
    c1->append(3);
    c1->insert(0, 1 + 1);
    c1->insert(c1->size(), c1->size());

    bool t1 = c1->size() == 3;
    int ttab1[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t1 = t1 && (ttab1[i] == c1->at(i));
    }
    showTestResult(1, t1);

    // 2nd test - konstruktor
    c2 = new aghDlist<int>(*c1);
    bool t2 = c2->size() == 3;
    int ttab2[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t2 = t2 && (ttab2[i] == c2->at(i));
    }
    showTestResult(2, t2);

    // 3rd test - odwolania
    try
    {
        c2->at(-1);
        c2->at(100);
        (*c2)[-1];
        (*c2)[100];
        showTestResult(3, false);
    }
    catch (aghException &e)
    {
        showTestResult(3, true);
    }
    catch (...)
    {
        showTestResult(3, false);
    }

    // 4th test - usuwanie z pojemnika
    c1->clear();
    for (int i = 0; i < 5; i++) {
        *c1 += i;
    }
    *c1 << 4 << 2 + 3;
    c1->remove(2);

    int ttab4[] = {0, 1, 3, 4, 4, 5};
    bool t4 = c1->size() == 6;
    for (int i = 0; t4 && i < 6; i++) {
        t4 = t4 && (ttab4[i] == c1->at(i));
    }
    showTestResult(4, t4);

    // 5th test - sprawdzenie dzialanie konstruktora kopiujacego
    bool t5 = c2->size() == 3;
    int ttab5[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t5 = t5 && (ttab5[i] == c2->at(i));
    }
    showTestResult(5, t5);

    // 6th test - metoda indexOf
    showTestResult(6, c1->indexOf(3) == 2);

    // 7th test - metoda indexOf
    showTestResult(7, c1->indexOf(4, 3) == 3);

    // 8th test - metoda indexOf
    showTestResult(8, c1->indexOf(4, 4) == 4);

    // 9th test - metoda indexOf
    showTestResult(9, c1->indexOf(3, 3) == -1);

    // 10th test - metoda contains
    showTestResult(10, !c1->contains(-6));

    // 11th test - operacje na pojemniku w pojemniku
    for (int i = 3; i >= 0; i--) {
        a.at(0) += i + 1;
    }

    bool t11 = a.at(0).size() == 4;
    int ttab11[] = {4, 3, 2, 1};
    for (int i = 0; t11 && i < 4; i++) {
        t11 = t11 && (a.at(0).at(i) == ttab11[i]);
    }
    showTestResult(11, t11);

    // 12th test - usuwanie z pojemnika
    a.at(0).remove(2);   // 4,3,1
    a.at(0).remove(1);   // 4,1
    a.at(0).remove(1);   // 4
    a.at(0).remove(0);   // empty
    showTestResult(12, a.at(0).isEmpty());

    // 13th test - dzialanie operatora przypisania
    *c2 = a.at(0) = *c1;
    showTestResult(13, *c1 == a.at(0));

    // 14th test - operator przypisania
    try
    {
        *c2 = *c2;
        showTestResult(14, *c1 == *c2);
    }
    catch (...)
    {
        showTestResult(14, false);
    }

    // 15th test - zwalnianie paieci
    try
    {
        delete c1;
        delete c2;
        showTestResult(15, true);
    }
    catch (...)
    {
        showTestResult(15, false);
    }

    // ---- testy dla aghSlist ----

    // aghSlist<aghSlist<int> > slist1;
    aghContainer<int> *slistptr1 = new aghSlist<int>;
    aghContainer<int> *slistptr2;
    // slist1 << *((aghSlist<int> *)slistptr1);

    // 16th test - dodawanie do pojemnika stalych, zmiennych, tymczasowych
    slistptr1->append(3);
    slistptr1->insert(0, 1 + 1);
    slistptr1->insert(slistptr1->size(), slistptr1->size());

    bool t16 = slistptr1->size() == 3;
    int ttab16[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t16 = t16 && (ttab16[i] == slistptr1->at(i));
    }

    showTestResult(16, t16);

    cout << "Finally, this is the end...\n";

    return 0;
}
// ---------------------------------------------------------

void showTestResult(int _ti, bool _r)
{
    if (_r) {
        cout << "Test" << _ti << " PASSED\n";
    }
    else {
        cout << "Test" << _ti << " FAILED\n";
    }
}
// ---------------------------------------------------------
<commit_msg>Dodałem testy dla aghSlist.<commit_after>#include "aghInclude.h"
#include <iostream>

using namespace std;
// ---------------------------------------------------------

void showTestResult(int, bool);
// ---------------------------------------------------------

int main(void)
{
    cout << "main by kk. modified by viters. Last updated 21.05.2016\n";

    // ---- testy dla aghDlist - listy podwojnej ----

    cout << "Testy dla aghDlist:\n";

    aghDlist<aghDlist<int> > a;
    aghContainer<int> *c1 = new aghDlist<int>;
    aghContainer<int> *c2;
    a << *((aghDlist<int> *)c1);

    // 1st test - dodawanie do pojemnika stalych, zmiennych, tymczasowych
    c1->append(3);
    c1->insert(0, 1 + 1);
    c1->insert(c1->size(), c1->size());

    bool t1 = c1->size() == 3;
    int ttab1[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t1 = t1 && (ttab1[i] == c1->at(i));
    }
    showTestResult(1, t1);

    // 2nd test - konstruktor
    c2 = new aghDlist<int>(*c1);
    bool t2 = c2->size() == 3;
    int ttab2[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t2 = t2 && (ttab2[i] == c2->at(i));
    }
    showTestResult(2, t2);

    // 3rd test - odwolania
    try
    {
        c2->at(-1);
        c2->at(100);
        (*c2)[-1];
        (*c2)[100];
        showTestResult(3, false);
    }
    catch (aghException &e)
    {
        showTestResult(3, true);
    }
    catch (...)
    {
        showTestResult(3, false);
    }

    // 4th test - usuwanie z pojemnika
    c1->clear();
    for (int i = 0; i < 5; i++) {
        *c1 += i;
    }
    *c1 << 4 << 2 + 3;
    c1->remove(2);

    int ttab4[] = {0, 1, 3, 4, 4, 5};
    bool t4 = c1->size() == 6;
    for (int i = 0; t4 && i < 6; i++) {
        t4 = t4 && (ttab4[i] == c1->at(i));
    }
    showTestResult(4, t4);

    // 5th test - sprawdzenie dzialania konstruktora kopiujacego
    bool t5 = c2->size() == 3;
    int ttab5[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t5 = t5 && (ttab5[i] == c2->at(i));
    }
    showTestResult(5, t5);

    // 6th test - metoda indexOf
    showTestResult(6, c1->indexOf(3) == 2);

    // 7th test - metoda indexOf
    showTestResult(7, c1->indexOf(4, 3) == 3);

    // 8th test - metoda indexOf
    showTestResult(8, c1->indexOf(4, 4) == 4);

    // 9th test - metoda indexOf
    showTestResult(9, c1->indexOf(3, 3) == -1);

    // 10th test - metoda contains
    showTestResult(10, !c1->contains(-6));

    // 11th test - operacje na pojemniku w pojemniku
    for (int i = 3; i >= 0; i--) {
        a.at(0) += i + 1;
    }

    bool t11 = a.at(0).size() == 4;
    int ttab11[] = {4, 3, 2, 1};
    for (int i = 0; t11 && i < 4; i++) {
        t11 = t11 && (a.at(0).at(i) == ttab11[i]);
    }
    showTestResult(11, t11);

    // 12th test - usuwanie z pojemnika
    a.at(0).remove(2);   // 4,3,1
    a.at(0).remove(1);   // 4,1
    a.at(0).remove(1);   // 4
    a.at(0).remove(0);   // empty
    showTestResult(12, a.at(0).isEmpty());

    // 13th test - dzialanie operatora przypisania
    *c2 = a.at(0) = *c1;
    showTestResult(13, *c1 == a.at(0));

    // 14th test - operator przypisania
    try
    {
        *c2 = *c2;
        showTestResult(14, *c1 == *c2);
    }
    catch (...)
    {
        showTestResult(14, false);
    }

    // 15th test - zwalnianie pamieci
    try
    {
        delete c1;
        delete c2;
        showTestResult(15, true);
    }
    catch (...)
    {
        showTestResult(15, false);
    }

    cout << "Finally, this is the end... or isn't it?\n";

    // ---- testy dla aghSlist - lista pojedyncza ----

    cout << "\nTesty dla aghSlist:\n";

    aghSlist<aghSlist<int> > slist1;
    aghContainer<int> *slistptr1 = new aghSlist<int>;
    aghContainer<int> *slistptr2;
    slist1 << *((aghSlist<int> *)slistptr1);

    // 16th test - dodawanie do pojemnika stalych, zmiennych, tymczasowych
    slistptr1->append(3);
    slistptr1->insert(0, 1 + 1);
    slistptr1->insert(slistptr1->size(), slistptr1->size());

    bool t16 = slistptr1->size() == 3;
    int ttab16[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t16 = t16 && (ttab16[i] == slistptr1->at(i));
    }

    showTestResult(16, t16);

    // 17th test - konstruktor
    slistptr2 = new aghDlist<int>(*slistptr1);
    bool t17 = slistptr2->size() == 3;
    int ttab17[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t17 = t17 && (ttab17[i] == slistptr2->at(i));
    }
    showTestResult(17, t17);

    // 18th test - odwolania
    try
    {
        slistptr2->at(-1);
        slistptr2->at(100);
        (*slistptr2)[-1];
        (*slistptr2)[100];
        showTestResult(18, false);
    }
    catch (aghException &e)
    {
        showTestResult(18, true);
    }
    catch (...) {
        showTestResult(18, false);
    }

    // 19th test - usuwanie z pojemnika
    slistptr1->clear();
    for (int i = 0; i < 5; i++) {
        *slistptr1 += i;
    }
    *slistptr1 << 4 << 2 + 3;
    slistptr1->remove(2);

    int ttab19[] = {0, 1, 3, 4, 4, 5};
    bool t19 = slistptr1->size() == 6;
    for (int i = 0; t19 && i < 6; i++) {
        t19 = t19 && (ttab19[i] == slistptr1->at(i));
    }
    showTestResult(19, t19);

    // 20th test - sprawdzenie dzialanie konstruktora kopiujacego
    bool t20 = slistptr2->size() == 3;
    int ttab20[] = {2, 3, 2};
    for (int i = 0; i < 3; i++) {
        t20 = t20 && (ttab20[i] == slistptr2->at(i));
    }
    showTestResult(20, t20);

    // 21th test - metoda indexOf
    showTestResult(21, slistptr1->indexOf(3) == 2);

    // 22th test - metoda indexOf
    showTestResult(22, slistptr1->indexOf(4, 3) == 3);

    // 23th test - metoda indexOf
    showTestResult(23, slistptr1->indexOf(4, 4) == 4);

    // 24th test - metoda indexOf
    showTestResult(24, slistptr1->indexOf(3, 3) == -1);

    // 25th test - metoda contains
    showTestResult(25, !slistptr1->contains(-6));

    // 26th test - operacje na pojemniku w pojemniku
    for (int i = 3; i >= 0; i--) {
        slist1.at(0) += i + 1;
    }

    bool t26 = slist1.at(0).size() == 4;
    int ttab26[] = {4, 3, 2, 1};
    for (int i = 0; t26 && i < 4; i++) {
        t26 = t26 && (slist1.at(0).at(i) == ttab26[i]);
    }
    showTestResult(26, t26);

    // 27th test - usuwanie z pojemnika
    slist1.at(0).remove(2);   // 4,3,1
    slist1.at(0).remove(1);   // 4,1
    slist1.at(0).remove(1);   // 4
    slist1.at(0).remove(0);   // empty
    showTestResult(27, slist1.at(0).isEmpty());

    // 28th test - dzialanie operatora przypisania
    *slistptr2 = slist1.at(0) = *slistptr1;
    showTestResult(28, *slistptr1 == slist1.at(0));

    // 29th test - operator przypisania
    try
    {
        *slistptr2 = *slistptr2;
        showTestResult(29, *slistptr1 == *slistptr2);
    }
    catch (...)
    {
        showTestResult(29, false);
    }

    // 30th test - zwalnianie pamieci
    try
    {
        delete slistptr1;
        delete slistptr2;
        showTestResult(30, true);
    }
    catch (...)
    {
        showTestResult(30, false);
    }

    cout << "Now it definitely is the end...\n";

    return 0;
}
// ---------------------------------------------------------

void showTestResult(int _ti, bool _r)
{
    if (_r) {
        cout << "Test" << _ti << " PASSED\n";
    }
    else {
        cout << "Test" << _ti << " FAILED\n";
    }
}
// ---------------------------------------------------------
<|endoftext|>


<commit_before>#include <iostream>
#include <stdlib.h>
#include <time.h>
#include <thread>
#include <unistd.h>
#include "DoryProcessSpawn.hpp"

using namespace std;
using namespace spawn;

std::thread *n;
uv_loop_t* uv_loop = uv_default_loop();
map<int, shared_ptr<DoryProcessSpawn>> processList;

void loop(uv_loop_t* uv_loop) {
  uv_timer_t timer;
  int r;

  r = uv_timer_init(uv_loop, &timer);
  ASSERT(r==0);

  // repeat every 1000 millisec to make the loop lives forever
  // later, it will be used as whatchdog.
  r = uv_timer_start(&timer, [](uv_timer_t* timer){
    cout << "in timer. process cout : " << processList.size() << endl;
  }, 0, 1000);
  ASSERT(r == 0);
  uv_run(uv_loop, UV_RUN_DEFAULT);
}

int main() {
  //init
  n = new std::thread(loop, uv_loop);
  n->detach();
  srand(time(NULL));

  int i=0;
  while(1) {
    i++;

    if (i > 10) {
      usleep(10*1000);
      continue;
    }
    char buf[10];
    sprintf(buf,"%i",rand()%10000);

    char* args[3];
    args[0] = (char*)"./child";
    args[1] = buf;  // (char*)"2000";
    args[2] = NULL;

    // FIXME : leak
    DoryProcessSpawn *process = new DoryProcessSpawn(uv_loop, args);
    //process.timeout = 1000;
    int r = process->on("timeout", []() {
      cout << "timeout fired" << endl;
    })
    .on("stdout", [](char* buf, ssize_t nread) {
      for(int i=0;i<nread;i++)
        printf("%c", buf[i]);
    })
    .on("stderr", [](char* buf, ssize_t nread) {
      for(int i=0;i<nread;i++)
        printf("%c", buf[i]);
    })
    .on("exit", [process](int64_t exitStatus, int termSignal) {
      cout << "exit code : " << exitStatus << endl;
      cout << "signal : " << termSignal << endl;

      processList.erase(process->getPid());
    })
    .spawn();

    // check the result of spawn()
    if (r != 0)
      cout << uv_err_name(r) << " " << uv_strerror(r) << endl;
    else {
      processList[process->getPid()] = std::make_shared<DoryProcessSpawn>(*process);
      cout << "child pid : " << process->getPid() << endl;
    }
  }

  return 0;
}<commit_msg>add mutex lock to example.cpp<commit_after>#include <iostream>
#include <stdlib.h>
#include <time.h>
#include <thread>
#include <unistd.h>
#include "DoryProcessSpawn.hpp"

using namespace std;
using namespace spawn;

std::thread *n;
uv_loop_t* uv_loop = uv_default_loop();
map<int, shared_ptr<DoryProcessSpawn>> processList;
static uv_mutex_t mutex;

void loop(uv_loop_t* uv_loop) {
  uv_timer_t timer;
  int r;

  r = uv_timer_init(uv_loop, &timer);
  ASSERT(r==0);

  // repeat every 1000 millisec to make the loop lives forever
  // later, it will be used as whatchdog.
  r = uv_timer_start(&timer, [](uv_timer_t* timer){
    cout << "in timer. process cout : " << processList.size() << endl;
  }, 0, 1000);
  ASSERT(r == 0);
  uv_run(uv_loop, UV_RUN_DEFAULT);
}

int main() {
  //init
  int r = uv_mutex_init(&mutex);
  ASSERT(r == 0);

  n = new std::thread(loop, uv_loop);
  n->detach();
  srand(time(NULL));

  int i=0;
  while(1) {
    i++;

    if (i > 10) {
      usleep(10*1000);
      continue;
    }
    char buf[10];
    sprintf(buf,"%i",rand()%10000);

    char* args[3];
    args[0] = (char*)"./child";
    args[1] = buf;  // (char*)"2000";
    args[2] = NULL;

    // FIXME : leak
    DoryProcessSpawn *process = new DoryProcessSpawn(uv_loop, args);
    //process.timeout = 1000;
    int r = process->on("timeout", []() {
      cout << "timeout fired" << endl;
    })
    .on("stdout", [](char* buf, ssize_t nread) {
      for(int i=0;i<nread;i++)
        printf("%c", buf[i]);
    })
    .on("stderr", [](char* buf, ssize_t nread) {
      for(int i=0;i<nread;i++)
        printf("%c", buf[i]);
    })
    .on("exit", [process](int64_t exitStatus, int termSignal) {
      cout << "exit code : " << exitStatus << endl;
      cout << "signal : " << termSignal << endl;

      uv_mutex_lock(&mutex);
      processList.erase(process->getPid());
      uv_mutex_unlock(&mutex);
    })
    .spawn();

    // check the result of spawn()
    if (r != 0)
      cout << uv_err_name(r) << " " << uv_strerror(r) << endl;
    else {
      uv_mutex_lock(&mutex);
      processList[process->getPid()] = std::make_shared<DoryProcessSpawn>(*process);
      cout << "child pid : " << process->getPid() << endl;
      uv_mutex_unlock(&mutex);
    }
  }

  return 0;
}<|endoftext|>


<commit_before>// To compile, use: g++ -O3 -std=c++11 -march=native -g -o example fastavoidance.cpp hashdb.cpp example.cpp
// To run, use: ./example

#include <assert.h>
#include <string.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <time.h>
#include <stdlib.h>
#include <bitset>
#include <vector>
#include <stdint.h>
// #include <unordered_set>
#include <queue>
#include "hashdb.h"
#include <sys/time.h>
#include "fastavoidance.h"
#include "countpatterns.h"
#include "perm.h"
using namespace std;

int main() {
  string patternlist = "123 213"; // write pattern set as list of strings

  // example building S_n(Pi)
  vector < vector < uint64_t > > avoiders; // avoiders[i] will store the avoiders appearing in S_i
  buildavoidersfrompatternlist(patternlist, 8, avoiders);
  for (int i = 1; i <= 8; i++) {
    cout<<"Number avoiders in S_"<<i<<" of "<<patternlist<<" is "<<avoiders[i].size()<<endl; // print number of avoiders in S_n
    cout<<"Example avoider: "<<endl;
    uint64_t perm = avoiders[i][0]; // Permutations are represented in 64-bit integers
    displayperm(perm, i); // Permutations have smallest digit 0 instead of 1 -- sorry for this inconsistancy with the patternset format
    cout<<"In particular, the third digit is "<<getdigit(perm, 2)<<endl; // the function getdigit give you the (i-1)-th digit (i.e., indexing starts at zero for digit positions)
  }

  cout<<"-------------------"<<endl;
  // example counting |S_n(Pi)| (slightly faster than building)
  patternlist = "123 213"; // write pattern set as list of strings
  vector < uint64_t > numavoiders; // numavoiders[i] will store |S_i(Pi)|
  countavoidersfrompatternlist(patternlist, 10, numavoiders);
  for (int i = 1; i <= 10; i++) {
    cout<<"Number of permutations in S_"<<i<<" avoiding "<<patternlist<<" is "<<numavoiders[i]<<endl; // print number of avoiders in S_n
  }

  // example using countpatterns
  cout<<"-------------------"<<endl;
  patternlist = "123 213";
  vector < vector <int> > tally;
  vector < vector <int> > completelist;
  countpatterns(patternlist, 10, tally, completelist, false, falsex);
  string permtocheck = "21345";
  cout<<"Using pattern set "<<patternlist<<endl;
  cout<<"Number of pattern-occurrances in "<<permtocheck<<" is "<<completelist[permtocheck.size()][permtonum(stringtoperm(permtocheck), permtocheck.size())]<<endl;
  cout<<"Number of permutations in S_10 with no patterns appearing is "<<tally[10][0]<<endl;
  cout<<"Number of permutations in S_10 with 1 pattern appearing is "<<tally[10][1]<<endl;
  return 0;
}
<commit_msg>updated example.cpp<commit_after>// To compile, use: g++ -O3 -std=c++11 -march=native -g -o example fastavoidance.cpp hashdb.cpp example.cpp
// To run, use: ./example

#include <assert.h>
#include <string.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <time.h>
#include <stdlib.h>
#include <bitset>
#include <vector>
#include <stdint.h>
// #include <unordered_set>
#include <queue>
#include "hashdb.h"
#include <sys/time.h>
#include "fastavoidance.h"
#include "countpatterns.h"
#include "perm.h"
using namespace std;

int main() {
  string patternlist = "123 213"; // write pattern set as list of strings

  // example building S_n(Pi)
  vector < vector < uint64_t > > avoiders; // avoiders[i] will store the avoiders appearing in S_i
  buildavoidersfrompatternlist(patternlist, 8, avoiders);
  for (int i = 1; i <= 8; i++) {
    cout<<"Number avoiders in S_"<<i<<" of "<<patternlist<<" is "<<avoiders[i].size()<<endl; // print number of avoiders in S_n
    cout<<"Example avoider: "<<endl;
    uint64_t perm = avoiders[i][0]; // Permutations are represented in 64-bit integers
    displayperm(perm, i); // Permutations have smallest digit 0 instead of 1 -- sorry for this inconsistancy with the patternset format
    cout<<"In particular, the third digit is "<<getdigit(perm, 2)<<endl; // the function getdigit give you the (i-1)-th digit (i.e., indexing starts at zero for digit positions)
  }

  cout<<"-------------------"<<endl;
  // example counting |S_n(Pi)| (slightly faster than building)
  patternlist = "123 213"; // write pattern set as list of strings
  vector < uint64_t > numavoiders; // numavoiders[i] will store |S_i(Pi)|
  countavoidersfrompatternlist(patternlist, 10, numavoiders);
  for (int i = 1; i <= 10; i++) {
    cout<<"Number of permutations in S_"<<i<<" avoiding "<<patternlist<<" is "<<numavoiders[i]<<endl; // print number of avoiders in S_n
  }

  // example using countpatterns
  cout<<"-------------------"<<endl;
  patternlist = "123 213";
  vector < vector <int> > tally;
  vector < vector <int> > completelist;
  countpatterns(patternlist, 10, tally, completelist, false, false);
  string permtocheck = "21345";
  cout<<"Using pattern set "<<patternlist<<endl;
  cout<<"Number of pattern-occurrances in "<<permtocheck<<" is "<<completelist[permtocheck.size()][permtonum(stringtoperm(permtocheck), permtocheck.size())]<<endl;
  cout<<"Number of permutations in S_10 with no patterns appearing is "<<tally[10][0]<<endl;
  cout<<"Number of permutations in S_10 with 1 pattern appearing is "<<tally[10][1]<<endl;
  return 0;
}
<|endoftext|>


<commit_before>// C++ Standard Library
#include <cstdlib>
#include <array>
#include <stdexcept>

// Armadillo
#include <armadillo>

// Cereal
#include <cereal/archives/json.hpp>
#include <cereal/types/polymorphic.hpp>

// Mantella
#include <mantella>

int main (const int argc, const char* argv[]) {
  try {
    arma::arma_rng::set_seed_random();

    unsigned int optimisationProblemIndex = std::stoi(argv[1]);
    unsigned int numberOfDimensions = std::stoi(argv[2]);
    unsigned int populationSize = std::stoi(argv[3]);

    std::array<std::shared_ptr<mant::OptimisationProblem<double>>, 24> optimisationProblems({
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SphereFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::EllipsoidalFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::RastriginFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::BuecheRastriginFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::LinearSlope(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::AttractiveSectorFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::StepEllipsoidalFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::RosenbrockFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::RosenbrockFunctionRotated(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::EllipsoidalFunctionRotated(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::DiscusFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::BentCigarFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SharpRidgeFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::DifferentPowersFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::RastriginFunctionRotated(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::WeierstrassFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SchaffersF7Function(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SchaffersF7FunctionIllConditioned(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::CompositeGriewankRosenbrockFunctionF8F2(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SchwefelFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::GallaghersGaussian101mePeaksFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::GallaghersGaussian21hiPeaksFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::KatsuuraFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::LunacekBiRastriginFunction(numberOfDimensions)),
    });

    std::shared_ptr<mant::OptimisationProblem<double>> optimisationProblem = optimisationProblems.at(optimisationProblemIndex);

    arma::Col<double> maximalLocalAttractionParameters = arma::linspace(0, 4, 10);
    arma::Col<double> maximalGlobalAttractionParameters = arma::linspace(0, 4, 10);
    arma::Col<double> maximalAccelerationParameters = arma::linspace(0, 4, 10);
    arma::Col<double> neighbourhoodParameters = arma::linspace(0.1, 1, 5);

    unsigned int iterations = 30;

    mant::StandardParticleSwarmOptimisation2011<mant::EuclideanDistance> optimisationAlgorithm(optimisationProblem, populationSize);
    optimisationAlgorithm.setMaximalNumberOfIterations(10000);

    for(auto maximalLocalAttractionParameter : maximalLocalAttractionParameters) {
      optimisationAlgorithm.setMaximalLocalAttraction(maximalLocalAttractionParameter);

      for(auto maximalGlobalAttractionParameter : maximalGlobalAttractionParameters) {
        optimisationAlgorithm.setMaximalGlobalAttraction(maximalGlobalAttractionParameter);

        for(auto maximalAccelerationParameter: maximalAccelerationParameters) {
          optimisationAlgorithm.setMaximalAcceleration(maximalAccelerationParameter);

          for(auto neighbourhoodParameter : neighbourhoodParameters) {
            optimisationAlgorithm.setNeighbourhoodProbability(neighbourhoodParameter);

             std::string filepath = "./parameterisation_prob" + mant::to_string(optimisationProblem) + "_dim" + std::to_string(numberOfDimensions)+ "_pop" + std::to_string(populationSize) + "_local" + std::to_string(maximalLocalAttractionParameter) + "_global" + std::to_string(maximalGlobalAttractionParameter) + "_acc" + std::to_string(maximalAccelerationParameter) + "_neigh" + std::to_string(neighbourhoodParameter) + ".mat";

             if(!mant::testDirectory(filepath)) {
              arma::Mat<double> results(iterations, 6 + numberOfDimensions);
              for(std::size_t n = 0; n < iterations; ++n) {
                optimisationAlgorithm.optimise();

                results.at(n, 0) = optimisationProblem->getNumberOfEvaluations();
                results.at(n, 1) = optimisationProblem->getNumberOfDistinctEvaluations();
                results.at(n, 2) = optimisationAlgorithm.getNumberOfIterations();
                results.at(n, 3) = optimisationAlgorithm.getBestObjectiveValue() - optimisationProblem->getAcceptableObjectiveValue();
                results.at(n, 4) = optimisationAlgorithm.isFinished();
                results.at(n, 5) = optimisationAlgorithm.isTerminated();
                results(n, arma::span(6, 6 + numberOfDimensions - 1)) = optimisationAlgorithm.getBestParameter().t();
              }

              results.save(filepath.string(), arma::raw_ascii);
            }
          }
        }
      }
    }
    return EXIT_SUCCESS;
  } catch(const std::exception& exception) {
    std::cout << exception.what();
  }
}
<commit_msg>devel: Corrected minor typo<commit_after>// C++ Standard Library
#include <cstdlib>
#include <array>
#include <stdexcept>

// Armadillo
#include <armadillo>

// Cereal
#include <cereal/archives/json.hpp>
#include <cereal/types/polymorphic.hpp>

// Mantella
#include <mantella>

int main (const int argc, const char* argv[]) {
  try {
    arma::arma_rng::set_seed_random();

    unsigned int optimisationProblemIndex = std::stoi(argv[1]);
    unsigned int numberOfDimensions = std::stoi(argv[2]);
    unsigned int populationSize = std::stoi(argv[3]);

    std::array<std::shared_ptr<mant::OptimisationProblem<double>>, 24> optimisationProblems({
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SphereFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::EllipsoidalFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::RastriginFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::BuecheRastriginFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::LinearSlope(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::AttractiveSectorFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::StepEllipsoidalFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::RosenbrockFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::RosenbrockFunctionRotated(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::EllipsoidalFunctionRotated(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::DiscusFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::BentCigarFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SharpRidgeFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::DifferentPowersFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::RastriginFunctionRotated(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::WeierstrassFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SchaffersF7Function(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SchaffersF7FunctionIllConditioned(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::CompositeGriewankRosenbrockFunctionF8F2(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::SchwefelFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::GallaghersGaussian101mePeaksFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::GallaghersGaussian21hiPeaksFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::KatsuuraFunction(numberOfDimensions)),
      std::shared_ptr<mant::OptimisationProblem<double>>(new mant::bbob2013::LunacekBiRastriginFunction(numberOfDimensions)),
    });

    std::shared_ptr<mant::OptimisationProblem<double>> optimisationProblem = optimisationProblems.at(optimisationProblemIndex);

    arma::Col<double> maximalLocalAttractionParameters = arma::linspace(0, 4, 10);
    arma::Col<double> maximalGlobalAttractionParameters = arma::linspace(0, 4, 10);
    arma::Col<double> maximalAccelerationParameters = arma::linspace(0, 4, 10);
    arma::Col<double> neighbourhoodParameters = arma::linspace(0.1, 1, 5);

    unsigned int iterations = 30;

    mant::StandardParticleSwarmOptimisation2011<mant::EuclideanDistance> optimisationAlgorithm(optimisationProblem, populationSize);
    optimisationAlgorithm.setMaximalNumberOfIterations(10000);

    for(auto maximalLocalAttractionParameter : maximalLocalAttractionParameters) {
      optimisationAlgorithm.setMaximalLocalAttraction(maximalLocalAttractionParameter);

      for(auto maximalGlobalAttractionParameter : maximalGlobalAttractionParameters) {
        optimisationAlgorithm.setMaximalGlobalAttraction(maximalGlobalAttractionParameter);

        for(auto maximalAccelerationParameter: maximalAccelerationParameters) {
          optimisationAlgorithm.setMaximalAcceleration(maximalAccelerationParameter);

          for(auto neighbourhoodParameter : neighbourhoodParameters) {
            optimisationAlgorithm.setNeighbourhoodProbability(neighbourhoodParameter);

             std::string filepath = "./parameterisation_prob" + mant::to_string(optimisationProblem) + "_dim" + std::to_string(numberOfDimensions)+ "_pop" + std::to_string(populationSize) + "_local" + std::to_string(maximalLocalAttractionParameter) + "_global" + std::to_string(maximalGlobalAttractionParameter) + "_acc" + std::to_string(maximalAccelerationParameter) + "_neigh" + std::to_string(neighbourhoodParameter) + ".mat";

             if(!mant::file_exists(filepath)) {
              arma::Mat<double> results(iterations, 6 + numberOfDimensions);
              for(std::size_t n = 0; n < iterations; ++n) {
                optimisationAlgorithm.optimise();

                results.at(n, 0) = optimisationProblem->getNumberOfEvaluations();
                results.at(n, 1) = optimisationProblem->getNumberOfDistinctEvaluations();
                results.at(n, 2) = optimisationAlgorithm.getNumberOfIterations();
                results.at(n, 3) = optimisationAlgorithm.getBestObjectiveValue() - optimisationProblem->getAcceptableObjectiveValue();
                results.at(n, 4) = optimisationAlgorithm.isFinished();
                results.at(n, 5) = optimisationAlgorithm.isTerminated();
                results(n, arma::span(6, 6 + numberOfDimensions - 1)) = optimisationAlgorithm.getBestParameter().t();
              }

              results.save(filepath, arma::raw_ascii);
            }
          }
        }
      }
    }
    return EXIT_SUCCESS;
  } catch(const std::exception& exception) {
    std::cout << exception.what();
  }
}
<|endoftext|>


<commit_before>///
/// @file   SieveOfEratosthenes.cpp
/// @brief  Implementation of the segmented sieve of Eratosthenes.
///
/// 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 <primesieve/config.hpp>
#include <primesieve/SieveOfEratosthenes.hpp>
#include <primesieve/PreSieve.hpp>
#include <primesieve/EratSmall.hpp>
#include <primesieve/EratMedium.hpp>
#include <primesieve/EratBig.hpp>
#include <primesieve/pmath.hpp>
#include <primesieve/primesieve_error.hpp>

#include <stdint.h>
#include <exception>
#include <cstddef>

namespace primesieve {

const uint_t SieveOfEratosthenes::bitValues_[8] = { 7, 11, 13, 17, 19, 23, 29, 31 };

/// De Bruijn bitscan table
const uint_t SieveOfEratosthenes::bruijnBitValues_[64] =
{
    7,  47,  11,  49,  67, 113,  13,  53,
   89,  71, 161, 101, 119, 187,  17, 233,
   59,  79,  91,  73, 133, 139, 163, 103,
  149, 121, 203, 169, 191, 217,  19, 239,
   43,  61, 109,  83, 157,  97, 181, 229,
   77, 131, 137, 143, 199, 167, 211,  41,
  107, 151, 179, 227, 127, 197, 209,  37,
  173, 223, 193,  31, 221,  29,  23, 241
};

/// @param start      Sieve primes >= start.
/// @param stop       Sieve primes <= stop.
/// @param sieveSize  A sieve size in kilobytes.
/// @pre   start      >= 7
/// @pre   sieveSize  >= 1 && <= 2048
///
SieveOfEratosthenes::SieveOfEratosthenes(uint64_t start,
                                         uint64_t stop,
                                         uint_t sieveSize) :
  start_(start),
  stop_(stop),
  sieve_(NULL),
  preSieve_(NULL),
  eratSmall_(NULL),
  eratMedium_(NULL),
  eratBig_(NULL)
{
  if (start_ < 7)
    throw primesieve_error("SieveOfEratosthenes: start must be >= 7");
  if (start_ > stop_)
    throw primesieve_error("SieveOfEratosthenes: start must be <= stop");

  // choose fastest pre-sieve setting
  limitPreSieve_ = 11;
  if ((stop_ - start_) >= (7 * 11 * 13) * NUMBERS_PER_BYTE)
    limitPreSieve_ = 13;
  if ((stop_ - start_) >= config::PRESIEVE_THRESHOLD)
    limitPreSieve_ = config::PRESIEVE;

  // sieveSize_ must be a power of 2
  sieveSize_ = getInBetween(1u, floorPowerOf2(sieveSize), 2048u);
  sieveSize_ *= 1024; // convert to bytes
  uint64_t dist = sieveSize_ * NUMBERS_PER_BYTE + 1;
  segmentLow_ = start_ - getByteRemainder(start_);
  segmentHigh_ = add_overflow_safe(segmentLow_, dist);
  sqrtStop_ = static_cast<uint_t>(isqrt(stop_));

  // allocate sieve of Eratosthenes array
  sieve_ = new byte_t[sieveSize_];
  init();
}

SieveOfEratosthenes::~SieveOfEratosthenes()
{
  cleanUp();
}

void SieveOfEratosthenes::cleanUp()
{
  delete[] sieve_;
  delete preSieve_;
  delete eratSmall_;
  delete eratMedium_;
  delete eratBig_;
}

void SieveOfEratosthenes::init()
{
  limitEratSmall_  = static_cast<uint_t>(sieveSize_ * config::FACTOR_ERATSMALL);
  limitEratMedium_ = static_cast<uint_t>(sieveSize_ * config::FACTOR_ERATMEDIUM);
  try
  {
    preSieve_ = new PreSieve(limitPreSieve_);

    if (sqrtStop_ > limitPreSieve_)   eratSmall_  = new EratSmall (stop_, sieveSize_, limitEratSmall_);
    if (sqrtStop_ > limitEratSmall_)  eratMedium_ = new EratMedium(stop_, sieveSize_, limitEratMedium_);
    if (sqrtStop_ > limitEratMedium_) eratBig_    = new EratBig   (stop_, sieveSize_, sqrtStop_);
  }
  catch (std::exception&)
  {
    cleanUp();
    throw;
  }
}

uint_t SieveOfEratosthenes::getSqrtStop() const
{
  return sqrtStop_;
}

uint_t SieveOfEratosthenes::getPreSieve() const
{
  return limitPreSieve_;
}

uint64_t SieveOfEratosthenes::getByteRemainder(uint64_t n)
{
  uint64_t r = n % NUMBERS_PER_BYTE;
  if (r <= 1)
    r += NUMBERS_PER_BYTE;
  return r;
}

/// Pre-sieve multiples of small primes e.g. <= 19
/// to speed up the sieve of Eratosthenes.
///
void SieveOfEratosthenes::preSieve()
{
  preSieve_->doIt(sieve_, sieveSize_, segmentLow_);

  // unset bits (numbers) < start_
  if (segmentLow_ <= start_)
  {
    if (start_ <= limitPreSieve_)
      sieve_[0] = 0xff;
    for (int i = 0; bitValues_[i] < getByteRemainder(start_); i++)
      sieve_[0] &= 0xfe << i;
  }
}

void SieveOfEratosthenes::crossOffMultiples()
{
  if (eratSmall_)   eratSmall_->crossOff(sieve_, &sieve_[sieveSize_]);
  if (eratMedium_) eratMedium_->crossOff(sieve_, sieveSize_);
  if (eratBig_)       eratBig_->crossOff(sieve_);
}

void SieveOfEratosthenes::sieveSegment()
{
  preSieve();
  crossOffMultiples();
  segmentFinished(sieve_, sieveSize_);

  // update for next segment
  uint64_t dist = sieveSize_ * NUMBERS_PER_BYTE;
  segmentLow_ = add_overflow_safe(segmentLow_, dist);
  segmentHigh_ = add_overflow_safe(segmentHigh_, dist);
}

/// Sieve the remaining segments after that addSievingPrime(uint_t)
/// has been called for all primes up to sqrt(stop).
///
void SieveOfEratosthenes::sieve()
{
  while (segmentHigh_ < stop_)
    sieveSegment();

  uint64_t remainder = getByteRemainder(stop_);
  uint64_t dist = stop_ - segmentLow_ - remainder;
  sieveSize_ = static_cast<uint_t>(dist) / NUMBERS_PER_BYTE + 1;
  dist = sieveSize_ * NUMBERS_PER_BYTE + 1;
  segmentHigh_ = add_overflow_safe(segmentLow_, dist);

  // sieve the last segment
  preSieve();
  crossOffMultiples();

  int i;
  // unset bits (numbers) > stop_
  for (i = 0; i < 8; i++)
    if (bitValues_[i] > remainder)
      break;
  int unsetBits = ~(0xff << i);
  sieve_[sieveSize_ - 1] &= unsetBits;

  // unset bytes (numbers) > stop_
  for (uint_t j = sieveSize_; j % 8 != 0; j++)
    sieve_[j] = 0;

  segmentFinished(sieve_, sieveSize_);
}

} // namespace
<commit_msg>Refactor<commit_after>///
/// @file   SieveOfEratosthenes.cpp
/// @brief  Implementation of the segmented sieve of Eratosthenes.
///
/// 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 <primesieve/config.hpp>
#include <primesieve/SieveOfEratosthenes.hpp>
#include <primesieve/PreSieve.hpp>
#include <primesieve/EratSmall.hpp>
#include <primesieve/EratMedium.hpp>
#include <primesieve/EratBig.hpp>
#include <primesieve/pmath.hpp>
#include <primesieve/primesieve_error.hpp>

#include <stdint.h>
#include <exception>
#include <cstddef>

namespace primesieve {

const uint_t SieveOfEratosthenes::bitValues_[8] = { 7, 11, 13, 17, 19, 23, 29, 31 };

/// De Bruijn bitscan table
const uint_t SieveOfEratosthenes::bruijnBitValues_[64] =
{
    7,  47,  11,  49,  67, 113,  13,  53,
   89,  71, 161, 101, 119, 187,  17, 233,
   59,  79,  91,  73, 133, 139, 163, 103,
  149, 121, 203, 169, 191, 217,  19, 239,
   43,  61, 109,  83, 157,  97, 181, 229,
   77, 131, 137, 143, 199, 167, 211,  41,
  107, 151, 179, 227, 127, 197, 209,  37,
  173, 223, 193,  31, 221,  29,  23, 241
};

/// @param start      Sieve primes >= start.
/// @param stop       Sieve primes <= stop.
/// @param sieveSize  A sieve size in kilobytes.
/// @pre   start      >= 7
/// @pre   sieveSize  >= 1 && <= 2048
///
SieveOfEratosthenes::SieveOfEratosthenes(uint64_t start,
                                         uint64_t stop,
                                         uint_t sieveSize) :
  start_(start),
  stop_(stop),
  sieve_(NULL),
  preSieve_(NULL),
  eratSmall_(NULL),
  eratMedium_(NULL),
  eratBig_(NULL)
{
  if (start_ < 7)
    throw primesieve_error("SieveOfEratosthenes: start must be >= 7");
  if (start_ > stop_)
    throw primesieve_error("SieveOfEratosthenes: start must be <= stop");

  // choose fastest pre-sieve setting
  limitPreSieve_ = 11;
  if ((stop_ - start_) >= (7 * 11 * 13) * NUMBERS_PER_BYTE)
    limitPreSieve_ = 13;
  if ((stop_ - start_) >= config::PRESIEVE_THRESHOLD)
    limitPreSieve_ = config::PRESIEVE;

  // sieveSize_ must be a power of 2
  sieveSize_ = getInBetween(1u, floorPowerOf2(sieveSize), 2048u);
  sieveSize_ *= 1024; // convert to bytes

  uint64_t dist = sieveSize_ * NUMBERS_PER_BYTE + 1;
  segmentLow_ = start_ - getByteRemainder(start_);
  segmentHigh_ = add_overflow_safe(segmentLow_, dist);
  sqrtStop_ = static_cast<uint_t>(isqrt(stop_));

  // allocate sieve of Eratosthenes array
  sieve_ = new byte_t[sieveSize_];
  init();
}

SieveOfEratosthenes::~SieveOfEratosthenes()
{
  cleanUp();
}

void SieveOfEratosthenes::cleanUp()
{
  delete[] sieve_;
  delete preSieve_;
  delete eratSmall_;
  delete eratMedium_;
  delete eratBig_;
}

void SieveOfEratosthenes::init()
{
  limitEratSmall_  = static_cast<uint_t>(sieveSize_ * config::FACTOR_ERATSMALL);
  limitEratMedium_ = static_cast<uint_t>(sieveSize_ * config::FACTOR_ERATMEDIUM);
  try
  {
    preSieve_ = new PreSieve(limitPreSieve_);

    if (sqrtStop_ > limitPreSieve_)   eratSmall_  = new EratSmall (stop_, sieveSize_, limitEratSmall_);
    if (sqrtStop_ > limitEratSmall_)  eratMedium_ = new EratMedium(stop_, sieveSize_, limitEratMedium_);
    if (sqrtStop_ > limitEratMedium_) eratBig_    = new EratBig   (stop_, sieveSize_, sqrtStop_);
  }
  catch (std::exception&)
  {
    cleanUp();
    throw;
  }
}

uint_t SieveOfEratosthenes::getSqrtStop() const
{
  return sqrtStop_;
}

uint_t SieveOfEratosthenes::getPreSieve() const
{
  return limitPreSieve_;
}

uint64_t SieveOfEratosthenes::getByteRemainder(uint64_t n)
{
  uint64_t r = n % NUMBERS_PER_BYTE;
  if (r <= 1)
    r += NUMBERS_PER_BYTE;
  return r;
}

/// Pre-sieve multiples of small primes e.g. <= 19
/// to speed up the sieve of Eratosthenes.
///
void SieveOfEratosthenes::preSieve()
{
  preSieve_->doIt(sieve_, sieveSize_, segmentLow_);

  // unset bits (numbers) < start_
  if (segmentLow_ <= start_)
  {
    if (start_ <= limitPreSieve_)
      sieve_[0] = 0xff;
    for (int i = 0; bitValues_[i] < getByteRemainder(start_); i++)
      sieve_[0] &= 0xfe << i;
  }
}

void SieveOfEratosthenes::crossOffMultiples()
{
  if (eratSmall_)   eratSmall_->crossOff(sieve_, &sieve_[sieveSize_]);
  if (eratMedium_) eratMedium_->crossOff(sieve_, sieveSize_);
  if (eratBig_)       eratBig_->crossOff(sieve_);
}

void SieveOfEratosthenes::sieveSegment()
{
  preSieve();
  crossOffMultiples();
  segmentFinished(sieve_, sieveSize_);

  // update for next segment
  uint64_t dist = sieveSize_ * NUMBERS_PER_BYTE;
  segmentLow_ = add_overflow_safe(segmentLow_, dist);
  segmentHigh_ = add_overflow_safe(segmentHigh_, dist);
}

/// Sieve the remaining segments after that addSievingPrime(uint_t)
/// has been called for all primes up to sqrt(stop).
///
void SieveOfEratosthenes::sieve()
{
  while (segmentHigh_ < stop_)
    sieveSegment();

  uint64_t remainder = getByteRemainder(stop_);
  uint64_t dist = stop_ - segmentLow_ - remainder;
  sieveSize_ = static_cast<uint_t>(dist) / NUMBERS_PER_BYTE + 1;
  dist = sieveSize_ * NUMBERS_PER_BYTE + 1;
  segmentHigh_ = add_overflow_safe(segmentLow_, dist);

  // sieve the last segment
  preSieve();
  crossOffMultiples();

  int i;
  // unset bits (numbers) > stop_
  for (i = 0; i < 8; i++)
    if (bitValues_[i] > remainder)
      break;
  int unsetBits = ~(0xff << i);
  sieve_[sieveSize_ - 1] &= unsetBits;

  // unset bytes (numbers) > stop_
  for (uint_t j = sieveSize_; j % 8 != 0; j++)
    sieve_[j] = 0;

  segmentFinished(sieve_, sieveSize_);
}

} // namespace
<|endoftext|>