<commit_before>// Polygon.cpp

#include "Polygon.h"
#include "Triangle.h"
#include "IndexTriangle.h"
#include "LineSegment.h"
#include "Surface.h"
#include "AffineTransform.h"
#include "Graph.h"
#include "Exception.h"

using namespace _3DMath;

Polygon::Polygon( void )
{
	vertexArray = new VectorArray();
	indexTriangleList = new IndexTriangleList();
}

/*virtual*/ Polygon::~Polygon( void )
{
	delete vertexArray;
	delete indexTriangleList;
}

void Polygon::Transform( const AffineTransform& transform )
{
	transform.Transform( *vertexArray );
}

bool Polygon::GetPlane( Plane& plane ) const
{
	if( vertexArray->size() < 3 )
		return false;

	Vector center;
	GetCenter( center );

	Vector normal;
	normal.Set( 0.0, 0.0, 0.0 );

	for( int i = 0; i < ( signed )vertexArray->size(); i++ )
	{
		// This is the Newel method.
		int j = ( i + 1 ) % vertexArray->size();
		const Vector& pointA = ( *vertexArray )[i];
		const Vector& pointB = ( *vertexArray )[j];
		normal.x += ( pointA.y - pointB.y ) * ( pointA.z + pointB.z );
		normal.y += ( pointA.z - pointB.z ) * ( pointA.x + pointB.x );
		normal.z += ( pointA.x - pointB.x ) * ( pointA.y + pointB.y );
	}

	plane.SetCenterAndNormal( center, normal );
	return true;
}

void Polygon::GetCenter( Vector& center ) const
{
	center.Set( 0.0, 0.0, 0.0 );
	for( int i = 0; i < ( signed )vertexArray->size(); i++ )
		center.Add( ( *vertexArray )[i] );
	center.Scale( 1.0 / double( vertexArray->size() ) );
}

bool Polygon::GetTriangleAverageCenter( Vector& center ) const
{
	if( indexTriangleList->size() == 0 )
		return false;

	center.Set( 0.0, 0.0, 0.0 );

	for( IndexTriangleList::const_iterator iter = indexTriangleList->cbegin(); iter != indexTriangleList->cend(); iter++ )
	{
		const IndexTriangle& indexTriangle = *iter;

		Triangle triangle;
		indexTriangle.GetTriangle( triangle, vertexArray );

		Vector triangleCenter;
		triangle.GetCenter( triangleCenter );

		center.Add( triangleCenter );
	}

	center.Scale( 1.0 / double( indexTriangleList->size() ) );
	return true;
}

void Polygon::GetIntegratedCenter( Vector& center, double delta ) const
{
	center.Set( 0.0, 0.0, 0.0 );
	int count = 0;

	for( int i = 0; i < ( signed )vertexArray->size(); i++ )
	{
		int j = ( i + 1 ) % vertexArray->size();
		LineSegment lineSegment( ( *vertexArray )[i], ( *vertexArray )[j] );

		int stepCount = ( int )floor( lineSegment.Length() / delta );

		for( int k = 0; k < stepCount; k++ )
		{
			double lambda = double(k) / double( stepCount );

			Vector point;
			lineSegment.Lerp( lambda, point );

			center.Add( point );
			count++;
		}
	}

	center.Scale( 1.0 / double( count ) );
}

// This assumes that we're properly tessellated.
bool Polygon::ContainsPoint( const Vector& point, double eps /*= EPSILON*/ ) const
{
	for( IndexTriangleList::const_iterator iter = indexTriangleList->cbegin(); iter != indexTriangleList->cend(); iter++ )
	{
		const IndexTriangle& indexTriangle = *iter;

		Triangle triangle;
		indexTriangle.GetTriangle( triangle, vertexArray );

		if( triangle.ContainsPoint( point, eps ) )
			return true;
	}

	return false;
}

// The set of returned polygons should be pair-wise disjoint and its union should equal this polygon.
// Note that we require this polygon to be tessellated for our algorithm here to be correct.
bool Polygon::SplitAgainstSurface( const Surface* surface, PolygonList& insidePolygonList, PolygonList& outsidePolygonList, double maxDistance ) const
{
	bool success = true;

	Plane plane;
	GetPlane( plane );

	struct PointData
	{
		Vector point;
		Surface::Side side;
		bool processed;
	};

	typedef TemplateGraphNode< PointData > Node;

	Node* anchorNode = nullptr;

	try
	{
		Node* node = nullptr;

		for( int i = 0; i < ( signed )vertexArray->size(); i++ )
		{
			Node* newNode = new Node();
			newNode->data.processed = false;
			newNode->data.point = ( *vertexArray )[i];
			newNode->data.side = surface->GetSide( newNode->data.point );

			if( !node )
				anchorNode = newNode;
			else
			{
				node->SetAdjacency( "ccw", newNode );
				newNode->SetAdjacency( "cw", node );
			}

			node = newNode;
		}

		if( !node )
			throw new Exception( "The polygon is empty." );
		else
		{
			node->SetAdjacency( "ccw", anchorNode );
			anchorNode->SetAdjacency( "cw", node );
		}

		node = nullptr;

		GraphNodeList graphNodeList;
		GraphNode* graphNode = nullptr;

		GraphTraversor traversor( anchorNode );
		while( traversor.Traverse( graphNode ) )
		{
			Node* cwNode = ( Node* )graphNode;
			Node* ccwNode = ( Node* )cwNode->GetAdjacency( "ccw" );

			if( ( cwNode->data.side == Surface::INSIDE && ccwNode->data.side == Surface::OUTSIDE ) ||
				( cwNode->data.side == Surface::OUTSIDE && ccwNode->data.side == Surface::INSIDE ) )
			{
				graphNodeList.push_back( cwNode );
			}
		}

		for( GraphNodeList::iterator iter = graphNodeList.begin(); iter != graphNodeList.end(); iter++ )
		{
			Node* cwNode = ( Node* )*iter;
			Node* ccwNode = ( Node* )cwNode->GetAdjacency( "ccw" );

			LineSegment lineSegment;
			lineSegment.vertex[0] = cwNode->data.point;
			lineSegment.vertex[1] = ccwNode->data.point;

			SurfacePoint* surfacePoint = surface->FindSingleIntersection( lineSegment );
			if( !surfacePoint )
				throw new Exception( "Failed to intersect polygon edge with surface." );
			else
			{
				Node* newNode = new Node();
				newNode->data.processed = false;
				newNode->data.side = Surface::NEITHER_SIDE;
				surfacePoint->GetLocation( newNode->data.point );
				delete surfacePoint;
				cwNode->SetAdjacency( "ccw", newNode );
				ccwNode->SetAdjacency( "cw", newNode );
				newNode->SetAdjacency( "cw", cwNode );
				newNode->SetAdjacency( "ccw", ccwNode );
			}
		}

		GraphNodeArray graphNodeArray;

		NamedAdjacencyGraphTraversor namedTraversor( "ccw", anchorNode );
		while( namedTraversor.Traverse( graphNode ) )
		{
			node = ( Node* )graphNode;

			Node* cwNode = ( Node* )node->GetAdjacency( "cw" );
			Node* ccwNode = ( Node* )node->GetAdjacency( "ccw" );

			if( node->data.side == Surface::NEITHER_SIDE )
			{
				if( ( cwNode->data.side == Surface::INSIDE && ccwNode->data.side == Surface::OUTSIDE ) ||
					( cwNode->data.side == Surface::OUTSIDE && ccwNode->data.side == Surface::INSIDE ) )
				{
					graphNodeArray.push_back( node );
				}
			}
		}

		int count = ( signed )graphNodeArray.size();
		if( count == 2 )
			count--;

		for( int i = 0; i < count; i++ )
		{
			int j = ( i + 1 ) % graphNodeArray.size();

			Node* nodeA = ( Node* )graphNodeArray[i];
			Node* nodeB = ( Node* )graphNodeArray[j];

			bool pathFound = false;

			SurfacePoint* surfacePointA = surface->GetNearestSurfacePoint( nodeA->data.point );
			SurfacePoint* surfacePointB = surface->GetNearestSurfacePoint( nodeB->data.point );
			SurfacePoint* surfacePointC = nullptr;

			VectorArray pointArray;

			if( surfacePointA && surfacePointB )
			{
				pathFound = surface->FindDirectPath( surfacePointA, surfacePointB, pointArray, maxDistance, &plane );
				if( !pathFound )
				{
					// This is a bit of a hack.
					Vector center;
					GetCenter( center );

					surfacePointC = surface->GetNearestSurfacePoint( center );
					pathFound = surface->FindDirectPath( surfacePointA, surfacePointC, pointArray, maxDistance, &plane );
					if( pathFound )
					{
						pointArray.pop_back();
						pathFound = surface->FindDirectPath( surfacePointC, surfacePointA, pointArray, maxDistance, &plane );
					}
				}
			}

			delete surfacePointA;
			delete surfacePointB;
			delete surfacePointC;

			if( !pathFound )
				throw new Exception( "Failed to find path along surface in plane." );

			for( j = 0; j < ( signed )pointArray.size(); j++ )
				if( !ContainsPoint( pointArray[j], 1e-3 ) )
					break;

			if( j == ( signed )pointArray.size() )
			{
				for( j = 1; j < ( signed )pointArray.size() - 1; j++ )
				{
					Node* newNode = new Node();
					newNode->data.processed = false;
					newNode->data.side = Surface::NEITHER_SIDE;
					newNode->data.point = pointArray[j];

					nodeA->SetAdjacency( "s_d0", newNode );
					newNode->SetAdjacency( "s_d1", nodeA );

					nodeA = newNode;
				}

				nodeA->SetAdjacency( "s_d0", nodeB );
				nodeB->SetAdjacency( "s_d1", nodeA );
			}
		}

		while( true )
		{
			Node* foundNode = nullptr;

			traversor.Reset( anchorNode );
			while( traversor.Traverse( graphNode ) )
			{
				node = ( Node* )graphNode;
				if( node->data.side != Surface::NEITHER_SIDE && !node->data.processed )
				{
					foundNode = node;
					break;
				}
			}

			if( !foundNode )
				break;

			Surface::Side side = foundNode->data.side;

			Polygon* polygon = new Polygon;
			if( side == Surface::INSIDE )
				insidePolygonList.push_back( polygon );
			else if( side == Surface::OUTSIDE )
				outsidePolygonList.push_back( polygon );
			else
			{
				delete polygon;
				throw new Exception( "Encountered unexpected surface side." );
			}

			node = foundNode;

			std::string surfaceDir;

			do
			{
				if( node->data.side != side && node->data.side != Surface::NEITHER_SIDE )
					throw new Exception( "Encountered node on wrong side." );

				polygon->vertexArray->push_back( node->data.point );
				node->data.processed = true;

				Node* adjacentNode = nullptr;

				if( node->data.side == Surface::NEITHER_SIDE )
				{
					if( !surfaceDir.empty() )
						adjacentNode = ( Node* )node->GetAdjacency( surfaceDir );
					else
					{
						Node* nodeDir0 = ( Node* )node->GetAdjacency( "s_d0" );
						Node* nodeDir1 = ( Node* )node->GetAdjacency( "s_d1" );

						if( nodeDir0 && nodeDir1 )
							throw new Exception( "Ambiguous case encountered." );
						else if( nodeDir0 )
						{
							adjacentNode = nodeDir0;
							surfaceDir = "s_d0";
						}
						else if( nodeDir1 )
						{
							adjacentNode = nodeDir1;
							surfaceDir = "s_d1";
						}
						else
							throw new Exception( "No where to go." );
					}
				}

				if( !adjacentNode )
				{
					adjacentNode = ( Node* )node->GetAdjacency( "ccw" );
					surfaceDir = "";
				}

				if( !adjacentNode )
					throw new Exception( "Encountered unexpected boundary." );

				node = adjacentNode;
			}
			while( node != foundNode );
		}
	}
	catch( Exception* exception )
	{
		exception->Handle();
		delete exception;
		success = false;
	}

	DeleteGraph( anchorNode );

	return success;
}

double Polygon::GetArea( void ) const
{
	double totalArea = 0.0;

	for( IndexTriangleList::const_iterator iter = indexTriangleList->cbegin(); iter != indexTriangleList->end(); iter++ )
	{
		const IndexTriangle& indexTriangle = *iter;
		Triangle triangle;
		indexTriangle.GetTriangle( triangle, vertexArray );
		totalArea += triangle.Area();
	}

	return totalArea;
}

bool Polygon::Tessellate( void ) const
{
	Plane plane;
	if( !GetPlane( plane ) )
		return false;

	indexTriangleList->clear();

	std::vector< int > indexArray;
	for( int i = 0; i < ( signed )vertexArray->size(); i++ )
		indexArray.push_back(i);

	while( indexArray.size() > 2 )
	{
		int i;
		for( i = 0; i < ( signed )indexArray.size(); i++ )
		{
			IndexTriangle indexTriangle(
					indexArray[i],
					indexArray[ ( i + 1 ) % indexArray.size() ],
					indexArray[ ( i + 2 ) % indexArray.size() ] );

			Triangle triangle;
			indexTriangle.GetTriangle( triangle, vertexArray );

			Vector edge[2];
			edge[0].Subtract( triangle.vertex[1], triangle.vertex[0] );
			edge[1].Subtract( triangle.vertex[2], triangle.vertex[1] );

			Vector cross;
			cross.Cross( edge[0], edge[1] );
			double dot = cross.Dot( plane.normal );
			if( dot < 0.0 )
				continue;

			int j;
			for( j = 0; j < ( signed )indexArray.size(); j++ )
			{
				if( j == i || j == ( i + 1 ) % indexArray.size() || j == ( i + 2 ) % indexArray.size() )
					continue;
				if( triangle.ContainsPoint( ( *vertexArray )[ indexArray[j] ] ) )
					break;
			}

			if( j < ( signed )indexArray.size() )
				continue;

			indexTriangleList->push_back( indexTriangle );
			indexArray.erase( indexArray.begin() + ( i + 1 ) % indexArray.size() );
			break;
		}

		if( i == ( signed )indexArray.size() )
			return false;		// Avoid an infinite loop.
	}

	return true;
}

// Polygon.cpp<commit_msg>thought<commit_after>// Polygon.cpp

#include "Polygon.h"
#include "Triangle.h"
#include "IndexTriangle.h"
#include "LineSegment.h"
#include "Surface.h"
#include "AffineTransform.h"
#include "Graph.h"
#include "Exception.h"

using namespace _3DMath;

Polygon::Polygon( void )
{
	vertexArray = new VectorArray();
	indexTriangleList = new IndexTriangleList();
}

/*virtual*/ Polygon::~Polygon( void )
{
	delete vertexArray;
	delete indexTriangleList;
}

void Polygon::Transform( const AffineTransform& transform )
{
	transform.Transform( *vertexArray );
}

bool Polygon::GetPlane( Plane& plane ) const
{
	if( vertexArray->size() < 3 )
		return false;

	Vector center;
	GetCenter( center );

	Vector normal;
	normal.Set( 0.0, 0.0, 0.0 );

	for( int i = 0; i < ( signed )vertexArray->size(); i++ )
	{
		// This is the Newel method.
		int j = ( i + 1 ) % vertexArray->size();
		const Vector& pointA = ( *vertexArray )[i];
		const Vector& pointB = ( *vertexArray )[j];
		normal.x += ( pointA.y - pointB.y ) * ( pointA.z + pointB.z );
		normal.y += ( pointA.z - pointB.z ) * ( pointA.x + pointB.x );
		normal.z += ( pointA.x - pointB.x ) * ( pointA.y + pointB.y );
	}

	plane.SetCenterAndNormal( center, normal );
	return true;
}

void Polygon::GetCenter( Vector& center ) const
{
	center.Set( 0.0, 0.0, 0.0 );
	for( int i = 0; i < ( signed )vertexArray->size(); i++ )
		center.Add( ( *vertexArray )[i] );
	center.Scale( 1.0 / double( vertexArray->size() ) );
}

bool Polygon::GetTriangleAverageCenter( Vector& center ) const
{
	if( indexTriangleList->size() == 0 )
		return false;

	center.Set( 0.0, 0.0, 0.0 );

	for( IndexTriangleList::const_iterator iter = indexTriangleList->cbegin(); iter != indexTriangleList->cend(); iter++ )
	{
		const IndexTriangle& indexTriangle = *iter;

		Triangle triangle;
		indexTriangle.GetTriangle( triangle, vertexArray );

		Vector triangleCenter;
		triangle.GetCenter( triangleCenter );

		center.Add( triangleCenter );
	}

	center.Scale( 1.0 / double( indexTriangleList->size() ) );
	return true;
}

void Polygon::GetIntegratedCenter( Vector& center, double delta ) const
{
	center.Set( 0.0, 0.0, 0.0 );
	int count = 0;

	for( int i = 0; i < ( signed )vertexArray->size(); i++ )
	{
		int j = ( i + 1 ) % vertexArray->size();
		LineSegment lineSegment( ( *vertexArray )[i], ( *vertexArray )[j] );

		int stepCount = ( int )floor( lineSegment.Length() / delta );

		for( int k = 0; k < stepCount; k++ )
		{
			double lambda = double(k) / double( stepCount );

			Vector point;
			lineSegment.Lerp( lambda, point );

			center.Add( point );
			count++;
		}
	}

	center.Scale( 1.0 / double( count ) );
}

// This assumes that we're properly tessellated.
bool Polygon::ContainsPoint( const Vector& point, double eps /*= EPSILON*/ ) const
{
	for( IndexTriangleList::const_iterator iter = indexTriangleList->cbegin(); iter != indexTriangleList->cend(); iter++ )
	{
		const IndexTriangle& indexTriangle = *iter;

		Triangle triangle;
		indexTriangle.GetTriangle( triangle, vertexArray );

		if( triangle.ContainsPoint( point, eps ) )
			return true;
	}

	return false;
}

// The set of returned polygons should be pair-wise disjoint and its union should equal this polygon.
// Note that we require this polygon to be tessellated for our algorithm here to be correct.
bool Polygon::SplitAgainstSurface( const Surface* surface, PolygonList& insidePolygonList, PolygonList& outsidePolygonList, double maxDistance ) const
{
	bool success = true;

	Plane plane;
	GetPlane( plane );

	struct PointData
	{
		Vector point;
		Surface::Side side;
		bool processed;
	};

	typedef TemplateGraphNode< PointData > Node;

	Node* anchorNode = nullptr;

	try
	{
		Node* node = nullptr;

		for( int i = 0; i < ( signed )vertexArray->size(); i++ )
		{
			Node* newNode = new Node();
			newNode->data.processed = false;
			newNode->data.point = ( *vertexArray )[i];
			newNode->data.side = surface->GetSide( newNode->data.point );

			if( !node )
				anchorNode = newNode;
			else
			{
				node->SetAdjacency( "ccw", newNode );
				newNode->SetAdjacency( "cw", node );
			}

			node = newNode;
		}

		if( !node )
			throw new Exception( "The polygon is empty." );
		else
		{
			node->SetAdjacency( "ccw", anchorNode );
			anchorNode->SetAdjacency( "cw", node );
		}

		node = nullptr;

		GraphNodeList graphNodeList;
		GraphNode* graphNode = nullptr;

		GraphTraversor traversor( anchorNode );
		while( traversor.Traverse( graphNode ) )
		{
			Node* cwNode = ( Node* )graphNode;
			Node* ccwNode = ( Node* )cwNode->GetAdjacency( "ccw" );

			if( ( cwNode->data.side == Surface::INSIDE && ccwNode->data.side == Surface::OUTSIDE ) ||
				( cwNode->data.side == Surface::OUTSIDE && ccwNode->data.side == Surface::INSIDE ) )
			{
				graphNodeList.push_back( cwNode );
			}
		}

		for( GraphNodeList::iterator iter = graphNodeList.begin(); iter != graphNodeList.end(); iter++ )
		{
			Node* cwNode = ( Node* )*iter;
			Node* ccwNode = ( Node* )cwNode->GetAdjacency( "ccw" );

			LineSegment lineSegment;
			lineSegment.vertex[0] = cwNode->data.point;
			lineSegment.vertex[1] = ccwNode->data.point;

			SurfacePoint* surfacePoint = surface->FindSingleIntersection( lineSegment );
			if( !surfacePoint )
				throw new Exception( "Failed to intersect polygon edge with surface." );
			else
			{
				Node* newNode = new Node();
				newNode->data.processed = false;
				newNode->data.side = Surface::NEITHER_SIDE;
				surfacePoint->GetLocation( newNode->data.point );
				delete surfacePoint;
				cwNode->SetAdjacency( "ccw", newNode );
				ccwNode->SetAdjacency( "cw", newNode );
				newNode->SetAdjacency( "cw", cwNode );
				newNode->SetAdjacency( "ccw", ccwNode );
			}
		}

		GraphNodeArray graphNodeArray;

		// Note that if the polygon didn't have a high-enough density of vertices,
		// (we can, without changing the polygon, add vertices on edges), then we
		// can miss nodes here that need to be processed.
		NamedAdjacencyGraphTraversor namedTraversor( "ccw", anchorNode );
		while( namedTraversor.Traverse( graphNode ) )
		{
			node = ( Node* )graphNode;

			Node* cwNode = ( Node* )node->GetAdjacency( "cw" );
			Node* ccwNode = ( Node* )node->GetAdjacency( "ccw" );

			if( node->data.side == Surface::NEITHER_SIDE )
			{
				if( ( cwNode->data.side == Surface::INSIDE && ccwNode->data.side == Surface::OUTSIDE ) ||
					( cwNode->data.side == Surface::OUTSIDE && ccwNode->data.side == Surface::INSIDE ) )
				{
					graphNodeArray.push_back( node );
				}
			}
		}

		int count = ( signed )graphNodeArray.size();
		if( count == 2 )
			count--;

		for( int i = 0; i < count; i++ )
		{
			int j = ( i + 1 ) % graphNodeArray.size();

			Node* nodeA = ( Node* )graphNodeArray[i];
			Node* nodeB = ( Node* )graphNodeArray[j];

			bool pathFound = false;

			SurfacePoint* surfacePointA = surface->GetNearestSurfacePoint( nodeA->data.point );
			SurfacePoint* surfacePointB = surface->GetNearestSurfacePoint( nodeB->data.point );
			SurfacePoint* surfacePointC = nullptr;

			VectorArray pointArray;

			if( surfacePointA && surfacePointB )
			{
				pathFound = surface->FindDirectPath( surfacePointA, surfacePointB, pointArray, maxDistance, &plane );
				if( !pathFound )
				{
					// This is a bit of a hack.
					Vector center;
					GetCenter( center );

					surfacePointC = surface->GetNearestSurfacePoint( center );
					pathFound = surface->FindDirectPath( surfacePointA, surfacePointC, pointArray, maxDistance, &plane );
					if( pathFound )
					{
						pointArray.pop_back();
						pathFound = surface->FindDirectPath( surfacePointC, surfacePointA, pointArray, maxDistance, &plane );
					}
				}
			}

			delete surfacePointA;
			delete surfacePointB;
			delete surfacePointC;

			if( !pathFound )
				throw new Exception( "Failed to find path along surface in plane." );

			for( j = 0; j < ( signed )pointArray.size(); j++ )
				if( !ContainsPoint( pointArray[j], 1e-3 ) )
					break;

			if( j == ( signed )pointArray.size() )
			{
				for( j = 1; j < ( signed )pointArray.size() - 1; j++ )
				{
					Node* newNode = new Node();
					newNode->data.processed = false;
					newNode->data.side = Surface::NEITHER_SIDE;
					newNode->data.point = pointArray[j];

					nodeA->SetAdjacency( "s_d0", newNode );
					newNode->SetAdjacency( "s_d1", nodeA );

					nodeA = newNode;
				}

				nodeA->SetAdjacency( "s_d0", nodeB );
				nodeB->SetAdjacency( "s_d1", nodeA );
			}
		}

		while( true )
		{
			Node* foundNode = nullptr;

			traversor.Reset( anchorNode );
			while( traversor.Traverse( graphNode ) )
			{
				node = ( Node* )graphNode;
				if( node->data.side != Surface::NEITHER_SIDE && !node->data.processed )
				{
					foundNode = node;
					break;
				}
			}

			if( !foundNode )
				break;

			Surface::Side side = foundNode->data.side;

			Polygon* polygon = new Polygon;
			if( side == Surface::INSIDE )
				insidePolygonList.push_back( polygon );
			else if( side == Surface::OUTSIDE )
				outsidePolygonList.push_back( polygon );
			else
			{
				delete polygon;
				throw new Exception( "Encountered unexpected surface side." );
			}

			node = foundNode;

			std::string surfaceDir;

			do
			{
				if( node->data.side != side && node->data.side != Surface::NEITHER_SIDE )
					throw new Exception( "Encountered node on wrong side." );

				polygon->vertexArray->push_back( node->data.point );
				node->data.processed = true;

				Node* adjacentNode = nullptr;

				if( node->data.side == Surface::NEITHER_SIDE )
				{
					if( !surfaceDir.empty() )
						adjacentNode = ( Node* )node->GetAdjacency( surfaceDir );
					else
					{
						Node* nodeDir0 = ( Node* )node->GetAdjacency( "s_d0" );
						Node* nodeDir1 = ( Node* )node->GetAdjacency( "s_d1" );

						if( nodeDir0 && nodeDir1 )
							throw new Exception( "Ambiguous case encountered." );
						else if( nodeDir0 )
						{
							adjacentNode = nodeDir0;
							surfaceDir = "s_d0";
						}
						else if( nodeDir1 )
						{
							adjacentNode = nodeDir1;
							surfaceDir = "s_d1";
						}
						else
							throw new Exception( "No where to go." );
					}
				}

				if( !adjacentNode )
				{
					adjacentNode = ( Node* )node->GetAdjacency( "ccw" );
					surfaceDir = "";
				}

				if( !adjacentNode )
					throw new Exception( "Encountered unexpected boundary." );

				node = adjacentNode;
			}
			while( node != foundNode );
		}
	}
	catch( Exception* exception )
	{
		exception->Handle();
		delete exception;
		success = false;
	}

	DeleteGraph( anchorNode );

	return success;
}

double Polygon::GetArea( void ) const
{
	double totalArea = 0.0;

	for( IndexTriangleList::const_iterator iter = indexTriangleList->cbegin(); iter != indexTriangleList->end(); iter++ )
	{
		const IndexTriangle& indexTriangle = *iter;
		Triangle triangle;
		indexTriangle.GetTriangle( triangle, vertexArray );
		totalArea += triangle.Area();
	}

	return totalArea;
}

bool Polygon::Tessellate( void ) const
{
	Plane plane;
	if( !GetPlane( plane ) )
		return false;

	indexTriangleList->clear();

	std::vector< int > indexArray;
	for( int i = 0; i < ( signed )vertexArray->size(); i++ )
		indexArray.push_back(i);

	while( indexArray.size() > 2 )
	{
		int i;
		for( i = 0; i < ( signed )indexArray.size(); i++ )
		{
			IndexTriangle indexTriangle(
					indexArray[i],
					indexArray[ ( i + 1 ) % indexArray.size() ],
					indexArray[ ( i + 2 ) % indexArray.size() ] );

			Triangle triangle;
			indexTriangle.GetTriangle( triangle, vertexArray );

			Vector edge[2];
			edge[0].Subtract( triangle.vertex[1], triangle.vertex[0] );
			edge[1].Subtract( triangle.vertex[2], triangle.vertex[1] );

			Vector cross;
			cross.Cross( edge[0], edge[1] );
			double dot = cross.Dot( plane.normal );
			if( dot < 0.0 )
				continue;

			int j;
			for( j = 0; j < ( signed )indexArray.size(); j++ )
			{
				if( j == i || j == ( i + 1 ) % indexArray.size() || j == ( i + 2 ) % indexArray.size() )
					continue;
				if( triangle.ContainsPoint( ( *vertexArray )[ indexArray[j] ] ) )
					break;
			}

			if( j < ( signed )indexArray.size() )
				continue;

			indexTriangleList->push_back( indexTriangle );
			indexArray.erase( indexArray.begin() + ( i + 1 ) % indexArray.size() );
			break;
		}

		if( i == ( signed )indexArray.size() )
			return false;		// Avoid an infinite loop.
	}

	return true;
}

// Polygon.cpp<|endoftext|>


<commit_before>/*
 * Copyright © 2010 Intel Corporation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice (including the next
 * paragraph) shall be included in all copies or substantial portions of the
 * Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

#include "main/core.h"
#include "glsl_symbol_table.h"
#include "glsl_parser_extras.h"
#include "ir.h"
#include "program.h"
#include "program/hash_table.h"
#include "linker.h"

static ir_function_signature *
find_matching_signature(const char *name, const exec_list *actual_parameters,
                        glsl_symbol_table *symbols, bool use_builtin);

namespace {

class call_link_visitor : public ir_hierarchical_visitor {
public:
   call_link_visitor(gl_shader_program *prog, gl_linked_shader *linked,
		     gl_shader **shader_list, unsigned num_shaders)
   {
      this->prog = prog;
      this->shader_list = shader_list;
      this->num_shaders = num_shaders;
      this->success = true;
      this->linked = linked;

      this->locals = hash_table_ctor(0, hash_table_pointer_hash,
				     hash_table_pointer_compare);
   }

   ~call_link_visitor()
   {
      hash_table_dtor(this->locals);
   }

   virtual ir_visitor_status visit(ir_variable *ir)
   {
      hash_table_insert(locals, ir, ir);
      return visit_continue;
   }

   virtual ir_visitor_status visit_enter(ir_call *ir)
   {
      /* If ir is an ir_call from a function that was imported from another
       * shader callee will point to an ir_function_signature in the original
       * shader.  In this case the function signature MUST NOT BE MODIFIED.
       * Doing so will modify the original shader.  This may prevent that
       * shader from being linkable in other programs.
       */
      const ir_function_signature *const callee = ir->callee;
      assert(callee != NULL);
      const char *const name = callee->function_name();

      /* Determine if the requested function signature already exists in the
       * final linked shader.  If it does, use it as the target of the call.
       */
      ir_function_signature *sig =
         find_matching_signature(name, &callee->parameters, linked->symbols,
                                 ir->use_builtin);
      if (sig != NULL) {
	 ir->callee = sig;
	 return visit_continue;
      }

      /* Try to find the signature in one of the other shaders that is being
       * linked.  If it's not found there, return an error.
       */
      for (unsigned i = 0; i < num_shaders; i++) {
         sig = find_matching_signature(name, &ir->actual_parameters,
                                       shader_list[i]->symbols,
                                       ir->use_builtin);
         if (sig)
            break;
      }

      if (sig == NULL) {
	 /* FINISHME: Log the full signature of unresolved function.
	  */
	 linker_error(this->prog, "unresolved reference to function `%s'\n",
		      name);
	 this->success = false;
	 return visit_stop;
      }

      /* Find the prototype information in the linked shader.  Generate any
       * details that may be missing.
       */
      ir_function *f = linked->symbols->get_function(name);
      if (f == NULL) {
	 f = new(linked) ir_function(name);

	 /* Add the new function to the linked IR.  Put it at the end
          * so that it comes after any global variable declarations
          * that it refers to.
	  */
	 linked->symbols->add_function(f);
	 linked->ir->push_tail(f);
      }

      ir_function_signature *linked_sig =
	 f->exact_matching_signature(NULL, &callee->parameters);
      if ((linked_sig == NULL)
	  || ((linked_sig != NULL)
	      && (linked_sig->is_builtin() != ir->use_builtin))) {
	 linked_sig = new(linked) ir_function_signature(callee->return_type);
	 f->add_signature(linked_sig);
      }

      /* At this point linked_sig and called may be the same.  If ir is an
       * ir_call from linked then linked_sig and callee will be
       * ir_function_signatures that have no definitions (is_defined is false).
       */
      assert(!linked_sig->is_defined);
      assert(linked_sig->body.is_empty());

      /* Create an in-place clone of the function definition.  This multistep
       * process introduces some complexity here, but it has some advantages.
       * The parameter list and the and function body are cloned separately.
       * The clone of the parameter list is used to prime the hashtable used
       * to replace variable references in the cloned body.
       *
       * The big advantage is that the ir_function_signature does not change.
       * This means that we don't have to process the rest of the IR tree to
       * patch ir_call nodes.  In addition, there is no way to remove or
       * replace signature stored in a function.  One could easily be added,
       * but this avoids the need.
       */
      struct hash_table *ht = hash_table_ctor(0, hash_table_pointer_hash,
					      hash_table_pointer_compare);
      exec_list formal_parameters;
      foreach_in_list(const ir_instruction, original, &sig->parameters) {
	 assert(const_cast<ir_instruction *>(original)->as_variable());

	 ir_instruction *copy = original->clone(linked, ht);
	 formal_parameters.push_tail(copy);
      }

      linked_sig->replace_parameters(&formal_parameters);

      linked_sig->is_intrinsic = sig->is_intrinsic;

      if (sig->is_defined) {
         foreach_in_list(const ir_instruction, original, &sig->body) {
            ir_instruction *copy = original->clone(linked, ht);
            linked_sig->body.push_tail(copy);
         }

         linked_sig->is_defined = true;
      }

      hash_table_dtor(ht);

      /* Patch references inside the function to things outside the function
       * (i.e., function calls and global variables).
       */
      linked_sig->accept(this);

      ir->callee = linked_sig;

      return visit_continue;
   }

   virtual ir_visitor_status visit_leave(ir_call *ir)
   {
      /* Traverse list of function parameters, and for array parameters
       * propagate max_array_access. Otherwise arrays that are only referenced
       * from inside functions via function parameters will be incorrectly
       * optimized. This will lead to incorrect code being generated (or worse).
       * Do it when leaving the node so the children would propagate their
       * array accesses first.
       */

      const exec_node *formal_param_node = ir->callee->parameters.get_head();
      if (formal_param_node) {
         const exec_node *actual_param_node = ir->actual_parameters.get_head();
         while (!actual_param_node->is_tail_sentinel()) {
            ir_variable *formal_param = (ir_variable *) formal_param_node;
            ir_rvalue *actual_param = (ir_rvalue *) actual_param_node;

            formal_param_node = formal_param_node->get_next();
            actual_param_node = actual_param_node->get_next();

            if (formal_param->type->is_array()) {
               ir_dereference_variable *deref = actual_param->as_dereference_variable();
               if (deref && deref->var && deref->var->type->is_array()) {
                  deref->var->data.max_array_access =
                     MAX2(formal_param->data.max_array_access,
                         deref->var->data.max_array_access);
               }
            }
         }
      }
      return visit_continue;
   }

   virtual ir_visitor_status visit(ir_dereference_variable *ir)
   {
      if (hash_table_find(locals, ir->var) == NULL) {
	 /* The non-function variable must be a global, so try to find the
	  * variable in the shader's symbol table.  If the variable is not
	  * found, then it's a global that *MUST* be defined in the original
	  * shader.
	  */
	 ir_variable *var = linked->symbols->get_variable(ir->var->name);
	 if (var == NULL) {
	    /* Clone the ir_variable that the dereference already has and add
	     * it to the linked shader.
	     */
	    var = ir->var->clone(linked, NULL);
	    linked->symbols->add_variable(var);
	    linked->ir->push_head(var);
	 } else {
            if (var->type->is_array()) {
               /* It is possible to have a global array declared in multiple
                * shaders without a size.  The array is implicitly sized by
                * the maximal access to it in *any* shader.  Because of this,
                * we need to track the maximal access to the array as linking
                * pulls more functions in that access the array.
                */
               var->data.max_array_access =
                  MAX2(var->data.max_array_access,
                       ir->var->data.max_array_access);

               if (var->type->length == 0 && ir->var->type->length != 0)
                  var->type = ir->var->type;
            }
            if (var->is_interface_instance()) {
               /* Similarly, we need implicit sizes of arrays within interface
                * blocks to be sized by the maximal access in *any* shader.
                */
               int *const linked_max_ifc_array_access =
                  var->get_max_ifc_array_access();
               int *const ir_max_ifc_array_access =
                  ir->var->get_max_ifc_array_access();

               assert(linked_max_ifc_array_access != NULL);
               assert(ir_max_ifc_array_access != NULL);

               for (unsigned i = 0; i < var->get_interface_type()->length;
                    i++) {
                  linked_max_ifc_array_access[i] =
                     MAX2(linked_max_ifc_array_access[i],
                          ir_max_ifc_array_access[i]);
               }
            }
	 }

	 ir->var = var;
      }

      return visit_continue;
   }

   /** Was function linking successful? */
   bool success;

private:
   /**
    * Shader program being linked
    *
    * This is only used for logging error messages.
    */
   gl_shader_program *prog;

   /** List of shaders available for linking. */
   gl_shader **shader_list;

   /** Number of shaders available for linking. */
   unsigned num_shaders;

   /**
    * Final linked shader
    *
    * This is used two ways.  It is used to find global variables in the
    * linked shader that are accessed by the function.  It is also used to add
    * global variables from the shader where the function originated.
    */
   gl_linked_shader *linked;

   /**
    * Table of variables local to the function.
    */
   hash_table *locals;
};

} /* anonymous namespace */

/**
 * Searches a list of shaders for a particular function definition
 */
ir_function_signature *
find_matching_signature(const char *name, const exec_list *actual_parameters,
                        glsl_symbol_table *symbols, bool use_builtin)
{
   ir_function *const f = symbols->get_function(name);

   if (f) {
      ir_function_signature *sig =
         f->matching_signature(NULL, actual_parameters, use_builtin);

      if (sig && (sig->is_defined || sig->is_intrinsic)) {
         /* If this function expects to bind to a built-in function and the
          * signature that we found isn't a built-in, keep looking.  Also keep
          * looking if we expect a non-built-in but found a built-in.
          */
         if (use_builtin == sig->is_builtin())
            return sig;
      }
   }

   return NULL;
}

bool
link_function_calls(gl_shader_program *prog, gl_linked_shader *main,
		    gl_shader **shader_list, unsigned num_shaders)
{
   call_link_visitor v(prog, main, shader_list, num_shaders);

   v.run(main->ir);
   return v.success;
}
<commit_msg>glsl: Change link_functions to use a set<commit_after>/*
 * Copyright © 2010 Intel Corporation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice (including the next
 * paragraph) shall be included in all copies or substantial portions of the
 * Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

#include "main/core.h"
#include "glsl_symbol_table.h"
#include "glsl_parser_extras.h"
#include "ir.h"
#include "program.h"
#include "util/set.h"
#include "util/hash_table.h"
#include "linker.h"

static ir_function_signature *
find_matching_signature(const char *name, const exec_list *actual_parameters,
                        glsl_symbol_table *symbols, bool use_builtin);

namespace {

class call_link_visitor : public ir_hierarchical_visitor {
public:
   call_link_visitor(gl_shader_program *prog, gl_linked_shader *linked,
		     gl_shader **shader_list, unsigned num_shaders)
   {
      this->prog = prog;
      this->shader_list = shader_list;
      this->num_shaders = num_shaders;
      this->success = true;
      this->linked = linked;

      this->locals = _mesa_set_create(NULL, _mesa_hash_pointer,
                                      _mesa_key_pointer_equal);
   }

   ~call_link_visitor()
   {
      _mesa_set_destroy(this->locals, NULL);
   }

   virtual ir_visitor_status visit(ir_variable *ir)
   {
      _mesa_set_add(locals, ir);
      return visit_continue;
   }

   virtual ir_visitor_status visit_enter(ir_call *ir)
   {
      /* If ir is an ir_call from a function that was imported from another
       * shader callee will point to an ir_function_signature in the original
       * shader.  In this case the function signature MUST NOT BE MODIFIED.
       * Doing so will modify the original shader.  This may prevent that
       * shader from being linkable in other programs.
       */
      const ir_function_signature *const callee = ir->callee;
      assert(callee != NULL);
      const char *const name = callee->function_name();

      /* Determine if the requested function signature already exists in the
       * final linked shader.  If it does, use it as the target of the call.
       */
      ir_function_signature *sig =
         find_matching_signature(name, &callee->parameters, linked->symbols,
                                 ir->use_builtin);
      if (sig != NULL) {
	 ir->callee = sig;
	 return visit_continue;
      }

      /* Try to find the signature in one of the other shaders that is being
       * linked.  If it's not found there, return an error.
       */
      for (unsigned i = 0; i < num_shaders; i++) {
         sig = find_matching_signature(name, &ir->actual_parameters,
                                       shader_list[i]->symbols,
                                       ir->use_builtin);
         if (sig)
            break;
      }

      if (sig == NULL) {
	 /* FINISHME: Log the full signature of unresolved function.
	  */
	 linker_error(this->prog, "unresolved reference to function `%s'\n",
		      name);
	 this->success = false;
	 return visit_stop;
      }

      /* Find the prototype information in the linked shader.  Generate any
       * details that may be missing.
       */
      ir_function *f = linked->symbols->get_function(name);
      if (f == NULL) {
	 f = new(linked) ir_function(name);

	 /* Add the new function to the linked IR.  Put it at the end
          * so that it comes after any global variable declarations
          * that it refers to.
	  */
	 linked->symbols->add_function(f);
	 linked->ir->push_tail(f);
      }

      ir_function_signature *linked_sig =
	 f->exact_matching_signature(NULL, &callee->parameters);
      if ((linked_sig == NULL)
	  || ((linked_sig != NULL)
	      && (linked_sig->is_builtin() != ir->use_builtin))) {
	 linked_sig = new(linked) ir_function_signature(callee->return_type);
	 f->add_signature(linked_sig);
      }

      /* At this point linked_sig and called may be the same.  If ir is an
       * ir_call from linked then linked_sig and callee will be
       * ir_function_signatures that have no definitions (is_defined is false).
       */
      assert(!linked_sig->is_defined);
      assert(linked_sig->body.is_empty());

      /* Create an in-place clone of the function definition.  This multistep
       * process introduces some complexity here, but it has some advantages.
       * The parameter list and the and function body are cloned separately.
       * The clone of the parameter list is used to prime the hashtable used
       * to replace variable references in the cloned body.
       *
       * The big advantage is that the ir_function_signature does not change.
       * This means that we don't have to process the rest of the IR tree to
       * patch ir_call nodes.  In addition, there is no way to remove or
       * replace signature stored in a function.  One could easily be added,
       * but this avoids the need.
       */
      struct hash_table *ht = _mesa_hash_table_create(NULL, _mesa_hash_pointer,
                                                      _mesa_key_pointer_equal);

      exec_list formal_parameters;
      foreach_in_list(const ir_instruction, original, &sig->parameters) {
         assert(const_cast<ir_instruction *>(original)->as_variable());

         ir_instruction *copy = original->clone(linked, ht);
         formal_parameters.push_tail(copy);
      }

      linked_sig->replace_parameters(&formal_parameters);

      linked_sig->is_intrinsic = sig->is_intrinsic;

      if (sig->is_defined) {
         foreach_in_list(const ir_instruction, original, &sig->body) {
            ir_instruction *copy = original->clone(linked, ht);
            linked_sig->body.push_tail(copy);
         }

         linked_sig->is_defined = true;
      }

      _mesa_hash_table_destroy(ht, NULL);

      /* Patch references inside the function to things outside the function
       * (i.e., function calls and global variables).
       */
      linked_sig->accept(this);

      ir->callee = linked_sig;

      return visit_continue;
   }

   virtual ir_visitor_status visit_leave(ir_call *ir)
   {
      /* Traverse list of function parameters, and for array parameters
       * propagate max_array_access. Otherwise arrays that are only referenced
       * from inside functions via function parameters will be incorrectly
       * optimized. This will lead to incorrect code being generated (or worse).
       * Do it when leaving the node so the children would propagate their
       * array accesses first.
       */

      const exec_node *formal_param_node = ir->callee->parameters.get_head();
      if (formal_param_node) {
         const exec_node *actual_param_node = ir->actual_parameters.get_head();
         while (!actual_param_node->is_tail_sentinel()) {
            ir_variable *formal_param = (ir_variable *) formal_param_node;
            ir_rvalue *actual_param = (ir_rvalue *) actual_param_node;

            formal_param_node = formal_param_node->get_next();
            actual_param_node = actual_param_node->get_next();

            if (formal_param->type->is_array()) {
               ir_dereference_variable *deref = actual_param->as_dereference_variable();
               if (deref && deref->var && deref->var->type->is_array()) {
                  deref->var->data.max_array_access =
                     MAX2(formal_param->data.max_array_access,
                         deref->var->data.max_array_access);
               }
            }
         }
      }
      return visit_continue;
   }

   virtual ir_visitor_status visit(ir_dereference_variable *ir)
   {
      if (_mesa_set_search(locals, ir->var) == NULL) {
	 /* The non-function variable must be a global, so try to find the
	  * variable in the shader's symbol table.  If the variable is not
	  * found, then it's a global that *MUST* be defined in the original
	  * shader.
	  */
	 ir_variable *var = linked->symbols->get_variable(ir->var->name);
	 if (var == NULL) {
	    /* Clone the ir_variable that the dereference already has and add
	     * it to the linked shader.
	     */
	    var = ir->var->clone(linked, NULL);
	    linked->symbols->add_variable(var);
	    linked->ir->push_head(var);
	 } else {
            if (var->type->is_array()) {
               /* It is possible to have a global array declared in multiple
                * shaders without a size.  The array is implicitly sized by
                * the maximal access to it in *any* shader.  Because of this,
                * we need to track the maximal access to the array as linking
                * pulls more functions in that access the array.
                */
               var->data.max_array_access =
                  MAX2(var->data.max_array_access,
                       ir->var->data.max_array_access);

               if (var->type->length == 0 && ir->var->type->length != 0)
                  var->type = ir->var->type;
            }
            if (var->is_interface_instance()) {
               /* Similarly, we need implicit sizes of arrays within interface
                * blocks to be sized by the maximal access in *any* shader.
                */
               int *const linked_max_ifc_array_access =
                  var->get_max_ifc_array_access();
               int *const ir_max_ifc_array_access =
                  ir->var->get_max_ifc_array_access();

               assert(linked_max_ifc_array_access != NULL);
               assert(ir_max_ifc_array_access != NULL);

               for (unsigned i = 0; i < var->get_interface_type()->length;
                    i++) {
                  linked_max_ifc_array_access[i] =
                     MAX2(linked_max_ifc_array_access[i],
                          ir_max_ifc_array_access[i]);
               }
            }
	 }

	 ir->var = var;
      }

      return visit_continue;
   }

   /** Was function linking successful? */
   bool success;

private:
   /**
    * Shader program being linked
    *
    * This is only used for logging error messages.
    */
   gl_shader_program *prog;

   /** List of shaders available for linking. */
   gl_shader **shader_list;

   /** Number of shaders available for linking. */
   unsigned num_shaders;

   /**
    * Final linked shader
    *
    * This is used two ways.  It is used to find global variables in the
    * linked shader that are accessed by the function.  It is also used to add
    * global variables from the shader where the function originated.
    */
   gl_linked_shader *linked;

   /**
    * Table of variables local to the function.
    */
   set *locals;
};

} /* anonymous namespace */

/**
 * Searches a list of shaders for a particular function definition
 */
ir_function_signature *
find_matching_signature(const char *name, const exec_list *actual_parameters,
                        glsl_symbol_table *symbols, bool use_builtin)
{
   ir_function *const f = symbols->get_function(name);

   if (f) {
      ir_function_signature *sig =
         f->matching_signature(NULL, actual_parameters, use_builtin);

      if (sig && (sig->is_defined || sig->is_intrinsic)) {
         /* If this function expects to bind to a built-in function and the
          * signature that we found isn't a built-in, keep looking.  Also keep
          * looking if we expect a non-built-in but found a built-in.
          */
         if (use_builtin == sig->is_builtin())
            return sig;
      }
   }

   return NULL;
}

bool
link_function_calls(gl_shader_program *prog, gl_linked_shader *main,
		    gl_shader **shader_list, unsigned num_shaders)
{
   call_link_visitor v(prog, main, shader_list, num_shaders);

   v.run(main->ir);
   return v.success;
}
<|endoftext|>


<commit_before>#include "./point_helpers.h"
#include <string>
#include <ostream>
#include "runtime/length.h"
#include "tree_sitter/runtime.h"

using namespace std;

bool operator==(const TSPoint &left, const TSPoint &right) {
  return left.row == right.row && left.column == right.column;
}

bool operator==(const TSRange &left, const TSRange &right) {
  return (
    left.start_byte == right.start_byte &&
    left.end_byte == right.end_byte &&
    left.start_point == right.start_point &&
    left.end_point == right.end_point
  );
}

bool operator==(const Length &left, const Length &right) {
  return left.bytes == right.bytes && left.extent == right.extent;
}

bool operator<(const TSPoint &left, const TSPoint &right) {
  if (left.row < right.row) return true;
  if (left.row > right.row) return false;

  return left.column < right.column;
}

bool operator>(const TSPoint &left, const TSPoint &right) {
  return right < left;
}

std::ostream &operator<<(std::ostream &stream, const TSPoint &point) {
  return stream << "{" << point.row << ", " << point.column << "}";
}

std::ostream &operator<<(std::ostream &stream, const TSRange &range) {
  return stream << "{" << range.start_point << ", " << range.end_point << "}";
}

ostream &operator<<(ostream &stream, const Length &length) {
  return stream << "{bytes:" << length.bytes << ", extent:" << length.extent << "}";
}

TSPoint extent_for_string(const string &text, size_t end_index) {
  if (end_index > text.size()) end_index = text.size();
  TSPoint result = {0, 0};
  for (size_t i = 0; i < end_index; i++) {
    if (text[i] == '\n') {
      result.row++;
      result.column = 0;
    } else {
      result.column++;
    }
  }
  return result;
}

TSRange range_for_substring(const string &text, const string &substring) {
  size_t start = text.find(substring);
  assert(start != string::npos);
  size_t end = start + substring.size();
  return TSRange {
    extent_for_string(text, start),
    extent_for_string(text, end),
    static_cast<uint32_t>(start),
    static_cast<uint32_t>(end),
  };
};
<commit_msg>Add missing #include in point_helpers<commit_after>#include "./point_helpers.h"
#include <string>
#include <ostream>
#include <cassert>
#include "runtime/length.h"
#include "tree_sitter/runtime.h"

using namespace std;

bool operator==(const TSPoint &left, const TSPoint &right) {
  return left.row == right.row && left.column == right.column;
}

bool operator==(const TSRange &left, const TSRange &right) {
  return (
    left.start_byte == right.start_byte &&
    left.end_byte == right.end_byte &&
    left.start_point == right.start_point &&
    left.end_point == right.end_point
  );
}

bool operator==(const Length &left, const Length &right) {
  return left.bytes == right.bytes && left.extent == right.extent;
}

bool operator<(const TSPoint &left, const TSPoint &right) {
  if (left.row < right.row) return true;
  if (left.row > right.row) return false;

  return left.column < right.column;
}

bool operator>(const TSPoint &left, const TSPoint &right) {
  return right < left;
}

std::ostream &operator<<(std::ostream &stream, const TSPoint &point) {
  return stream << "{" << point.row << ", " << point.column << "}";
}

std::ostream &operator<<(std::ostream &stream, const TSRange &range) {
  return stream << "{" << range.start_point << ", " << range.end_point << "}";
}

ostream &operator<<(ostream &stream, const Length &length) {
  return stream << "{bytes:" << length.bytes << ", extent:" << length.extent << "}";
}

TSPoint extent_for_string(const string &text, size_t end_index) {
  if (end_index > text.size()) end_index = text.size();
  TSPoint result = {0, 0};
  for (size_t i = 0; i < end_index; i++) {
    if (text[i] == '\n') {
      result.row++;
      result.column = 0;
    } else {
      result.column++;
    }
  }
  return result;
}

TSRange range_for_substring(const string &text, const string &substring) {
  size_t start = text.find(substring);
  assert(start != string::npos);
  size_t end = start + substring.size();
  return TSRange {
    extent_for_string(text, start),
    extent_for_string(text, end),
    static_cast<uint32_t>(start),
    static_cast<uint32_t>(end),
  };
};
<|endoftext|>


<commit_before>/* -*- mode: C++; tab-width: 8; indent-tabs-mode: t; c-basic-offset: 8 -*- */
// vim:sts=8:sw=8:ts=8:noet:sr:cino=>s,f0,{0,g0,(0,\:0,t0,+0,=s

/* fflas_fdot.inl
 * Copyright (C) 2005 Clement Pernet
 *
 * Written by Clement Pernet <Clement.Pernet@imag.fr>
 *
 *
 * ========LICENCE========
 * This file is part of the library FFLAS-FFPACK.
 *
 * FFLAS-FFPACK is free software: you can redistribute it and/or modify
 * it under the terms of the  GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 * ========LICENCE========
 *.
 */

#ifndef __FFLASFFPACK_fdot_INL
#define __FFLASFFPACK_fdot_INL

#include "fflas-ffpack/fflas/fflas_helpers.inl"
// Default implementation
// Specializations should be written
// to increase efficiency

namespace FFLAS {

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy ,
	      ModeCategories::DefaultTag& MT)
	{

		typename Field::Element d;
		typename Field::ConstElement_ptr xi = x;
		typename Field::ConstElement_ptr yi = y;
		F.init (d);
		F.assign (d, F.zero);
		for ( ; xi < x+N*incx; xi+=incx, yi+=incy )
			F.axpyin( d, *xi, *yi );
		return d;
	}

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy ,
	      ModeCategories::DelayedTag& MT)
	{
		typedef typename associatedDelayedField<const Field>::field DelayedField;
		typedef typename associatedDelayedField<const Field>::type DelayedField_t;
		typedef typename DelayedField::Element DFElt;
		DelayedField_t delayedF;

		const DFElt MaxStorableValue = limits<typename DelayedField::Element>::max();
		const DFElt AbsMax = std::max(-F.minElement(), F.maxElement());
		const DFElt r = MaxStorableValue / (AbsMax*AbsMax);
		size_t delayedDim = FFLAS::Protected::min_types<DFElt>(r);

		typename Field::Element d;
		F.init (d);
		F.assign (d, F.zero);
		ModeCategories::DefaultTag DM;
		typename Field::ConstElement_ptr xi = x, yi = y;
		size_t i=delayedDim;
		typename Field::Element dp;
		F.init(dp);
		for (; i<N; i+= delayedDim, xi += incx*delayedDim, yi += incy*delayedDim){
			F.assign(dp, fdot (delayedF, delayedDim, xi, incx, yi, incy, DM));
			F.addin(d, dp);
		}
		F.assign (dp,fdot (delayedF, N+delayedDim-i, xi, incx, yi, incy, DM));
		F.addin (d, dp);
		return d;
	}

	template<>
	inline Givaro::DoubleDomain::Element
	fdot( const Givaro::DoubleDomain& , const size_t N,
	      Givaro::DoubleDomain::ConstElement_ptr x, const size_t incx,
	      Givaro::DoubleDomain::ConstElement_ptr y, const size_t incy,
	      ModeCategories::DefaultTag& MT)
	{

#ifdef __FFLASFFPACK_OPENBLAS_NUM_THREADS
		openblas_set_num_threads(__FFLASFFPACK_OPENBLAS_NUM_THREADS);
#endif
		return cblas_ddot( (int)N, x, (int)incx, y, (int)incy );
	}

	template<>
	inline Givaro::FloatDomain::Element
	fdot( const Givaro::FloatDomain& , const size_t N,
	      Givaro::FloatDomain::ConstElement_ptr x, const size_t incx,
	      Givaro::FloatDomain::ConstElement_ptr y, const size_t incy,
	      ModeCategories::DefaultTag& MT)
	{

#ifdef __FFLASFFPACK_OPENBLAS_NUM_THREADS
		openblas_set_num_threads(__FFLASFFPACK_OPENBLAS_NUM_THREADS);
#endif
		return cblas_sdot( (int)N, x, (int)incx, y, (int)incy );
	}

	template<class Field, class T>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy,
	      ModeCategories::ConvertTo<T>& MT)
	{
		typename ModeCategories::DefaultTag mt;
		return fdot (F, N, x, incx, y, incy, mt);
	}

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy,
	      ModeCategories::DefaultBoundedTag& dbt)
	{
            // Nothing special in fdot for Bounded
		typename ModeCategories::DefaultTag dt;
		return fdot (F, N, x, incx, y, incy, dt);
	}

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy,
          const ParSeqHelper::Sequential seq)
	{
		typename ModeTraits<Field>::value mt;
		return fdot (F, N, x, incx, y, incy, mt);
	}

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy)
	{
		return fdot (F, N, x, incx, y, incy, FFLAS::ParSeqHelper::Sequential());
	}

} // FFLAS

#endif // __FFLASFFPACK_fdot_INL
<commit_msg>this init should have stayed here<commit_after>/* -*- mode: C++; tab-width: 8; indent-tabs-mode: t; c-basic-offset: 8 -*- */
// vim:sts=8:sw=8:ts=8:noet:sr:cino=>s,f0,{0,g0,(0,\:0,t0,+0,=s

/* fflas_fdot.inl
 * Copyright (C) 2005 Clement Pernet
 *
 * Written by Clement Pernet <Clement.Pernet@imag.fr>
 *
 *
 * ========LICENCE========
 * This file is part of the library FFLAS-FFPACK.
 *
 * FFLAS-FFPACK is free software: you can redistribute it and/or modify
 * it under the terms of the  GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 * ========LICENCE========
 *.
 */

#ifndef __FFLASFFPACK_fdot_INL
#define __FFLASFFPACK_fdot_INL

#include "fflas-ffpack/fflas/fflas_helpers.inl"
// Default implementation
// Specializations should be written
// to increase efficiency

namespace FFLAS {

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy ,
	      ModeCategories::DefaultTag& MT)
	{

		typename Field::Element d;
		typename Field::ConstElement_ptr xi = x;
		typename Field::ConstElement_ptr yi = y;
		F.init (d);
		F.assign (d, F.zero);
		for ( ; xi < x+N*incx; xi+=incx, yi+=incy )
			F.axpyin( d, *xi, *yi );
		return d;
	}

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy ,
	      ModeCategories::DelayedTag& MT)
	{
		typedef typename associatedDelayedField<const Field>::field DelayedField;
		typedef typename associatedDelayedField<const Field>::type DelayedField_t;
		typedef typename DelayedField::Element DFElt;
		DelayedField_t delayedF;

		const DFElt MaxStorableValue = limits<typename DelayedField::Element>::max();
		const DFElt AbsMax = std::max(-F.minElement(), F.maxElement());
		const DFElt r = MaxStorableValue / (AbsMax*AbsMax);
		size_t delayedDim = FFLAS::Protected::min_types<DFElt>(r);

		typename Field::Element d;
		F.init (d);
		F.assign (d, F.zero);
		ModeCategories::DefaultTag DM;
		typename Field::ConstElement_ptr xi = x, yi = y;
		size_t i=delayedDim;
		typename Field::Element dp;
		for (; i<N; i+= delayedDim, xi += incx*delayedDim, yi += incy*delayedDim){
			F.init(dp, fdot (delayedF, delayedDim, xi, incx, yi, incy, DM));
			F.addin(d, dp);
		}
		F.init (dp,fdot (delayedF, N+delayedDim-i, xi, incx, yi, incy, DM));
		F.addin (d, dp);
		return d;
	}

	template<>
	inline Givaro::DoubleDomain::Element
	fdot( const Givaro::DoubleDomain& , const size_t N,
	      Givaro::DoubleDomain::ConstElement_ptr x, const size_t incx,
	      Givaro::DoubleDomain::ConstElement_ptr y, const size_t incy,
	      ModeCategories::DefaultTag& MT)
	{

#ifdef __FFLASFFPACK_OPENBLAS_NUM_THREADS
		openblas_set_num_threads(__FFLASFFPACK_OPENBLAS_NUM_THREADS);
#endif
		return cblas_ddot( (int)N, x, (int)incx, y, (int)incy );
	}

	template<>
	inline Givaro::FloatDomain::Element
	fdot( const Givaro::FloatDomain& , const size_t N,
	      Givaro::FloatDomain::ConstElement_ptr x, const size_t incx,
	      Givaro::FloatDomain::ConstElement_ptr y, const size_t incy,
	      ModeCategories::DefaultTag& MT)
	{

#ifdef __FFLASFFPACK_OPENBLAS_NUM_THREADS
		openblas_set_num_threads(__FFLASFFPACK_OPENBLAS_NUM_THREADS);
#endif
		return cblas_sdot( (int)N, x, (int)incx, y, (int)incy );
	}

	template<class Field, class T>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy,
	      ModeCategories::ConvertTo<T>& MT)
	{
		typename ModeCategories::DefaultTag mt;
		return fdot (F, N, x, incx, y, incy, mt);
	}

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy,
	      ModeCategories::DefaultBoundedTag& dbt)
	{
            // Nothing special in fdot for Bounded
		typename ModeCategories::DefaultTag dt;
		return fdot (F, N, x, incx, y, incy, dt);
	}

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy,
          const ParSeqHelper::Sequential seq)
	{
		typename ModeTraits<Field>::value mt;
		return fdot (F, N, x, incx, y, incy, mt);
	}

	template<class Field>
	inline typename Field::Element
	fdot( const Field& F, const size_t N,
	      typename Field::ConstElement_ptr x, const size_t incx,
	      typename Field::ConstElement_ptr y, const size_t incy)
	{
		return fdot (F, N, x, incx, y, incy, FFLAS::ParSeqHelper::Sequential());
	}

} // FFLAS

#endif // __FFLASFFPACK_fdot_INL
<|endoftext|>


<commit_before>#include "test_services.hpp"

#define BOOST_TEST_MODULE
#include <boost/test/included/unit_test.hpp>

#include <iterator>

BOOST_AUTO_TEST_CASE( service_without_any_characteristic_results_in_one_attribute )
{
    const auto number = empty_service::number_of_attributes;
    BOOST_CHECK_EQUAL( 1u, number );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );
    BOOST_CHECK_EQUAL( 0x2800, attr.uuid );
}

static const std::uint8_t global_temperature_service_uuid[ 16 ] = { 0x2A, 0xD9, 0x91, 0x11, 0xAB, 0x5B, 0x58, 0xB0, 0x3B, 0x4F, 0x50, 0x44, 0x52, 0x6E, 0x42, 0xF0 };

static void check_service_uuid( const bluetoe::details::attribute_access_arguments& args, std::size_t size = sizeof( global_temperature_service_uuid ) )
{
    BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( global_temperature_service_uuid ), std::begin( global_temperature_service_uuid ) + size, args.buffer, args.buffer + args.buffer_size );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );
    BOOST_REQUIRE( attr.access );

    std::uint8_t buffer[ 16 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::success );
    check_service_uuid( read );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read_buffer_larger )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[ 20 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::success );
    check_service_uuid( read );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read_buffer_larger_with_offset )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[ 20 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 4 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::success );
    BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( global_temperature_service_uuid ) + 4, std::end( global_temperature_service_uuid ),
        &read.buffer[ 0 ], &read.buffer[ read.buffer_size ]);
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read_buffer_larger_with_offset_16 )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[ 20 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 16 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::success );
    BOOST_CHECK_EQUAL( 0, read.buffer_size );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read_buffer_to_small )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[ 15 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::read_truncated );
    check_service_uuid( read, sizeof( buffer ) );
}

BOOST_AUTO_TEST_CASE( write_to_primary_service )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[] = { 1, 2, 3 };
    auto write = bluetoe::details::attribute_access_arguments::write( buffer );
    const auto access_result = attr.access( write, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::write_not_permitted );
}

BOOST_FIXTURE_TEST_CASE( accessing_all_attributes, service_with_3_characteristics )
{
    static constexpr std::size_t expected_number_of_attributes = 7u;
    BOOST_REQUIRE_EQUAL( unsigned( number_of_attributes ), expected_number_of_attributes );

    BOOST_CHECK_EQUAL( 0x2800, attribute_at< 0 >( 0 ).uuid );
    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 1 ).uuid );
    BOOST_CHECK_EQUAL( 0x0001, attribute_at< 0 >( 2 ).uuid );
    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 3 ).uuid );
    BOOST_CHECK_EQUAL( 0x0001, attribute_at< 0 >( 4 ).uuid );
    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 5 ).uuid );
    BOOST_CHECK_EQUAL( 0x0815, attribute_at< 0 >( 6 ).uuid );
}

BOOST_FIXTURE_TEST_CASE( read_by_group_type_response, service_with_3_characteristics )
{
    std::uint8_t    buffer[ 100 ];

    std::uint8_t* const end = read_primary_service_response( std::begin( buffer ), std::end( buffer ), 12, true );

    BOOST_CHECK_EQUAL( end - std::begin( buffer ), 20u );

    static const std::uint8_t expected_result[] =
    {
        0x0c, 0x00,              // Starting Handle
        0x12, 0x00,              // Ending Handle
        0xA9, 0x3C, 0xC7, 0x5B,  // Attribute Value == 128 bit UUID
        0xED, 0x4E, 0x8A, 0xA2,
        0x9F, 0x49, 0xE2, 0x0D,
        0x94, 0x40, 0x8B, 0x8C
    };

    BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( buffer ), end, std::begin( expected_result ), std::end( expected_result ) );
}

BOOST_FIXTURE_TEST_CASE( read_by_group_type_response_buffer_to_small, service_with_3_characteristics )
{
    std::uint8_t    buffer[ 19 ];
    std::uint16_t   index = 1;

    std::uint8_t* const end = read_primary_service_response( std::begin( buffer ), std::end( buffer ), index, true );

    BOOST_CHECK( end == std::begin( buffer ) );
}

BOOST_FIXTURE_TEST_CASE( read_by_group_type_response_for_16bit_uuid, cycling_speed_and_cadence_service )
{
    std::uint8_t    buffer[ 100 ];

    std::uint8_t* const end = read_primary_service_response( std::begin( buffer ), std::end( buffer ), 1, false );

    BOOST_CHECK_EQUAL( end - std::begin( buffer ), 6u );

    static const std::uint8_t expected_result[] =
    {
        0x01, 0x00,   // Starting Handle
        0x05, 0x00,   // Ending Handle
        0x16, 0x18    // Attribute Value == 16 bit UUID
    };

    BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( buffer ), end, std::begin( expected_result ), std::end( expected_result ) );
}

BOOST_FIXTURE_TEST_CASE( primary_service_value_compare_128bit, global_temperature_service )
{
    auto compare = bluetoe::details::attribute_access_arguments::compare_value( &global_temperature_service_uuid[ 0 ], &global_temperature_service_uuid[ sizeof global_temperature_service_uuid ]);
    BOOST_CHECK( attribute_at< 0 >( 0 ).access( compare, 1 ) == bluetoe::details::attribute_access_result::value_equal );
}

BOOST_AUTO_TEST_SUITE( number_of_client_configs )

BOOST_AUTO_TEST_CASE( without_notifications_there_is_no_demand_for_client_configurations )
{
    BOOST_CHECK_EQUAL( 0, int( service_with_3_characteristics::number_of_client_configs ) );
}

char v1, v2, v3;

typedef bluetoe::service<
    bluetoe::service_uuid16< 0x8C8B4 >,
    bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0x8C8B4 >,
        bluetoe::bind_characteristic_value< decltype( v1 ), &v1 >,
        bluetoe::notify
    >,
    bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0x8C8B5 >,
        bluetoe::bind_characteristic_value< decltype( v2 ), &v2 >
    >,
    bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0x8C8B6 >,
        bluetoe::bind_characteristic_value< decltype( v2 ), &v3 >,
        bluetoe::notify
    >
> service_with_2_notifications;

BOOST_AUTO_TEST_CASE( service_with_2_notifications_has_2_client_configurations )
{
    BOOST_CHECK_EQUAL( 2, int( service_with_2_notifications::number_of_client_configs ) );
}

BOOST_AUTO_TEST_SUITE_END()
<commit_msg>search the attribute for the notificated value<commit_after>#include <iostream>
#include "test_services.hpp"

#define BOOST_TEST_MODULE
#include <boost/test/included/unit_test.hpp>

#include <iterator>

BOOST_AUTO_TEST_CASE( service_without_any_characteristic_results_in_one_attribute )
{
    const auto number = empty_service::number_of_attributes;
    BOOST_CHECK_EQUAL( 1u, number );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );
    BOOST_CHECK_EQUAL( 0x2800, attr.uuid );
}

static const std::uint8_t global_temperature_service_uuid[ 16 ] = { 0x2A, 0xD9, 0x91, 0x11, 0xAB, 0x5B, 0x58, 0xB0, 0x3B, 0x4F, 0x50, 0x44, 0x52, 0x6E, 0x42, 0xF0 };

static void check_service_uuid( const bluetoe::details::attribute_access_arguments& args, std::size_t size = sizeof( global_temperature_service_uuid ) )
{
    BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( global_temperature_service_uuid ), std::begin( global_temperature_service_uuid ) + size, args.buffer, args.buffer + args.buffer_size );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );
    BOOST_REQUIRE( attr.access );

    std::uint8_t buffer[ 16 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::success );
    check_service_uuid( read );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read_buffer_larger )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[ 20 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::success );
    check_service_uuid( read );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read_buffer_larger_with_offset )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[ 20 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 4 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::success );
    BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( global_temperature_service_uuid ) + 4, std::end( global_temperature_service_uuid ),
        &read.buffer[ 0 ], &read.buffer[ read.buffer_size ]);
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read_buffer_larger_with_offset_16 )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[ 20 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 16 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::success );
    BOOST_CHECK_EQUAL( 0, read.buffer_size );
}

BOOST_AUTO_TEST_CASE( first_attribute_is_the_primary_service_and_can_be_read_buffer_to_small )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[ 15 ];
    auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );
    const auto access_result = attr.access( read, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::read_truncated );
    check_service_uuid( read, sizeof( buffer ) );
}

BOOST_AUTO_TEST_CASE( write_to_primary_service )
{
    const auto attr = empty_service::attribute_at< 0 >( 0 );

    std::uint8_t buffer[] = { 1, 2, 3 };
    auto write = bluetoe::details::attribute_access_arguments::write( buffer );
    const auto access_result = attr.access( write, 1 );

    BOOST_CHECK( access_result == bluetoe::details::attribute_access_result::write_not_permitted );
}

BOOST_FIXTURE_TEST_CASE( accessing_all_attributes, service_with_3_characteristics )
{
    static constexpr std::size_t expected_number_of_attributes = 7u;
    BOOST_REQUIRE_EQUAL( unsigned( number_of_attributes ), expected_number_of_attributes );

    BOOST_CHECK_EQUAL( 0x2800, attribute_at< 0 >( 0 ).uuid );
    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 1 ).uuid );
    BOOST_CHECK_EQUAL( 0x0001, attribute_at< 0 >( 2 ).uuid );
    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 3 ).uuid );
    BOOST_CHECK_EQUAL( 0x0001, attribute_at< 0 >( 4 ).uuid );
    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 5 ).uuid );
    BOOST_CHECK_EQUAL( 0x0815, attribute_at< 0 >( 6 ).uuid );
}

BOOST_FIXTURE_TEST_CASE( read_by_group_type_response, service_with_3_characteristics )
{
    std::uint8_t    buffer[ 100 ];

    std::uint8_t* const end = read_primary_service_response( std::begin( buffer ), std::end( buffer ), 12, true );

    BOOST_CHECK_EQUAL( end - std::begin( buffer ), 20u );

    static const std::uint8_t expected_result[] =
    {
        0x0c, 0x00,              // Starting Handle
        0x12, 0x00,              // Ending Handle
        0xA9, 0x3C, 0xC7, 0x5B,  // Attribute Value == 128 bit UUID
        0xED, 0x4E, 0x8A, 0xA2,
        0x9F, 0x49, 0xE2, 0x0D,
        0x94, 0x40, 0x8B, 0x8C
    };

    BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( buffer ), end, std::begin( expected_result ), std::end( expected_result ) );
}

BOOST_FIXTURE_TEST_CASE( read_by_group_type_response_buffer_to_small, service_with_3_characteristics )
{
    std::uint8_t    buffer[ 19 ];
    std::uint16_t   index = 1;

    std::uint8_t* const end = read_primary_service_response( std::begin( buffer ), std::end( buffer ), index, true );

    BOOST_CHECK( end == std::begin( buffer ) );
}

BOOST_FIXTURE_TEST_CASE( read_by_group_type_response_for_16bit_uuid, cycling_speed_and_cadence_service )
{
    std::uint8_t    buffer[ 100 ];

    std::uint8_t* const end = read_primary_service_response( std::begin( buffer ), std::end( buffer ), 1, false );

    BOOST_CHECK_EQUAL( end - std::begin( buffer ), 6u );

    static const std::uint8_t expected_result[] =
    {
        0x01, 0x00,   // Starting Handle
        0x05, 0x00,   // Ending Handle
        0x16, 0x18    // Attribute Value == 16 bit UUID
    };

    BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( buffer ), end, std::begin( expected_result ), std::end( expected_result ) );
}

BOOST_FIXTURE_TEST_CASE( primary_service_value_compare_128bit, global_temperature_service )
{
    auto compare = bluetoe::details::attribute_access_arguments::compare_value( &global_temperature_service_uuid[ 0 ], &global_temperature_service_uuid[ sizeof global_temperature_service_uuid ]);
    BOOST_CHECK( attribute_at< 0 >( 0 ).access( compare, 1 ) == bluetoe::details::attribute_access_result::value_equal );
}

BOOST_AUTO_TEST_SUITE( number_of_client_configs )

BOOST_AUTO_TEST_CASE( without_notifications_there_is_no_demand_for_client_configurations )
{
    BOOST_CHECK_EQUAL( 0, int( service_with_3_characteristics::number_of_client_configs ) );
}

char v1, v2, v3;

typedef bluetoe::service<
    bluetoe::service_uuid16< 0x8C8B >,
    bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0x8C8B >,
        bluetoe::bind_characteristic_value< decltype( v1 ), &v1 >,
        bluetoe::notify
    >,
    bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0x8C8C >,
        bluetoe::bind_characteristic_value< decltype( v2 ), &v2 >
    >,
    bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0x8C8D >,
        bluetoe::bind_characteristic_value< decltype( v3 ), &v3 >,
        bluetoe::notify
    >
> service_with_2_notifications;

BOOST_AUTO_TEST_CASE( service_with_2_notifications_has_2_client_configurations )
{
    BOOST_CHECK_EQUAL( 2, int( service_with_2_notifications::number_of_client_configs ) );
}

// just to be sure
BOOST_FIXTURE_TEST_CASE( check_service_with_2_notifications_attribute_layout, service_with_2_notifications )
{
    BOOST_CHECK_EQUAL( 0x2800, attribute_at< 0 >( 0 ).uuid );

    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 1 ).uuid );
    BOOST_CHECK_EQUAL( 0x8C8B, attribute_at< 0 >( 2 ).uuid );
    BOOST_CHECK_EQUAL( 0x2902, attribute_at< 0 >( 3 ).uuid );

    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 4 ).uuid );
    BOOST_CHECK_EQUAL( 0x8C8C, attribute_at< 0 >( 5 ).uuid );

    BOOST_CHECK_EQUAL( 0x2803, attribute_at< 0 >( 6 ).uuid );
    BOOST_CHECK_EQUAL( 0x8C8D, attribute_at< 0 >( 7 ).uuid );
    BOOST_CHECK_EQUAL( 0x2902, attribute_at< 0 >( 8 ).uuid );
}

BOOST_FIXTURE_TEST_CASE( notification_value_not_found, service_with_2_notifications )
{
    BOOST_CHECK_EQUAL( find_characteristic_value_declaration< 1 >( nullptr ).first, 0 );
}

BOOST_FIXTURE_TEST_CASE( notification_value_found, service_with_2_notifications )
{
    std::pair< std::uint16_t, bluetoe::details::attribute > result1 = find_characteristic_value_declaration< 1 >( &v1 );
    std::pair< std::uint16_t, bluetoe::details::attribute > result2 = find_characteristic_value_declaration< 1 >( &v3 );

    BOOST_CHECK_EQUAL( result1.first, 3 );
    BOOST_CHECK_EQUAL( result2.first, 8 );
}

BOOST_AUTO_TEST_SUITE_END()
<|endoftext|>


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

#include "LoadMon.hpp"

// if free stack space falls below this, print a warning
#if defined(CONFIG_ARMV7M_STACKCHECK)
static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 100;
#else
static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 300;
#endif

static constexpr unsigned FDS_LOW_WARNING_THRESHOLD = 2; ///< if free file descriptors fall below this, print a warning

using namespace time_literals;

namespace load_mon
{

LoadMon::LoadMon() :
	ModuleParams(nullptr),
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
}

LoadMon::~LoadMon()
{
	ScheduleClear();
	perf_free(_cycle_perf);
}

int LoadMon::task_spawn(int argc, char *argv[])
{
	LoadMon *obj = new LoadMon();

	if (!obj) {
		PX4_ERR("alloc failed");
		return -1;
	}

	_object.store(obj);
	_task_id = task_id_is_work_queue;

	/* Schedule a cycle to start things. */
	obj->start();

	return 0;
}

void LoadMon::start()
{
	ScheduleOnInterval(300_ms);
}

void LoadMon::Run()
{
	perf_begin(_cycle_perf);

	cpuload();

	if (_param_sys_stck_en.get()) {
		stack_usage();
	}

	if (should_exit()) {
		ScheduleClear();
		exit_and_cleanup();
	}

	perf_end(_cycle_perf);
}

void LoadMon::cpuload()
{
	if (_last_idle_time == 0) {
		// Just get the time in the first iteration */
		_last_idle_time = system_load.tasks[0].total_runtime;
		_last_idle_time_sample = hrt_absolute_time();
		return;
	}

	irqstate_t irqstate = enter_critical_section();
	const hrt_abstime now = hrt_absolute_time();
	const hrt_abstime total_runtime = system_load.tasks[0].total_runtime;
	leave_critical_section(irqstate);

	// compute system load
	const float interval = now - _last_idle_time_sample;
	const float interval_idletime = total_runtime - _last_idle_time;

	// get ram usage
	struct mallinfo mem = mallinfo();
	float ram_usage = (float)mem.uordblks / mem.arena;

	cpuload_s cpuload{};
	cpuload.load = 1.f - interval_idletime / interval;
	cpuload.ram_usage = ram_usage;
	cpuload.timestamp = hrt_absolute_time();

	_cpuload_pub.publish(cpuload);

	// store for next iteration
	_last_idle_time = total_runtime;
	_last_idle_time_sample = now;
}

void LoadMon::stack_usage()
{
	unsigned stack_free = 0;
	unsigned fds_free = FDS_LOW_WARNING_THRESHOLD + 1;

	bool checked_task = false;

	task_stack_info_s task_stack_info{};
	static_assert(sizeof(task_stack_info.task_name) == CONFIG_TASK_NAME_SIZE,
		      "task_stack_info.task_name must match NuttX CONFIG_TASK_NAME_SIZE");

	sched_lock();

	if (system_load.tasks[_stack_task_index].valid && (system_load.tasks[_stack_task_index].tcb->pid > 0)) {

		stack_free = up_check_tcbstack_remain(system_load.tasks[_stack_task_index].tcb);

		strncpy((char *)task_stack_info.task_name, system_load.tasks[_stack_task_index].tcb->name, CONFIG_TASK_NAME_SIZE - 1);
		task_stack_info.task_name[CONFIG_TASK_NAME_SIZE - 1] = '\0';

		checked_task = true;

#if CONFIG_NFILE_DESCRIPTORS > 0
		FAR struct task_group_s *group = system_load.tasks[_stack_task_index].tcb->group;

		unsigned tcb_num_used_fds = 0;

		if (group) {
			for (int fd_index = 0; fd_index < CONFIG_NFILE_DESCRIPTORS; ++fd_index) {
				if (group->tg_filelist.fl_files[fd_index].f_inode) {
					++tcb_num_used_fds;
				}
			}

			fds_free = CONFIG_NFILE_DESCRIPTORS - tcb_num_used_fds;
		}

#endif // CONFIG_NFILE_DESCRIPTORS
	}

	sched_unlock();

	if (checked_task) {
		task_stack_info.stack_free = stack_free;
		task_stack_info.timestamp = hrt_absolute_time();

		_task_stack_info_pub.publish(task_stack_info);

		// Found task low on stack, report and exit. Continue here in next cycle.
		if (stack_free < STACK_LOW_WARNING_THRESHOLD) {
			PX4_WARN("%s low on stack! (%i bytes left)", task_stack_info.task_name, stack_free);
		}

		// Found task low on file descriptors, report and exit. Continue here in next cycle.
		if (fds_free < FDS_LOW_WARNING_THRESHOLD) {
			PX4_WARN("%s low on FDs! (%i FDs left)", task_stack_info.task_name, fds_free);
		}
	}

	// Continue after last checked task next cycle
	_stack_task_index = (_stack_task_index + 1) % CONFIG_MAX_TASKS;
}

int LoadMon::print_usage(const char *reason)
{
	if (reason) {
		PX4_ERR("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
Background process running periodically with 1 Hz on the low priority work queue to calculate the CPU load and RAM
usage and publish the `cpuload` topic.

On NuttX it also checks the stack usage of each process and if it falls below 300 bytes, a warning is output,
which will also appear in the log file.
)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("load_mon", "system");
	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
	return 0;
}

extern "C" __EXPORT int load_mon_main(int argc, char *argv[])
{
	return LoadMon::main(argc, argv);
}

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

#include "LoadMon.hpp"

// if free stack space falls below this, print a warning
#if defined(CONFIG_ARMV7M_STACKCHECK)
static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 100;
#else
static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 300;
#endif

static constexpr unsigned FDS_LOW_WARNING_THRESHOLD = 2; ///< if free file descriptors fall below this, print a warning

using namespace time_literals;

namespace load_mon
{

LoadMon::LoadMon() :
	ModuleParams(nullptr),
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
}

LoadMon::~LoadMon()
{
	ScheduleClear();
	perf_free(_cycle_perf);
}

int LoadMon::task_spawn(int argc, char *argv[])
{
	LoadMon *obj = new LoadMon();

	if (!obj) {
		PX4_ERR("alloc failed");
		return -1;
	}

	_object.store(obj);
	_task_id = task_id_is_work_queue;

	/* Schedule a cycle to start things. */
	obj->start();

	return 0;
}

void LoadMon::start()
{
	ScheduleOnInterval(300_ms);
}

void LoadMon::Run()
{
	perf_begin(_cycle_perf);

	cpuload();

	if (_param_sys_stck_en.get()) {
		stack_usage();
	}

	if (should_exit()) {
		ScheduleClear();
		exit_and_cleanup();
	}

	perf_end(_cycle_perf);
}

void LoadMon::cpuload()
{
	if (_last_idle_time == 0) {
		// Just get the time in the first iteration */
		_last_idle_time = system_load.tasks[0].total_runtime;
		_last_idle_time_sample = hrt_absolute_time();
		return;
	}

	irqstate_t irqstate = enter_critical_section();
	const hrt_abstime now = hrt_absolute_time();
	const hrt_abstime total_runtime = system_load.tasks[0].total_runtime;
	leave_critical_section(irqstate);

	// compute system load
	const float interval = now - _last_idle_time_sample;
	const float interval_idletime = total_runtime - _last_idle_time;

	// get ram usage
	struct mallinfo mem = mallinfo();
	float ram_usage = (float)mem.uordblks / mem.arena;

	cpuload_s cpuload{};
	cpuload.load = 1.f - interval_idletime / interval;
	cpuload.ram_usage = ram_usage;
	cpuload.timestamp = hrt_absolute_time();

	_cpuload_pub.publish(cpuload);

	// store for next iteration
	_last_idle_time = total_runtime;
	_last_idle_time_sample = now;
}

void LoadMon::stack_usage()
{
	unsigned stack_free = 0;
	unsigned fds_free = FDS_LOW_WARNING_THRESHOLD + 1;

	bool checked_task = false;

	task_stack_info_s task_stack_info{};
	static_assert(sizeof(task_stack_info.task_name) == CONFIG_TASK_NAME_SIZE,
		      "task_stack_info.task_name must match NuttX CONFIG_TASK_NAME_SIZE");

	sched_lock();

	if (system_load.tasks[_stack_task_index].valid && (system_load.tasks[_stack_task_index].tcb->pid > 0)) {

		stack_free = up_check_tcbstack_remain(system_load.tasks[_stack_task_index].tcb);

		strncpy((char *)task_stack_info.task_name, system_load.tasks[_stack_task_index].tcb->name, CONFIG_TASK_NAME_SIZE - 1);
		task_stack_info.task_name[CONFIG_TASK_NAME_SIZE - 1] = '\0';

		checked_task = true;

#if CONFIG_NFILE_DESCRIPTORS > 0
		FAR struct task_group_s *group = system_load.tasks[_stack_task_index].tcb->group;

		unsigned tcb_num_used_fds = 0;

		if (group) {
			for (int fd_index = 0; fd_index < CONFIG_NFILE_DESCRIPTORS; ++fd_index) {
				if (group->tg_filelist.fl_files[fd_index].f_inode) {
					++tcb_num_used_fds;
				}
			}

			fds_free = CONFIG_NFILE_DESCRIPTORS - tcb_num_used_fds;
		}

#endif // CONFIG_NFILE_DESCRIPTORS
	}

	sched_unlock();

	if (checked_task) {
		task_stack_info.stack_free = stack_free;
		task_stack_info.timestamp = hrt_absolute_time();

		_task_stack_info_pub.publish(task_stack_info);

		// Found task low on stack, report and exit. Continue here in next cycle.
		if (stack_free < STACK_LOW_WARNING_THRESHOLD) {
			PX4_WARN("%s low on stack! (%i bytes left)", task_stack_info.task_name, stack_free);
		}

		// Found task low on file descriptors, report and exit. Continue here in next cycle.
		if (fds_free < FDS_LOW_WARNING_THRESHOLD) {
			PX4_WARN("%s low on FDs! (%i FDs left)", task_stack_info.task_name, fds_free);
		}
	}

	// Continue after last checked task next cycle
	_stack_task_index = (_stack_task_index + 1) % CONFIG_MAX_TASKS;
}

int LoadMon::print_usage(const char *reason)
{
	if (reason) {
		PX4_ERR("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
Background process running periodically on the low priority work queue to calculate the CPU load and RAM
usage and publish the `cpuload` topic.

On NuttX it also checks the stack usage of each process and if it falls below 300 bytes, a warning is output,
which will also appear in the log file.
)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("load_mon", "system");
	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
	return 0;
}

extern "C" __EXPORT int load_mon_main(int argc, char *argv[])
{
	return LoadMon::main(argc, argv);
}

} // namespace load_mon
<|endoftext|>