<commit_before>//////////////////////////////////////////////////////////////////////////
//
//  Copyright (c) 2008, Image Engine Design Inc. All rights reserved.
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions are
//  met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of Image Engine Design nor the names of any
//       other contributors to this software 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 "IECore/CompoundObject.h"
#include "IECore/MeshPrimitive.h"
#include "IECore/TriangulateOp.h"
#include "IECore/DespatchTypedData.h"
#include "IECore/TriangleAlgo.h"
#include "IECore/Exception.h"
#include "IECore/CompoundParameter.h"

using namespace IECore;

TriangulateOp::TriangulateOp() : MeshPrimitiveOp( staticTypeName(), "A MeshPrimitiveOp to triangulate a mesh" )
{
	m_toleranceParameter = new FloatParameter(
		"tolerance",
		"The floating point tolerance to use for various operations, such as determining planarity of faces",
		1.e-6f,
		0.0f
	);

	m_throwExceptionsParameter = new BoolParameter(
		"throwExceptions",
		"When enabled, exceptions are thrown when invalid geometry is encountered (e.g. non-planar or concave faces).",
		true
	);

	parameters()->addParameter( m_toleranceParameter );
	parameters()->addParameter( m_throwExceptionsParameter );
}

TriangulateOp::~TriangulateOp()
{
}

FloatParameterPtr TriangulateOp::toleranceParameter()
{
	return m_toleranceParameter;
}

ConstFloatParameterPtr TriangulateOp::toleranceParameter() const
{
	return m_toleranceParameter;
}

BoolParameterPtr TriangulateOp::throwExceptionsParameter()
{
	return m_throwExceptionsParameter;
}

ConstBoolParameterPtr TriangulateOp::throwExceptionsParameter() const
{
	return m_throwExceptionsParameter;
}

/// A functor for use with despatchTypedData, which copies elements from another vector, as specified by an array of indices into that data
struct TriangleDataRemap
{
	typedef size_t ReturnType;

	TriangleDataRemap( const std::vector<int> &indices ) : m_indices( indices )
	{
	}

	ConstDataPtr m_other;
	const std::vector<int> &m_indices;

	template<typename T>
	size_t operator() ( typename T::Ptr data )
	{
		assert( data );
		typename T::ConstPtr otherData = runTimeCast<const T>( m_other );
		assert( otherData );

		data->writable().clear();
		data->writable().reserve( m_indices.size() );

		for ( std::vector<int>::const_iterator it = m_indices.begin(); it != m_indices.end(); ++it )
		{
			data->writable().push_back( otherData->readable()[ *it ] );
		}

		assert( data->readable().size() == m_indices.size() );

		return data->readable().size();
	}
};

void TriangulateOp::modifyTypedPrimitive( MeshPrimitivePtr mesh, ConstCompoundObjectPtr operands )
{

	if (! mesh->arePrimitiveVariablesValid() )
	{
		throw InvalidArgumentException( "Mesh with invalid primitive variables given to TriangulateOp");
	}

	bool alreadyTriangulated = true;
	ConstIntVectorDataPtr verticesPerFace = mesh->verticesPerFace();
	IntVectorData::ValueType::const_iterator it = verticesPerFace->readable().begin();
	while ( it != verticesPerFace->readable().end() && alreadyTriangulated )
	{
		if (*it++ != 3)
		{
			alreadyTriangulated = false;
		}
	}

	if ( alreadyTriangulated )
	{
		return;
	}

	const float tolerance = toleranceParameter()->getNumericValue();
	bool throwExceptions = boost::static_pointer_cast<const BoolData>(throwExceptionsParameter()->getValue())->readable();

	ConstV3fVectorDataPtr p = 0;

	PrimitiveVariableMap::const_iterator pvIt = mesh->variables.find("P");
	if (pvIt != mesh->variables.end())
	{
		const DataPtr &verticesData = pvIt->second.data;
		assert( verticesData );

		if (runTimeCast<V3fVectorData>(verticesData))
		{
			p = runTimeCast<V3fVectorData>(verticesData);
		}
		else
		{
			std::string err = "MeshPrimitive has \"P\" of unsupported type \"";
			err += verticesData->typeName();
			err += "\" in TriangulateOp";
			throw InvalidArgumentException( err );
		}
	}
	else
	{
		throw InvalidArgumentException("MeshPrimitive has no \"P\" data in TriangulateOp");
	}
	assert( p );

	ConstIntVectorDataPtr vertexIds = mesh->vertexIds();

	IntVectorDataPtr newVertexIds = new IntVectorData();
	newVertexIds->writable().reserve( vertexIds->readable().size() );

	IntVectorDataPtr newVerticesPerFace = new IntVectorData();
	newVerticesPerFace->writable().reserve( verticesPerFace->readable().size() );

	std::vector<int> faceVaryingIndices;
	int faceVertexIdStart = 0;
	for ( IntVectorData::ValueType::const_iterator it = verticesPerFace->readable().begin(); it != verticesPerFace->readable().end(); ++it )
	{
		int numFaceVerts = *it;

		if ( numFaceVerts > 3 )
		{
			/// For the time being, just do a simple triangle fan.

			const int i0 = faceVertexIdStart + 0;
			const int v0 = vertexIds->readable()[ i0 ];

			int i1 = faceVertexIdStart + 1;
			int i2 = faceVertexIdStart + 2;
			int v1 = vertexIds->readable()[ i1 ];
			int v2 = vertexIds->readable()[ i2 ];

			const Imath::V3f firstTriangleNormal = triangleNormal( p->readable()[ v0 ], p->readable()[ v1 ], p->readable()[ v2 ] );

			if (throwExceptions)
			{
				/// Convexivity test - for each edge, all other vertices must be on the same "side" of it
				for (int i = 0; i < numFaceVerts - 1; i++)
				{
					const int edgeStartIndex = faceVertexIdStart + i + 0;
					const int edgeStart = vertexIds->readable()[ edgeStartIndex ];

					const int edgeEndIndex = faceVertexIdStart + i + 1;
					const int edgeEnd = vertexIds->readable()[ edgeEndIndex ];

					const Imath::V3f edge = p->readable()[ edgeEnd ] -  p->readable()[ edgeStart ];
					const float edgeLength = edge.length();

					if (edgeLength > tolerance)
					{
						const Imath::V3f edgeDirection = edge / edgeLength;

						/// Construct a plane whose normal is perpendicular to both the edge and the polygon's normal
						const Imath::V3f planeNormal = edgeDirection.cross( firstTriangleNormal );
						const float planeConstant = planeNormal.dot( p->readable()[ edgeStart ] );

						int sign = 0;
						bool first = true;
						for (int j = 0; j < numFaceVerts; j++)
						{
							const int testVertexIndex = faceVertexIdStart + j;
							const int testVertex = vertexIds->readable()[ testVertexIndex ];

							if ( testVertex != edgeStart && testVertex != edgeEnd )
							{
								float signedDistance = planeNormal.dot( p->readable()[ testVertex ] ) - planeConstant;

								if ( fabs(signedDistance) > tolerance)
								{
									int thisSign = 1;
									if ( signedDistance < 0.0 )
									{
										thisSign = -1;
									}
									if (first)
									{
										sign = thisSign;
										first = false;
									}
									else if ( thisSign != sign )
									{
										assert( sign != 0 );
										throw InvalidArgumentException("TriangulateOp cannot deal with concave polygons");
									}
								}
							}
						}
					}
				}
			}

			for (int i = 1; i < numFaceVerts - 1; i++)
			{
				i1 = faceVertexIdStart + ( (i + 0) % numFaceVerts );
				i2 = faceVertexIdStart + ( (i + 1) % numFaceVerts );
				v1 = vertexIds->readable()[ i1 ];
				v2 = vertexIds->readable()[ i2 ];

				if ( throwExceptions && fabs( triangleNormal( p->readable()[ v0 ], p->readable()[ v1 ], p->readable()[ v2 ] ).dot( firstTriangleNormal ) - 1.0 ) > tolerance )
				{
					throw InvalidArgumentException("TriangulateOp cannot deal with non-planar polygons");
				}

				/// Create a new triangle
				newVerticesPerFace->writable().push_back( 3 );

				/// Triangulate the vertices
				newVertexIds->writable().push_back( v0 );
				newVertexIds->writable().push_back( v1 );
				newVertexIds->writable().push_back( v2 );

				/// Store the indices required to rebuild the facevarying primvars
				faceVaryingIndices.push_back( i0 );
				faceVaryingIndices.push_back( i1 );
				faceVaryingIndices.push_back( i2 );
			}
		}
		else
		{
			assert( numFaceVerts == 3 );

			int i0 = faceVertexIdStart + 0;
			int i1 = faceVertexIdStart + 1;
			int i2 = faceVertexIdStart + 2;

			newVerticesPerFace->writable().push_back( 3 );

			/// Copy across the vertexId data
			newVertexIds->writable().push_back( vertexIds->readable()[ i0 ] );
			newVertexIds->writable().push_back( vertexIds->readable()[ i1 ] );
			newVertexIds->writable().push_back( vertexIds->readable()[ i2 ] );

			/// Store the indices required to rebuild the facevarying primvars
			faceVaryingIndices.push_back( i0 );
			faceVaryingIndices.push_back( i1 );
			faceVaryingIndices.push_back( i2 );
		}

		faceVertexIdStart += numFaceVerts;
	}

	mesh->setTopology( newVerticesPerFace, newVertexIds );

	/// Rebuild all the facevarying primvars, using the list of indices into the old data we created above.
	assert( faceVaryingIndices.size() == newVertexIds->readable().size() );
	TriangleDataRemap func( faceVaryingIndices );
	for ( PrimitiveVariableMap::iterator it = mesh->variables.begin(); it != mesh->variables.end(); ++it )
	{
		if ( it->second.interpolation == PrimitiveVariable::FaceVarying )
		{
 			assert( it->second.data );
			func.m_other = it->second.data;
			DataPtr data = it->second.data->copy();

			size_t primVarSize = despatchTypedData<TriangleDataRemap, TypeTraits::IsVectorTypedData>( data, func );
			assert( primVarSize == faceVaryingIndices.size() );
			(void)primVarSize;

			it->second.data = data;
		}
	}

	assert( mesh->arePrimitiveVariablesValid() );
}
<commit_msg>Added todo<commit_after>//////////////////////////////////////////////////////////////////////////
//
//  Copyright (c) 2008, Image Engine Design Inc. All rights reserved.
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions are
//  met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of Image Engine Design nor the names of any
//       other contributors to this software 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 "IECore/CompoundObject.h"
#include "IECore/MeshPrimitive.h"
#include "IECore/TriangulateOp.h"
#include "IECore/DespatchTypedData.h"
#include "IECore/TriangleAlgo.h"
#include "IECore/Exception.h"
#include "IECore/CompoundParameter.h"

using namespace IECore;

TriangulateOp::TriangulateOp() : MeshPrimitiveOp( staticTypeName(), "A MeshPrimitiveOp to triangulate a mesh" )
{
	m_toleranceParameter = new FloatParameter(
		"tolerance",
		"The floating point tolerance to use for various operations, such as determining planarity of faces",
		1.e-6f,
		0.0f
	);

	m_throwExceptionsParameter = new BoolParameter(
		"throwExceptions",
		"When enabled, exceptions are thrown when invalid geometry is encountered (e.g. non-planar or concave faces).",
		true
	);

	parameters()->addParameter( m_toleranceParameter );
	parameters()->addParameter( m_throwExceptionsParameter );
}

TriangulateOp::~TriangulateOp()
{
}

FloatParameterPtr TriangulateOp::toleranceParameter()
{
	return m_toleranceParameter;
}

ConstFloatParameterPtr TriangulateOp::toleranceParameter() const
{
	return m_toleranceParameter;
}

BoolParameterPtr TriangulateOp::throwExceptionsParameter()
{
	return m_throwExceptionsParameter;
}

ConstBoolParameterPtr TriangulateOp::throwExceptionsParameter() const
{
	return m_throwExceptionsParameter;
}

/// A functor for use with despatchTypedData, which copies elements from another vector, as specified by an array of indices into that data
struct TriangleDataRemap
{
	typedef size_t ReturnType;

	TriangleDataRemap( const std::vector<int> &indices ) : m_indices( indices )
	{
	}

	ConstDataPtr m_other;
	const std::vector<int> &m_indices;

	template<typename T>
	size_t operator() ( typename T::Ptr data )
	{
		assert( data );
		typename T::ConstPtr otherData = runTimeCast<const T>( m_other );
		assert( otherData );

		data->writable().clear();
		data->writable().reserve( m_indices.size() );

		for ( std::vector<int>::const_iterator it = m_indices.begin(); it != m_indices.end(); ++it )
		{
			data->writable().push_back( otherData->readable()[ *it ] );
		}

		assert( data->readable().size() == m_indices.size() );

		return data->readable().size();
	}
};

void TriangulateOp::modifyTypedPrimitive( MeshPrimitivePtr mesh, ConstCompoundObjectPtr operands )
{

	if (! mesh->arePrimitiveVariablesValid() )
	{
		throw InvalidArgumentException( "Mesh with invalid primitive variables given to TriangulateOp");
	}

	bool alreadyTriangulated = true;
	ConstIntVectorDataPtr verticesPerFace = mesh->verticesPerFace();
	IntVectorData::ValueType::const_iterator it = verticesPerFace->readable().begin();
	while ( it != verticesPerFace->readable().end() && alreadyTriangulated )
	{
		if (*it++ != 3)
		{
			alreadyTriangulated = false;
		}
	}

	if ( alreadyTriangulated )
	{
		return;
	}

	const float tolerance = toleranceParameter()->getNumericValue();
	bool throwExceptions = boost::static_pointer_cast<const BoolData>(throwExceptionsParameter()->getValue())->readable();

	ConstV3fVectorDataPtr p = 0;

	PrimitiveVariableMap::const_iterator pvIt = mesh->variables.find("P");
	if (pvIt != mesh->variables.end())
	{
		const DataPtr &verticesData = pvIt->second.data;
		assert( verticesData );

		/// \todo Use depatchTypedData
		if (runTimeCast<V3fVectorData>(verticesData))
		{
			p = runTimeCast<V3fVectorData>(verticesData);
		}
		else
		{
			std::string err = "MeshPrimitive has \"P\" of unsupported type \"";
			err += verticesData->typeName();
			err += "\" in TriangulateOp";
			throw InvalidArgumentException( err );
		}
	}
	else
	{
		throw InvalidArgumentException("MeshPrimitive has no \"P\" data in TriangulateOp");
	}
	assert( p );

	ConstIntVectorDataPtr vertexIds = mesh->vertexIds();

	IntVectorDataPtr newVertexIds = new IntVectorData();
	newVertexIds->writable().reserve( vertexIds->readable().size() );

	IntVectorDataPtr newVerticesPerFace = new IntVectorData();
	newVerticesPerFace->writable().reserve( verticesPerFace->readable().size() );

	std::vector<int> faceVaryingIndices;
	int faceVertexIdStart = 0;
	for ( IntVectorData::ValueType::const_iterator it = verticesPerFace->readable().begin(); it != verticesPerFace->readable().end(); ++it )
	{
		int numFaceVerts = *it;

		if ( numFaceVerts > 3 )
		{
			/// For the time being, just do a simple triangle fan.

			const int i0 = faceVertexIdStart + 0;
			const int v0 = vertexIds->readable()[ i0 ];

			int i1 = faceVertexIdStart + 1;
			int i2 = faceVertexIdStart + 2;
			int v1 = vertexIds->readable()[ i1 ];
			int v2 = vertexIds->readable()[ i2 ];

			const Imath::V3f firstTriangleNormal = triangleNormal( p->readable()[ v0 ], p->readable()[ v1 ], p->readable()[ v2 ] );

			if (throwExceptions)
			{
				/// Convexivity test - for each edge, all other vertices must be on the same "side" of it
				for (int i = 0; i < numFaceVerts - 1; i++)
				{
					const int edgeStartIndex = faceVertexIdStart + i + 0;
					const int edgeStart = vertexIds->readable()[ edgeStartIndex ];

					const int edgeEndIndex = faceVertexIdStart + i + 1;
					const int edgeEnd = vertexIds->readable()[ edgeEndIndex ];

					const Imath::V3f edge = p->readable()[ edgeEnd ] -  p->readable()[ edgeStart ];
					const float edgeLength = edge.length();

					if (edgeLength > tolerance)
					{
						const Imath::V3f edgeDirection = edge / edgeLength;

						/// Construct a plane whose normal is perpendicular to both the edge and the polygon's normal
						const Imath::V3f planeNormal = edgeDirection.cross( firstTriangleNormal );
						const float planeConstant = planeNormal.dot( p->readable()[ edgeStart ] );

						int sign = 0;
						bool first = true;
						for (int j = 0; j < numFaceVerts; j++)
						{
							const int testVertexIndex = faceVertexIdStart + j;
							const int testVertex = vertexIds->readable()[ testVertexIndex ];

							if ( testVertex != edgeStart && testVertex != edgeEnd )
							{
								float signedDistance = planeNormal.dot( p->readable()[ testVertex ] ) - planeConstant;

								if ( fabs(signedDistance) > tolerance)
								{
									int thisSign = 1;
									if ( signedDistance < 0.0 )
									{
										thisSign = -1;
									}
									if (first)
									{
										sign = thisSign;
										first = false;
									}
									else if ( thisSign != sign )
									{
										assert( sign != 0 );
										throw InvalidArgumentException("TriangulateOp cannot deal with concave polygons");
									}
								}
							}
						}
					}
				}
			}

			for (int i = 1; i < numFaceVerts - 1; i++)
			{
				i1 = faceVertexIdStart + ( (i + 0) % numFaceVerts );
				i2 = faceVertexIdStart + ( (i + 1) % numFaceVerts );
				v1 = vertexIds->readable()[ i1 ];
				v2 = vertexIds->readable()[ i2 ];

				if ( throwExceptions && fabs( triangleNormal( p->readable()[ v0 ], p->readable()[ v1 ], p->readable()[ v2 ] ).dot( firstTriangleNormal ) - 1.0 ) > tolerance )
				{
					throw InvalidArgumentException("TriangulateOp cannot deal with non-planar polygons");
				}

				/// Create a new triangle
				newVerticesPerFace->writable().push_back( 3 );

				/// Triangulate the vertices
				newVertexIds->writable().push_back( v0 );
				newVertexIds->writable().push_back( v1 );
				newVertexIds->writable().push_back( v2 );

				/// Store the indices required to rebuild the facevarying primvars
				faceVaryingIndices.push_back( i0 );
				faceVaryingIndices.push_back( i1 );
				faceVaryingIndices.push_back( i2 );
			}
		}
		else
		{
			assert( numFaceVerts == 3 );

			int i0 = faceVertexIdStart + 0;
			int i1 = faceVertexIdStart + 1;
			int i2 = faceVertexIdStart + 2;

			newVerticesPerFace->writable().push_back( 3 );

			/// Copy across the vertexId data
			newVertexIds->writable().push_back( vertexIds->readable()[ i0 ] );
			newVertexIds->writable().push_back( vertexIds->readable()[ i1 ] );
			newVertexIds->writable().push_back( vertexIds->readable()[ i2 ] );

			/// Store the indices required to rebuild the facevarying primvars
			faceVaryingIndices.push_back( i0 );
			faceVaryingIndices.push_back( i1 );
			faceVaryingIndices.push_back( i2 );
		}

		faceVertexIdStart += numFaceVerts;
	}

	mesh->setTopology( newVerticesPerFace, newVertexIds );

	/// Rebuild all the facevarying primvars, using the list of indices into the old data we created above.
	assert( faceVaryingIndices.size() == newVertexIds->readable().size() );
	TriangleDataRemap func( faceVaryingIndices );
	for ( PrimitiveVariableMap::iterator it = mesh->variables.begin(); it != mesh->variables.end(); ++it )
	{
		if ( it->second.interpolation == PrimitiveVariable::FaceVarying )
		{
 			assert( it->second.data );
			func.m_other = it->second.data;
			DataPtr data = it->second.data->copy();

			size_t primVarSize = despatchTypedData<TriangleDataRemap, TypeTraits::IsVectorTypedData>( data, func );
			assert( primVarSize == faceVaryingIndices.size() );
			(void)primVarSize;

			it->second.data = data;
		}
	}

	assert( mesh->arePrimitiveVariablesValid() );
}
<|endoftext|>


<commit_before>/*
  Parsing.cpp - HTTP request parsing.

  Copyright (c) 2015 Ivan Grokhotkov. All rights reserved.

  This library 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 St, Fifth Floor, Boston, MA  02110-1301  USA
  Modified 8 May 2015 by Hristo Gochkov (proper post and file upload handling)
*/

#include <Arduino.h>
#include "WiFiServer.h"
#include "WiFiClient.h"
#include "ESP8266WebServer.h"

// #define DEBUG
#define DEBUG_OUTPUT Serial1

bool ESP8266WebServer::_parseRequest(WiFiClient& client) {
  // Read the first line of HTTP request
  String req = client.readStringUntil('\r');
  client.readStringUntil('\n');

  // First line of HTTP request looks like "GET /path HTTP/1.1"
  // Retrieve the "/path" part by finding the spaces
  int addr_start = req.indexOf(' ');
  int addr_end = req.indexOf(' ', addr_start + 1);
  if (addr_start == -1 || addr_end == -1) {
#ifdef DEBUG
    DEBUG_OUTPUT.print("Invalid request: ");
    DEBUG_OUTPUT.println(req);
#endif
    return false;
  }

  String methodStr = req.substring(0, addr_start);
  String url = req.substring(addr_start + 1, addr_end);
  String searchStr = "";
  int hasSearch = url.indexOf('?');
  if (hasSearch != -1){
    searchStr = url.substring(hasSearch + 1);
    url = url.substring(0, hasSearch);
  }
  _currentUri = url;

  HTTPMethod method = HTTP_GET;
  if (methodStr == "POST") {
    method = HTTP_POST;
  } else if (methodStr == "DELETE") {
    method = HTTP_DELETE;
  } else if (methodStr == "PUT") {
    method = HTTP_PUT;
  } else if (methodStr == "PATCH") {
    method = HTTP_PATCH;
  }
  _currentMethod = method;

#ifdef DEBUG
  DEBUG_OUTPUT.print("method: ");
  DEBUG_OUTPUT.print(methodStr);
  DEBUG_OUTPUT.print(" url: ");
  DEBUG_OUTPUT.print(url);
  DEBUG_OUTPUT.print(" search: ");
  DEBUG_OUTPUT.println(searchStr);
#endif

  String formData;
  // below is needed only when POST type request
  if (method == HTTP_POST || method == HTTP_PUT || method == HTTP_PATCH || method == HTTP_DELETE){
    String boundaryStr;
    String headerName;
    String headerValue;
    bool isForm = false;
    uint32_t contentLength = 0;
    //parse headers
    while(1){
      req = client.readStringUntil('\r');
      client.readStringUntil('\n');
      if (req == "") break;//no moar headers
      int headerDiv = req.indexOf(':');
      if (headerDiv == -1){
        break;
      }
      headerName = req.substring(0, headerDiv);
      headerValue = req.substring(headerDiv + 2);
      if (headerName == "Content-Type"){
        if (headerValue.startsWith("text/plain")){
          isForm = false;
        } else if (headerValue.startsWith("multipart/form-data")){
          boundaryStr = headerValue.substring(headerValue.indexOf('=')+1);
          isForm = true;
        }
      } else if (headerName == "Content-Length"){
        contentLength = headerValue.toInt();
      }
    }

    if (!isForm){
      if (searchStr != "") searchStr += '&';
      String bodyLine = client.readStringUntil('\r');
      if(bodyLine.startsWith("{") || bodyLine.startsWith("[") || bodyLine.indexOf('=') == -1){
        //plain post json or other data
        searchStr += "plain";
      }
      searchStr += bodyLine;
      client.readStringUntil('\n');
    }
    _parseArguments(searchStr);
    if (isForm){
      _parseForm(client, boundaryStr, contentLength);
    }
  } else {
    _parseArguments(searchStr);
  }
  client.flush();

#ifdef DEBUG
  DEBUG_OUTPUT.print("Request: ");
  DEBUG_OUTPUT.println(url);
  DEBUG_OUTPUT.print(" Arguments: ");
  DEBUG_OUTPUT.println(searchStr);
#endif

  return true;
}

void ESP8266WebServer::_parseArguments(String data) {
#ifdef DEBUG
  DEBUG_OUTPUT.print("args: ");
  DEBUG_OUTPUT.println(data);
#endif
  if (_currentArgs)
    delete[] _currentArgs;
  _currentArgs = 0;
  if (data.length() == 0) {
    _currentArgCount = 0;
    return;
  }
  _currentArgCount = 1;

  for (int i = 0; i < data.length(); ) {
    i = data.indexOf('&', i);
    if (i == -1)
      break;
    ++i;
    ++_currentArgCount;
  }
#ifdef DEBUG
  DEBUG_OUTPUT.print("args count: ");
  DEBUG_OUTPUT.println(_currentArgCount);
#endif

  _currentArgs = new RequestArgument[_currentArgCount];
  int pos = 0;
  int iarg;
  for (iarg = 0; iarg < _currentArgCount;) {
    int equal_sign_index = data.indexOf('=', pos);
    int next_arg_index = data.indexOf('&', pos);
#ifdef DEBUG
    DEBUG_OUTPUT.print("pos ");
    DEBUG_OUTPUT.print(pos);
    DEBUG_OUTPUT.print("=@ ");
    DEBUG_OUTPUT.print(equal_sign_index);
    DEBUG_OUTPUT.print(" &@ ");
    DEBUG_OUTPUT.println(next_arg_index);
#endif
    if ((equal_sign_index == -1) || ((equal_sign_index > next_arg_index) && (next_arg_index != -1))) {
#ifdef DEBUG
      DEBUG_OUTPUT.print("arg missing value: ");
      DEBUG_OUTPUT.println(iarg);
#endif
      if (next_arg_index == -1)
        break;
      pos = next_arg_index + 1;
      continue;
    }
    RequestArgument& arg = _currentArgs[iarg];
    arg.key = data.substring(pos, equal_sign_index);
    arg.value = data.substring(equal_sign_index + 1, next_arg_index);
#ifdef DEBUG
    DEBUG_OUTPUT.print("arg ");
    DEBUG_OUTPUT.print(iarg);
    DEBUG_OUTPUT.print(" key: ");
    DEBUG_OUTPUT.print(arg.key);
    DEBUG_OUTPUT.print(" value: ");
    DEBUG_OUTPUT.println(arg.value);
#endif
    ++iarg;
    if (next_arg_index == -1)
      break;
    pos = next_arg_index + 1;
  }
  _currentArgCount = iarg;
#ifdef DEBUG
  DEBUG_OUTPUT.print("args count: ");
  DEBUG_OUTPUT.println(_currentArgCount);
#endif

}

void ESP8266WebServer::_uploadWriteByte(uint8_t b){
  if (_currentUpload.currentSize == HTTP_UPLOAD_BUFLEN){
    if (_fileUploadHandler) _fileUploadHandler();
    _currentUpload.totalSize += _currentUpload.currentSize;
    _currentUpload.currentSize = 0;
  }
  _currentUpload.buf[_currentUpload.currentSize++] = b;
}

uint8_t ESP8266WebServer::_uploadReadByte(WiFiClient& client){
  int res = client.read();
  if(res == -1){
    while(!client.available())
      yield();
    res = client.read();
  }
  return (uint8_t)res;
}

void ESP8266WebServer::_parseForm(WiFiClient& client, String boundary, uint32_t len){

#ifdef DEBUG
  DEBUG_OUTPUT.print("Parse Form: Boundary: ");
  DEBUG_OUTPUT.print(boundary);
  DEBUG_OUTPUT.print(" Length: ");
  DEBUG_OUTPUT.println(len);
#endif
  String line;
  line = client.readStringUntil('\r');
  client.readStringUntil('\n');
  //start reading the form
  if (line == ("--"+boundary)){
    RequestArgument* postArgs = new RequestArgument[32];
    int postArgsLen = 0;
    while(1){
      String argName;
      String argValue;
      String argType;
      String argFilename;
      bool argIsFile = false;

      line = client.readStringUntil('\r');
      client.readStringUntil('\n');
      if (line.startsWith("Content-Disposition")){
        int nameStart = line.indexOf('=');
        if (nameStart != -1){
          argName = line.substring(nameStart+2);
          nameStart = argName.indexOf('=');
          if (nameStart == -1){
            argName = argName.substring(0, argName.length() - 1);
          } else {
            argFilename = argName.substring(nameStart+2, argName.length() - 1);
            argName = argName.substring(0, argName.indexOf('"'));
            argIsFile = true;
#ifdef DEBUG
            DEBUG_OUTPUT.print("PostArg FileName: ");
            DEBUG_OUTPUT.println(argFilename);
#endif
            //use GET to set the filename if uploading using blob
            if (argFilename == "blob" && hasArg("filename")) argFilename = arg("filename");
          }
#ifdef DEBUG
          DEBUG_OUTPUT.print("PostArg Name: ");
          DEBUG_OUTPUT.println(argName);
#endif
          argType = "text/plain";
          line = client.readStringUntil('\r');
          client.readStringUntil('\n');
          if (line.startsWith("Content-Type")){
            argType = line.substring(line.indexOf(':')+2);
            //skip next line
            client.readStringUntil('\r');
            client.readStringUntil('\n');
          }
#ifdef DEBUG
          DEBUG_OUTPUT.print("PostArg Type: ");
          DEBUG_OUTPUT.println(argType);
#endif
          if (!argIsFile){
            while(1){
              line = client.readStringUntil('\r');
              client.readStringUntil('\n');
              if (line.startsWith("--"+boundary)) break;
              if (argValue.length() > 0) argValue += "\n";
              argValue += line;
            }
#ifdef DEBUG
            DEBUG_OUTPUT.print("PostArg Value: ");
            DEBUG_OUTPUT.println(argValue);
            DEBUG_OUTPUT.println();
#endif

            RequestArgument& arg = postArgs[postArgsLen++];
            arg.key = argName;
            arg.value = argValue;

            if (line == ("--"+boundary+"--")){
#ifdef DEBUG
              DEBUG_OUTPUT.println("Done Parsing POST");
#endif
              break;
            }
          } else {
            _currentUpload.status = UPLOAD_FILE_START;
            _currentUpload.name = argName;
            _currentUpload.filename = argFilename;
            _currentUpload.type = argType;
            _currentUpload.totalSize = 0;
            _currentUpload.currentSize = 0;
#ifdef DEBUG
            DEBUG_OUTPUT.print("Start File: ");
            DEBUG_OUTPUT.print(_currentUpload.filename);
            DEBUG_OUTPUT.print(" Type: ");
            DEBUG_OUTPUT.println(_currentUpload.type);
#endif
            if (_fileUploadHandler) _fileUploadHandler();
            _currentUpload.status = UPLOAD_FILE_WRITE;
            uint8_t argByte = _uploadReadByte(client);
readfile:
            while(argByte != 0x0D){
              _uploadWriteByte(argByte);
              argByte = _uploadReadByte(client);
            }

            argByte = _uploadReadByte(client);
            if (argByte == 0x0A){
              argByte = _uploadReadByte(client);
              if ((char)argByte != '-'){
                //continue reading the file
                _uploadWriteByte(0x0D);
                _uploadWriteByte(0x0A);
                goto readfile;
              } else {
                argByte = _uploadReadByte(client);
                if ((char)argByte != '-'){
                  //continue reading the file
                  _uploadWriteByte(0x0D);
                  _uploadWriteByte(0x0A);
                  _uploadWriteByte((uint8_t)('-'));
                  goto readfile;
                }
              }

              uint8_t endBuf[boundary.length()];
              client.readBytes(endBuf, boundary.length());

              if (strstr((const char*)endBuf, boundary.c_str()) != NULL){
                if (_fileUploadHandler) _fileUploadHandler();
                _currentUpload.totalSize += _currentUpload.currentSize;
                _currentUpload.status = UPLOAD_FILE_END;
                if (_fileUploadHandler) _fileUploadHandler();
#ifdef DEBUG
                DEBUG_OUTPUT.print("End File: ");
                DEBUG_OUTPUT.print(_currentUpload.filename);
                DEBUG_OUTPUT.print(" Type: ");
                DEBUG_OUTPUT.print(_currentUpload.type);
                DEBUG_OUTPUT.print(" Size: ");
                DEBUG_OUTPUT.println(_currentUpload.totalSize);
#endif
                line = client.readStringUntil(0x0D);
                client.readStringUntil(0x0A);
                if (line == "--"){
#ifdef DEBUG
                  DEBUG_OUTPUT.println("Done Parsing POST");
#endif
                  break;
                }
                continue;
              } else {
                _uploadWriteByte(0x0D);
                _uploadWriteByte(0x0A);
                _uploadWriteByte((uint8_t)('-'));
                _uploadWriteByte((uint8_t)('-'));
                uint32_t i = 0;
                while(i < boundary.length()){
                  _uploadWriteByte(endBuf[i++]);
                }
                argByte = _uploadReadByte(client);
                goto readfile;
              }
            } else {
              _uploadWriteByte(0x0D);
              goto readfile;
            }
            break;
          }
        }
      }
    }

    int iarg;
    int totalArgs = ((32 - postArgsLen) < _currentArgCount)?(32 - postArgsLen):_currentArgCount;
    for (iarg = 0; iarg < totalArgs; iarg++){
      RequestArgument& arg = postArgs[postArgsLen++];
      arg.key = _currentArgs[iarg].key;
      arg.value = _currentArgs[iarg].value;
    }
    if (_currentArgs) delete[] _currentArgs;
    _currentArgs = new RequestArgument[postArgsLen];
    for (iarg = 0; iarg < postArgsLen; iarg++){
      RequestArgument& arg = _currentArgs[iarg];
      arg.key = postArgs[iarg].key;
      arg.value = postArgs[iarg].value;
    }
    _currentArgCount = iarg;
    if (postArgs) delete[] postArgs;
  }
}

<commit_msg>missingg the equal sign<commit_after>/*
  Parsing.cpp - HTTP request parsing.

  Copyright (c) 2015 Ivan Grokhotkov. All rights reserved.

  This library 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 St, Fifth Floor, Boston, MA  02110-1301  USA
  Modified 8 May 2015 by Hristo Gochkov (proper post and file upload handling)
*/

#include <Arduino.h>
#include "WiFiServer.h"
#include "WiFiClient.h"
#include "ESP8266WebServer.h"

// #define DEBUG
#define DEBUG_OUTPUT Serial1

bool ESP8266WebServer::_parseRequest(WiFiClient& client) {
  // Read the first line of HTTP request
  String req = client.readStringUntil('\r');
  client.readStringUntil('\n');

  // First line of HTTP request looks like "GET /path HTTP/1.1"
  // Retrieve the "/path" part by finding the spaces
  int addr_start = req.indexOf(' ');
  int addr_end = req.indexOf(' ', addr_start + 1);
  if (addr_start == -1 || addr_end == -1) {
#ifdef DEBUG
    DEBUG_OUTPUT.print("Invalid request: ");
    DEBUG_OUTPUT.println(req);
#endif
    return false;
  }

  String methodStr = req.substring(0, addr_start);
  String url = req.substring(addr_start + 1, addr_end);
  String searchStr = "";
  int hasSearch = url.indexOf('?');
  if (hasSearch != -1){
    searchStr = url.substring(hasSearch + 1);
    url = url.substring(0, hasSearch);
  }
  _currentUri = url;

  HTTPMethod method = HTTP_GET;
  if (methodStr == "POST") {
    method = HTTP_POST;
  } else if (methodStr == "DELETE") {
    method = HTTP_DELETE;
  } else if (methodStr == "PUT") {
    method = HTTP_PUT;
  } else if (methodStr == "PATCH") {
    method = HTTP_PATCH;
  }
  _currentMethod = method;

#ifdef DEBUG
  DEBUG_OUTPUT.print("method: ");
  DEBUG_OUTPUT.print(methodStr);
  DEBUG_OUTPUT.print(" url: ");
  DEBUG_OUTPUT.print(url);
  DEBUG_OUTPUT.print(" search: ");
  DEBUG_OUTPUT.println(searchStr);
#endif

  String formData;
  // below is needed only when POST type request
  if (method == HTTP_POST || method == HTTP_PUT || method == HTTP_PATCH || method == HTTP_DELETE){
    String boundaryStr;
    String headerName;
    String headerValue;
    bool isForm = false;
    uint32_t contentLength = 0;
    //parse headers
    while(1){
      req = client.readStringUntil('\r');
      client.readStringUntil('\n');
      if (req == "") break;//no moar headers
      int headerDiv = req.indexOf(':');
      if (headerDiv == -1){
        break;
      }
      headerName = req.substring(0, headerDiv);
      headerValue = req.substring(headerDiv + 2);
      if (headerName == "Content-Type"){
        if (headerValue.startsWith("text/plain")){
          isForm = false;
        } else if (headerValue.startsWith("multipart/form-data")){
          boundaryStr = headerValue.substring(headerValue.indexOf('=')+1);
          isForm = true;
        }
      } else if (headerName == "Content-Length"){
        contentLength = headerValue.toInt();
      }
    }

    if (!isForm){
      if (searchStr != "") searchStr += '&';
      String bodyLine = client.readStringUntil('\r');
      if(bodyLine.startsWith("{") || bodyLine.startsWith("[") || bodyLine.indexOf('=') == -1){
        //plain post json or other data
        searchStr += "plain=";
      }
      searchStr += bodyLine;
      client.readStringUntil('\n');
    }
    _parseArguments(searchStr);
    if (isForm){
      _parseForm(client, boundaryStr, contentLength);
    }
  } else {
    _parseArguments(searchStr);
  }
  client.flush();

#ifdef DEBUG
  DEBUG_OUTPUT.print("Request: ");
  DEBUG_OUTPUT.println(url);
  DEBUG_OUTPUT.print(" Arguments: ");
  DEBUG_OUTPUT.println(searchStr);
#endif

  return true;
}

void ESP8266WebServer::_parseArguments(String data) {
#ifdef DEBUG
  DEBUG_OUTPUT.print("args: ");
  DEBUG_OUTPUT.println(data);
#endif
  if (_currentArgs)
    delete[] _currentArgs;
  _currentArgs = 0;
  if (data.length() == 0) {
    _currentArgCount = 0;
    return;
  }
  _currentArgCount = 1;

  for (int i = 0; i < data.length(); ) {
    i = data.indexOf('&', i);
    if (i == -1)
      break;
    ++i;
    ++_currentArgCount;
  }
#ifdef DEBUG
  DEBUG_OUTPUT.print("args count: ");
  DEBUG_OUTPUT.println(_currentArgCount);
#endif

  _currentArgs = new RequestArgument[_currentArgCount];
  int pos = 0;
  int iarg;
  for (iarg = 0; iarg < _currentArgCount;) {
    int equal_sign_index = data.indexOf('=', pos);
    int next_arg_index = data.indexOf('&', pos);
#ifdef DEBUG
    DEBUG_OUTPUT.print("pos ");
    DEBUG_OUTPUT.print(pos);
    DEBUG_OUTPUT.print("=@ ");
    DEBUG_OUTPUT.print(equal_sign_index);
    DEBUG_OUTPUT.print(" &@ ");
    DEBUG_OUTPUT.println(next_arg_index);
#endif
    if ((equal_sign_index == -1) || ((equal_sign_index > next_arg_index) && (next_arg_index != -1))) {
#ifdef DEBUG
      DEBUG_OUTPUT.print("arg missing value: ");
      DEBUG_OUTPUT.println(iarg);
#endif
      if (next_arg_index == -1)
        break;
      pos = next_arg_index + 1;
      continue;
    }
    RequestArgument& arg = _currentArgs[iarg];
    arg.key = data.substring(pos, equal_sign_index);
    arg.value = data.substring(equal_sign_index + 1, next_arg_index);
#ifdef DEBUG
    DEBUG_OUTPUT.print("arg ");
    DEBUG_OUTPUT.print(iarg);
    DEBUG_OUTPUT.print(" key: ");
    DEBUG_OUTPUT.print(arg.key);
    DEBUG_OUTPUT.print(" value: ");
    DEBUG_OUTPUT.println(arg.value);
#endif
    ++iarg;
    if (next_arg_index == -1)
      break;
    pos = next_arg_index + 1;
  }
  _currentArgCount = iarg;
#ifdef DEBUG
  DEBUG_OUTPUT.print("args count: ");
  DEBUG_OUTPUT.println(_currentArgCount);
#endif

}

void ESP8266WebServer::_uploadWriteByte(uint8_t b){
  if (_currentUpload.currentSize == HTTP_UPLOAD_BUFLEN){
    if (_fileUploadHandler) _fileUploadHandler();
    _currentUpload.totalSize += _currentUpload.currentSize;
    _currentUpload.currentSize = 0;
  }
  _currentUpload.buf[_currentUpload.currentSize++] = b;
}

uint8_t ESP8266WebServer::_uploadReadByte(WiFiClient& client){
  int res = client.read();
  if(res == -1){
    while(!client.available())
      yield();
    res = client.read();
  }
  return (uint8_t)res;
}

void ESP8266WebServer::_parseForm(WiFiClient& client, String boundary, uint32_t len){

#ifdef DEBUG
  DEBUG_OUTPUT.print("Parse Form: Boundary: ");
  DEBUG_OUTPUT.print(boundary);
  DEBUG_OUTPUT.print(" Length: ");
  DEBUG_OUTPUT.println(len);
#endif
  String line;
  line = client.readStringUntil('\r');
  client.readStringUntil('\n');
  //start reading the form
  if (line == ("--"+boundary)){
    RequestArgument* postArgs = new RequestArgument[32];
    int postArgsLen = 0;
    while(1){
      String argName;
      String argValue;
      String argType;
      String argFilename;
      bool argIsFile = false;

      line = client.readStringUntil('\r');
      client.readStringUntil('\n');
      if (line.startsWith("Content-Disposition")){
        int nameStart = line.indexOf('=');
        if (nameStart != -1){
          argName = line.substring(nameStart+2);
          nameStart = argName.indexOf('=');
          if (nameStart == -1){
            argName = argName.substring(0, argName.length() - 1);
          } else {
            argFilename = argName.substring(nameStart+2, argName.length() - 1);
            argName = argName.substring(0, argName.indexOf('"'));
            argIsFile = true;
#ifdef DEBUG
            DEBUG_OUTPUT.print("PostArg FileName: ");
            DEBUG_OUTPUT.println(argFilename);
#endif
            //use GET to set the filename if uploading using blob
            if (argFilename == "blob" && hasArg("filename")) argFilename = arg("filename");
          }
#ifdef DEBUG
          DEBUG_OUTPUT.print("PostArg Name: ");
          DEBUG_OUTPUT.println(argName);
#endif
          argType = "text/plain";
          line = client.readStringUntil('\r');
          client.readStringUntil('\n');
          if (line.startsWith("Content-Type")){
            argType = line.substring(line.indexOf(':')+2);
            //skip next line
            client.readStringUntil('\r');
            client.readStringUntil('\n');
          }
#ifdef DEBUG
          DEBUG_OUTPUT.print("PostArg Type: ");
          DEBUG_OUTPUT.println(argType);
#endif
          if (!argIsFile){
            while(1){
              line = client.readStringUntil('\r');
              client.readStringUntil('\n');
              if (line.startsWith("--"+boundary)) break;
              if (argValue.length() > 0) argValue += "\n";
              argValue += line;
            }
#ifdef DEBUG
            DEBUG_OUTPUT.print("PostArg Value: ");
            DEBUG_OUTPUT.println(argValue);
            DEBUG_OUTPUT.println();
#endif

            RequestArgument& arg = postArgs[postArgsLen++];
            arg.key = argName;
            arg.value = argValue;

            if (line == ("--"+boundary+"--")){
#ifdef DEBUG
              DEBUG_OUTPUT.println("Done Parsing POST");
#endif
              break;
            }
          } else {
            _currentUpload.status = UPLOAD_FILE_START;
            _currentUpload.name = argName;
            _currentUpload.filename = argFilename;
            _currentUpload.type = argType;
            _currentUpload.totalSize = 0;
            _currentUpload.currentSize = 0;
#ifdef DEBUG
            DEBUG_OUTPUT.print("Start File: ");
            DEBUG_OUTPUT.print(_currentUpload.filename);
            DEBUG_OUTPUT.print(" Type: ");
            DEBUG_OUTPUT.println(_currentUpload.type);
#endif
            if (_fileUploadHandler) _fileUploadHandler();
            _currentUpload.status = UPLOAD_FILE_WRITE;
            uint8_t argByte = _uploadReadByte(client);
readfile:
            while(argByte != 0x0D){
              _uploadWriteByte(argByte);
              argByte = _uploadReadByte(client);
            }

            argByte = _uploadReadByte(client);
            if (argByte == 0x0A){
              argByte = _uploadReadByte(client);
              if ((char)argByte != '-'){
                //continue reading the file
                _uploadWriteByte(0x0D);
                _uploadWriteByte(0x0A);
                goto readfile;
              } else {
                argByte = _uploadReadByte(client);
                if ((char)argByte != '-'){
                  //continue reading the file
                  _uploadWriteByte(0x0D);
                  _uploadWriteByte(0x0A);
                  _uploadWriteByte((uint8_t)('-'));
                  goto readfile;
                }
              }

              uint8_t endBuf[boundary.length()];
              client.readBytes(endBuf, boundary.length());

              if (strstr((const char*)endBuf, boundary.c_str()) != NULL){
                if (_fileUploadHandler) _fileUploadHandler();
                _currentUpload.totalSize += _currentUpload.currentSize;
                _currentUpload.status = UPLOAD_FILE_END;
                if (_fileUploadHandler) _fileUploadHandler();
#ifdef DEBUG
                DEBUG_OUTPUT.print("End File: ");
                DEBUG_OUTPUT.print(_currentUpload.filename);
                DEBUG_OUTPUT.print(" Type: ");
                DEBUG_OUTPUT.print(_currentUpload.type);
                DEBUG_OUTPUT.print(" Size: ");
                DEBUG_OUTPUT.println(_currentUpload.totalSize);
#endif
                line = client.readStringUntil(0x0D);
                client.readStringUntil(0x0A);
                if (line == "--"){
#ifdef DEBUG
                  DEBUG_OUTPUT.println("Done Parsing POST");
#endif
                  break;
                }
                continue;
              } else {
                _uploadWriteByte(0x0D);
                _uploadWriteByte(0x0A);
                _uploadWriteByte((uint8_t)('-'));
                _uploadWriteByte((uint8_t)('-'));
                uint32_t i = 0;
                while(i < boundary.length()){
                  _uploadWriteByte(endBuf[i++]);
                }
                argByte = _uploadReadByte(client);
                goto readfile;
              }
            } else {
              _uploadWriteByte(0x0D);
              goto readfile;
            }
            break;
          }
        }
      }
    }

    int iarg;
    int totalArgs = ((32 - postArgsLen) < _currentArgCount)?(32 - postArgsLen):_currentArgCount;
    for (iarg = 0; iarg < totalArgs; iarg++){
      RequestArgument& arg = postArgs[postArgsLen++];
      arg.key = _currentArgs[iarg].key;
      arg.value = _currentArgs[iarg].value;
    }
    if (_currentArgs) delete[] _currentArgs;
    _currentArgs = new RequestArgument[postArgsLen];
    for (iarg = 0; iarg < postArgsLen; iarg++){
      RequestArgument& arg = _currentArgs[iarg];
      arg.key = postArgs[iarg].key;
      arg.value = postArgs[iarg].value;
    }
    _currentArgCount = iarg;
    if (postArgs) delete[] postArgs;
  }
}

<|endoftext|>


<commit_before>
// the syntax for messages is:
//   wd 'mb type buttons title message'
//
// type specifies the icon and default behaviour:
//  info      (default OK button)
//  warn      (default OK button)
//  critical  (default OK button)
//  query     (requires two or three buttons)
//
// if one button, there is no result,
// otherwise the result is the button name (ok, cancel, ...)
//
// buttons are from the set:
//  mb_ok
//  mb_cancel
//  mb_yes
//  mb_no
//  mb_save
//  mb_discard

#include <QApplication>
#include <QColorDialog>
#include <QFileDialog>
#include <QFontDialog>
#include <QMessageBox>

#include "wd.h"
#include "cmd.h"

QString mbcolor();
QString mbdir();
QString mbfont();
QString mbinfo(QString);
QString mbmsg();
QString mbopen();
QString mbsave();

QString type;
QStringList arg;

QMessageBox::StandardButton getdefaultbutton();
QMessageBox::StandardButton getonebutton();
QMessageBox::StandardButtons getotherbuttons();
QString getname(int);

// ---------------------------------------------------------------------
QString mb(string p)
{
  arg=qsplit(p);
  if (arg.size()<2) return "";

  type=arg.first();
  arg.removeFirst();
  if (type=="color")
    return mbcolor();
  if (type=="dir")
    return mbdir();
  if (type=="font")
    return mbfont();
  if (type=="open")
    return mbopen();
  if (type=="save")
    return mbsave();
  if (type=="info"||type=="query"||type=="warn"||type=="critical")
    return mbmsg();
  return mbinfo("invalid mb type: " + type);
}

// ---------------------------------------------------------------------
QString mbmsg()
{
  int r;
  QString t,m;

  QMessageBox::StandardButton button1=getdefaultbutton();
  QMessageBox::StandardButtons buttons=getotherbuttons();

  if (arg.size()==1) {
    t="Message Box";
    m=arg.at(0);
  } else if (arg.size()==2) {
    t=arg.at(0);
    m=arg.at(1);
  } else {
    return mbinfo ("Need title and message: "+arg.join(" "));
  }

  if (button1==-1) {
    button1=QMessageBox::Ok;
    if (type=="query")
      buttons=QMessageBox::Cancel;
  }
  buttons|=button1;

  if (type=="query") {
    r=QMessageBox::question(QApplication::focusWidget(),t,m,buttons,button1);
    return getname(r);
  }
  if (type=="critical")
    QMessageBox::critical(QApplication::focusWidget(),t,m,buttons,button1);
  else if (type=="info")
    QMessageBox::information(QApplication::focusWidget(),t,m,buttons,button1);
  else if (type=="warn")
    QMessageBox::warning(QApplication::focusWidget(),t,m,buttons,button1);
  return "";
}

// ---------------------------------------------------------------------
QString mbcolor()
{
  QColor c;
  int r,g,b;

  if (arg.size()==3) {
    r=arg.at(0).toInt();
    g=arg.at(1).toInt();
    b=arg.at(2).toInt();
    c=QColor(r,g,b);
  } else
    c=Qt::white;
  c=QColorDialog::getColor(c,QApplication::focusWidget());
  if (!c.isValid()) return "";
  return s2q(i2s(c.red()) + " " + i2s(c.green()) + " " + i2s(c.blue()));
}

// ---------------------------------------------------------------------
QString mbdir()
{
  QString title,dir;
  if (arg.size()!=2)
    return mbinfo("dir needs title and directory");
  title=arg.at(0);
  dir=arg.at(1);
  return QFileDialog::getExistingDirectory(
           QApplication::focusWidget(),title,dir);
}

// ---------------------------------------------------------------------
QString mbfont()
{
  bool ok;
  QFont font, def;
  QString s;
  if (arg.size())
    def.setFamily(arg.at(0));
  if (arg.size()>1)
    def.setPointSize(arg.at(1).toInt());
  for (int i=2; i<arg.size(); i++) {
    s=arg.at(i);
    if (s=="bold")
      def.setBold(true);
    if (s=="italic")
      def.setItalic(true);
    if (s=="strikeout")
      def.setStrikeOut(true);
    if (s=="underline")
      def.setUnderline(true);
  }
  font=QFontDialog::getFont(&ok,def,QApplication::focusWidget());
  if (!ok) return "";
  QString r;
  r="\"" + font.family() + "\" " + QString::number(font.pointSize());
  if (font.bold()) r+=" bold";
  if (font.italic()) r+=" italic";
  if (font.strikeOut()) r+=" strikeout";
  if (font.underline()) r+=" underline";
  return r;
}

// ---------------------------------------------------------------------
QString mbopen()
{
  QString title,dir,filter;
  if (arg.size()<2)
    return mbinfo("open needs title, directory, [filters]");
  title=arg.at(0);
  dir=arg.at(1);
  if (arg.size()==3)
    filter=arg.at(2);
  return QFileDialog::getOpenFileName(
           QApplication::focusWidget(),title,dir,filter);
}

// ---------------------------------------------------------------------
QString mbsave()
{
  QString title,dir,filter;
  if (arg.size()<2)
    return mbinfo("save needs title, directory, [filters]");
  title=arg.at(0);
  dir=arg.at(1);
  if (arg.size()==3)
    filter=arg.at(2);
  return QFileDialog::getSaveFileName(
           QApplication::focusWidget(),title,dir,filter);
}

// ---------------------------------------------------------------------
QString getname(int b)
{
  if (b==QMessageBox::Ok)
    return "ok";
  if (b==QMessageBox::Cancel)
    return "cancel";
  if (b==QMessageBox::Yes)
    return "yes";
  if (b==QMessageBox::No)
    return "no";
  if (b==QMessageBox::Save)
    return "save";
  if (b==QMessageBox::Discard)
    return "discard";
  return "unknown button";
}

// ---------------------------------------------------------------------
QMessageBox::StandardButton getonebutton()
{
  if (arg.isEmpty()) return QMessageBox::NoButton;
  QString s=arg.first();
  if (s=="mb_ok")
    return QMessageBox::Ok;
  if (s=="mb_cancel")
    return QMessageBox::Cancel;
  if (s=="mb_yes")
    return QMessageBox::Yes;
  if (s=="mb_no")
    return QMessageBox::No;
  if (s=="mb_save")
    return QMessageBox::Save;
  if (s=="mb_discard")
    return QMessageBox::Discard;
  return QMessageBox::NoButton;
}

// ---------------------------------------------------------------------
QMessageBox::StandardButton getdefaultbutton()
{
  QMessageBox::StandardButton r=getonebutton();
  if (r!=QMessageBox::NoButton)
    arg.removeFirst();
  return r;
}

// ---------------------------------------------------------------------
QMessageBox::StandardButtons getotherbuttons()
{
  QMessageBox::StandardButtons r=QMessageBox::NoButton;
  QMessageBox::StandardButton b;
  while (arg.size()) {
    b=getonebutton();
    if (b==QMessageBox::NoButton)
      return r;
    r|=b;
    arg.removeFirst();
  }
  return r;
}

// ---------------------------------------------------------------------
QString mbinfo(QString s)
{
  info("Message Box",s);
  return "";
}
<commit_msg>mbfont without inital fontspec<commit_after>
// the syntax for messages is:
//   wd 'mb type buttons title message'
//
// type specifies the icon and default behaviour:
//  info      (default OK button)
//  warn      (default OK button)
//  critical  (default OK button)
//  query     (requires two or three buttons)
//
// if one button, there is no result,
// otherwise the result is the button name (ok, cancel, ...)
//
// buttons are from the set:
//  mb_ok
//  mb_cancel
//  mb_yes
//  mb_no
//  mb_save
//  mb_discard

#include <QApplication>
#include <QColorDialog>
#include <QFileDialog>
#include <QFontDialog>
#include <QMessageBox>

#include "wd.h"
#include "cmd.h"

QString mbcolor();
QString mbdir();
QString mbfont();
QString mbinfo(QString);
QString mbmsg();
QString mbopen();
QString mbsave();

QString type;
QStringList arg;

QMessageBox::StandardButton getdefaultbutton();
QMessageBox::StandardButton getonebutton();
QMessageBox::StandardButtons getotherbuttons();
QString getname(int);

// ---------------------------------------------------------------------
QString mb(string p)
{
  arg=qsplit(p);
  if (arg.size()<1) return mbinfo("missing mb type");

  type=arg.first();
  arg.removeFirst();
  if (type=="color")
    return mbcolor();
  if (type=="dir")
    return mbdir();
  if (type=="font")
    return mbfont();
  if (type=="open")
    return mbopen();
  if (type=="save")
    return mbsave();
  if (type=="info"||type=="query"||type=="warn"||type=="critical")
    return mbmsg();
  return mbinfo("invalid mb type: " + type);
}

// ---------------------------------------------------------------------
QString mbmsg()
{
  int r;
  QString t,m;

  QMessageBox::StandardButton button1=getdefaultbutton();
  QMessageBox::StandardButtons buttons=getotherbuttons();

  if (arg.size()==1) {
    t="Message Box";
    m=arg.at(0);
  } else if (arg.size()==2) {
    t=arg.at(0);
    m=arg.at(1);
  } else {
    return mbinfo ("Need title and message: "+arg.join(" "));
  }

  if (button1==-1) {
    button1=QMessageBox::Ok;
    if (type=="query")
      buttons=QMessageBox::Cancel;
  }
  buttons|=button1;

  if (type=="query") {
    r=QMessageBox::question(QApplication::focusWidget(),t,m,buttons,button1);
    return getname(r);
  }
  if (type=="critical")
    QMessageBox::critical(QApplication::focusWidget(),t,m,buttons,button1);
  else if (type=="info")
    QMessageBox::information(QApplication::focusWidget(),t,m,buttons,button1);
  else if (type=="warn")
    QMessageBox::warning(QApplication::focusWidget(),t,m,buttons,button1);
  return "";
}

// ---------------------------------------------------------------------
QString mbcolor()
{
  QColor c;
  int r,g,b;

  if (arg.size()==3) {
    r=arg.at(0).toInt();
    g=arg.at(1).toInt();
    b=arg.at(2).toInt();
    c=QColor(r,g,b);
  } else
    c=Qt::white;
  c=QColorDialog::getColor(c,QApplication::focusWidget());
  if (!c.isValid()) return "";
  return s2q(i2s(c.red()) + " " + i2s(c.green()) + " " + i2s(c.blue()));
}

// ---------------------------------------------------------------------
QString mbdir()
{
  QString title,dir;
  if (arg.size()!=2)
    return mbinfo("dir needs title and directory");
  title=arg.at(0);
  dir=arg.at(1);
  return QFileDialog::getExistingDirectory(
           QApplication::focusWidget(),title,dir);
}

// ---------------------------------------------------------------------
QString mbfont()
{
  bool ok;
  QFont font, def;
  QString s;
  def.setStrikeOut(false);
  def.setUnderline(false);
  if (arg.size())
    def.setFamily(arg.at(0));
  if (arg.size()>1)
    def.setPointSize(arg.at(1).toInt());
  for (int i=2; i<arg.size(); i++) {
    s=arg.at(i);
    if (s=="bold")
      def.setBold(true);
    if (s=="italic")
      def.setItalic(true);
    if (s=="strikeout")
      def.setStrikeOut(true);
    if (s=="underline")
      def.setUnderline(true);
  }
  font=QFontDialog::getFont(&ok,def,QApplication::focusWidget());
  if (!ok) return "";
  QString r;
  r="\"" + font.family() + "\" " + QString::number(font.pointSize());
  if (font.bold()) r+=" bold";
  if (font.italic()) r+=" italic";
  if (font.strikeOut()) r+=" strikeout";
  if (font.underline()) r+=" underline";
  return r;
}

// ---------------------------------------------------------------------
QString mbopen()
{
  QString title,dir,filter;
  if (arg.size()<2)
    return mbinfo("open needs title, directory, [filters]");
  title=arg.at(0);
  dir=arg.at(1);
  if (arg.size()==3)
    filter=arg.at(2);
  return QFileDialog::getOpenFileName(
           QApplication::focusWidget(),title,dir,filter);
}

// ---------------------------------------------------------------------
QString mbsave()
{
  QString title,dir,filter;
  if (arg.size()<2)
    return mbinfo("save needs title, directory, [filters]");
  title=arg.at(0);
  dir=arg.at(1);
  if (arg.size()==3)
    filter=arg.at(2);
  return QFileDialog::getSaveFileName(
           QApplication::focusWidget(),title,dir,filter);
}

// ---------------------------------------------------------------------
QString getname(int b)
{
  if (b==QMessageBox::Ok)
    return "ok";
  if (b==QMessageBox::Cancel)
    return "cancel";
  if (b==QMessageBox::Yes)
    return "yes";
  if (b==QMessageBox::No)
    return "no";
  if (b==QMessageBox::Save)
    return "save";
  if (b==QMessageBox::Discard)
    return "discard";
  return "unknown button";
}

// ---------------------------------------------------------------------
QMessageBox::StandardButton getonebutton()
{
  if (arg.isEmpty()) return QMessageBox::NoButton;
  QString s=arg.first();
  if (s=="mb_ok")
    return QMessageBox::Ok;
  if (s=="mb_cancel")
    return QMessageBox::Cancel;
  if (s=="mb_yes")
    return QMessageBox::Yes;
  if (s=="mb_no")
    return QMessageBox::No;
  if (s=="mb_save")
    return QMessageBox::Save;
  if (s=="mb_discard")
    return QMessageBox::Discard;
  return QMessageBox::NoButton;
}

// ---------------------------------------------------------------------
QMessageBox::StandardButton getdefaultbutton()
{
  QMessageBox::StandardButton r=getonebutton();
  if (r!=QMessageBox::NoButton)
    arg.removeFirst();
  return r;
}

// ---------------------------------------------------------------------
QMessageBox::StandardButtons getotherbuttons()
{
  QMessageBox::StandardButtons r=QMessageBox::NoButton;
  QMessageBox::StandardButton b;
  while (arg.size()) {
    b=getonebutton();
    if (b==QMessageBox::NoButton)
      return r;
    r|=b;
    arg.removeFirst();
  }
  return r;
}

// ---------------------------------------------------------------------
QString mbinfo(QString s)
{
  info("Message Box",s);
  return "";
}
<|endoftext|>


<commit_before>/***************************************************************************
 *   Copyright (c) 2020 sliptonic <shopinthewoods@gmail.com>               *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 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 Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#include "PreCompiled.h"

#ifndef _PreComp_
# include <Standard_math.hxx>
# include <cinttypes>
# include <iomanip>
# include <boost/algorithm/string.hpp>
# include <boost/lexical_cast.hpp>
#endif

#include <Base/Vector3D.h>
#include <Base/Writer.h>
#include <Base/Reader.h>
#include <Base/Exception.h>
#include "Voronoi.h"

using namespace Base;
using namespace Path;

TYPESYSTEM_SOURCE(Path::Voronoi , Base::BaseClass)

// Helpers

// Voronoi::diagram_type

Voronoi::diagram_type::diagram_type()
  :scale(1000)
{
}

double Voronoi::diagram_type::getScale() const {
  return scale;
}

void Voronoi::diagram_type::setScale(double s) {
  scale = s;
}

Base::Vector3d Voronoi::diagram_type::scaledVector(double x, double y, double z) const {
  return Base::Vector3d(x / scale, y / scale, z);
}

Base::Vector3d Voronoi::diagram_type::scaledVector(const point_type &p, double z) const {
  return scaledVector(p.x(), p.y(), z);
}

Base::Vector3d Voronoi::diagram_type::scaledVector(const vertex_type &v, double z) const {
  return scaledVector(v.x(), v.y(), z);
}

int Voronoi::diagram_type::index(const Voronoi::diagram_type::cell_type   *cell)   const {
  auto it = cell_index.find(intptr_t(cell));
  if (it == cell_index.end()) {
    return Voronoi::InvalidIndex;
  }
  return it->second;
}
int Voronoi::diagram_type::index(const Voronoi::diagram_type::edge_type   *edge)   const {
  auto it = edge_index.find(intptr_t(edge));
  if (it == edge_index.end()) {
    return Voronoi::InvalidIndex;
  }
  return it->second;
}
int Voronoi::diagram_type::index(const Voronoi::diagram_type::vertex_type *vertex) const {
  auto it = vertex_index.find(intptr_t(vertex));
  if (it == vertex_index.end()) {
    return Voronoi::InvalidIndex;
  }
  return it->second;
}

void Voronoi::diagram_type::reIndex() {
  int idx = 0;
  cell_index.clear();
  edge_index.clear();
  vertex_index.clear();

  idx = 0;
  for (auto it = cells().begin(); it != cells().end(); ++it, ++idx) {
    cell_index[intptr_t(&(*it))] = idx;
  }
  idx = 0;
  for (auto it = edges().begin(); it != edges().end(); ++it, ++idx) {
    edge_index[intptr_t(&(*it))] = idx;
  }
  idx = 0;
  for (auto it = vertices().begin(); it != vertices().end(); ++it, ++idx) {
    vertex_index[intptr_t(&(*it))] = idx;
  }
}

Voronoi::point_type Voronoi::diagram_type::retrievePoint(const Voronoi::diagram_type::cell_type *cell) const {
  Voronoi::diagram_type::cell_type::source_index_type index = cell->source_index();
  Voronoi::diagram_type::cell_type::source_category_type category = cell->source_category();
  if (category == boost::polygon::SOURCE_CATEGORY_SINGLE_POINT) {
    return points[index];
  }
  index -= points.size();
  if (category == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) {
    return low(segments[index]);
  } else {
    return high(segments[index]);
  }
}

Voronoi::segment_type Voronoi::diagram_type::retrieveSegment(const Voronoi::diagram_type::cell_type *cell) const {
  Voronoi::diagram_type::cell_type::source_index_type index = cell->source_index() - points.size();
  return segments[index];
}

// Voronoi

Voronoi::Voronoi()
  :vd(new diagram_type)
{
}

Voronoi::~Voronoi()
{
}

void Voronoi::addPoint(const Voronoi::point_type &p) {
  Voronoi::point_type pi;
  pi.x(p.x() * vd->getScale());
  pi.y(p.y() * vd->getScale());
  vd->points.push_back(pi);
}

void Voronoi::addSegment(const Voronoi::segment_type &s) {
  Voronoi::point_type pil, pih;
  pil.x(low(s).x() * vd->getScale());
  pil.y(low(s).y() * vd->getScale());
  pih.x(high(s).x() * vd->getScale());
  pih.y(high(s).y() * vd->getScale());
  vd->segments.push_back(segment_type(pil, pih));
}

long Voronoi::numPoints() const {
  return vd->points.size();
}

long Voronoi::numSegments() const {
  return vd->segments.size();
}

long Voronoi::numCells() const {
  return vd->num_cells();
}

long Voronoi::numEdges() const {
  return vd->num_edges();
}

long Voronoi::numVertices() const {
  return vd->num_vertices();
}

void Voronoi::construct()
{
  vd->clear();
  construct_voronoi(vd->points.begin(), vd->points.end(), vd->segments.begin(), vd->segments.end(), (voronoi_diagram_type*)vd);
  vd->reIndex();
}

void Voronoi::colorExterior(const Voronoi::diagram_type::edge_type *edge, std::size_t colorValue) {
  if (edge->color()) {
    // end recursion
    return;
  }
  edge->color(colorValue);
  edge->twin()->color(colorValue);
  auto v = edge->vertex1();
  if (v == NULL || !edge->is_primary()) {
    return;
  }
  v->color(colorValue);
  auto e = v->incident_edge();
  do {
    colorExterior(e, colorValue);
    e = e->rot_next();
  } while (e != v->incident_edge());
}

void Voronoi::colorExterior(Voronoi::color_type color) {
  for (diagram_type::const_edge_iterator it = vd->edges().begin(); it != vd->edges().end(); ++it) {
    if (it->is_infinite()) {
      colorExterior(&(*it), color);
    }
  }
}

void Voronoi::colorTwins(Voronoi::color_type color) {
  for (diagram_type::const_edge_iterator it = vd->edges().begin(); it != vd->edges().end(); ++it) {
    if (!it->color()) {
      auto twin = it->twin();
      if (!twin->color()) {
        twin->color(color);
      }
    }
  }
}

double Voronoi::diagram_type::angleOfSegment(int i, Voronoi::diagram_type::angle_map_t *angle) const {
  Voronoi::diagram_type::angle_map_t::const_iterator a = angle ? angle->find(i) : Voronoi::diagram_type::angle_map_t::const_iterator();
  if (!angle || a == angle->end()) {
    Voronoi::point_type p0 = low(segments[i]);
    Voronoi::point_type p1 = high(segments[i]);
    double ang = 0;
    if (p0.x() == p1.x()) {
      if ((p0.y() > 0 && p1.y() > 0) || (p0.y() > 0 && p1.y() > 0)) {
        ang = M_PI_2;
      } else {
        ang = -M_PI_2;
      }
    } else {
      ang = atan((p0.y() - p1.y()) / (p0.x() - p1.x()));
    }
    if (angle) {
      angle->insert(angle_map_t::value_type(i, ang));
    }
    return ang;
  }
  return a->second;
}

static bool pointsMatch(const Voronoi::point_type &p0, const Voronoi::point_type &p1) {
  return long(p0.x()) == long(p1.x()) && long(p0.y()) == long(p1.y());
}

bool Voronoi::diagram_type::segmentsAreConnected(int i, int j) const {
  return
       pointsMatch(low(segments[i]), low(segments[j]))
    || pointsMatch(low(segments[i]), high(segments[j]))
    || pointsMatch(high(segments[i]), low(segments[j]))
    || pointsMatch(high(segments[i]), high(segments[j]));
}

void Voronoi::colorColinear(Voronoi::color_type color, double degree) {
  double rad = degree * M_PI / 180;

  Voronoi::diagram_type::angle_map_t angle;
  int psize = vd->points.size();

  for (diagram_type::const_edge_iterator it = vd->edges().begin(); it != vd->edges().end(); ++it) {
    int i0 = it->cell()->source_index() - psize;
    int i1 = it->twin()->cell()->source_index() - psize;
    if (it->color() == 0
        && it->cell()->contains_segment()
        && it->twin()->cell()->contains_segment()
        && vd->segmentsAreConnected(i0, i1)) {
      double a0 = vd->angleOfSegment(i0, &angle);
      double a1 = vd->angleOfSegment(i1, &angle);
      double a = a0 - a1;
      if (a > M_PI_2) {
        a -= M_PI;
      } else if (a < -M_PI_2) {
        a += M_PI;
      }
      if (fabs(a) < rad) {
        it->color(color);
        it->twin()->color(color);
      }
    }
  }
}

void Voronoi::resetColor(Voronoi::color_type color) {
  for (auto it = vd->cells().begin(); it != vd->cells().end(); ++it) {
    if (color == 0 || it->color() == color) {
      it->color(0);
    }
  }
  for (auto it = vd->edges().begin(); it != vd->edges().end(); ++it) {
    if (it->color() == color) {
      it->color(0);
    }
  }
  for (auto it = vd->vertices().begin(); it != vd->vertices().end(); ++it) {
    if (it->color() == color) {
      it->color(0);
    }
  }
}
<commit_msg>Fixed angle detection<commit_after>/***************************************************************************
 *   Copyright (c) 2020 sliptonic <shopinthewoods@gmail.com>               *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 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 Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#include "PreCompiled.h"

#ifndef _PreComp_
# include <Standard_math.hxx>
# include <cinttypes>
# include <iomanip>
# include <boost/algorithm/string.hpp>
# include <boost/lexical_cast.hpp>
#endif

#include <Base/Vector3D.h>
#include <Base/Writer.h>
#include <Base/Reader.h>
#include <Base/Exception.h>
#include "Voronoi.h"

using namespace Base;
using namespace Path;

TYPESYSTEM_SOURCE(Path::Voronoi , Base::BaseClass)

// Helpers

// Voronoi::diagram_type

Voronoi::diagram_type::diagram_type()
  :scale(1000)
{
}

double Voronoi::diagram_type::getScale() const {
  return scale;
}

void Voronoi::diagram_type::setScale(double s) {
  scale = s;
}

Base::Vector3d Voronoi::diagram_type::scaledVector(double x, double y, double z) const {
  return Base::Vector3d(x / scale, y / scale, z);
}

Base::Vector3d Voronoi::diagram_type::scaledVector(const point_type &p, double z) const {
  return scaledVector(p.x(), p.y(), z);
}

Base::Vector3d Voronoi::diagram_type::scaledVector(const vertex_type &v, double z) const {
  return scaledVector(v.x(), v.y(), z);
}

int Voronoi::diagram_type::index(const Voronoi::diagram_type::cell_type   *cell)   const {
  auto it = cell_index.find(intptr_t(cell));
  if (it == cell_index.end()) {
    return Voronoi::InvalidIndex;
  }
  return it->second;
}
int Voronoi::diagram_type::index(const Voronoi::diagram_type::edge_type   *edge)   const {
  auto it = edge_index.find(intptr_t(edge));
  if (it == edge_index.end()) {
    return Voronoi::InvalidIndex;
  }
  return it->second;
}
int Voronoi::diagram_type::index(const Voronoi::diagram_type::vertex_type *vertex) const {
  auto it = vertex_index.find(intptr_t(vertex));
  if (it == vertex_index.end()) {
    return Voronoi::InvalidIndex;
  }
  return it->second;
}

void Voronoi::diagram_type::reIndex() {
  int idx = 0;
  cell_index.clear();
  edge_index.clear();
  vertex_index.clear();

  idx = 0;
  for (auto it = cells().begin(); it != cells().end(); ++it, ++idx) {
    cell_index[intptr_t(&(*it))] = idx;
  }
  idx = 0;
  for (auto it = edges().begin(); it != edges().end(); ++it, ++idx) {
    edge_index[intptr_t(&(*it))] = idx;
  }
  idx = 0;
  for (auto it = vertices().begin(); it != vertices().end(); ++it, ++idx) {
    vertex_index[intptr_t(&(*it))] = idx;
  }
}

Voronoi::point_type Voronoi::diagram_type::retrievePoint(const Voronoi::diagram_type::cell_type *cell) const {
  Voronoi::diagram_type::cell_type::source_index_type index = cell->source_index();
  Voronoi::diagram_type::cell_type::source_category_type category = cell->source_category();
  if (category == boost::polygon::SOURCE_CATEGORY_SINGLE_POINT) {
    return points[index];
  }
  index -= points.size();
  if (category == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) {
    return low(segments[index]);
  } else {
    return high(segments[index]);
  }
}

Voronoi::segment_type Voronoi::diagram_type::retrieveSegment(const Voronoi::diagram_type::cell_type *cell) const {
  Voronoi::diagram_type::cell_type::source_index_type index = cell->source_index() - points.size();
  return segments[index];
}

// Voronoi

Voronoi::Voronoi()
  :vd(new diagram_type)
{
}

Voronoi::~Voronoi()
{
}

void Voronoi::addPoint(const Voronoi::point_type &p) {
  Voronoi::point_type pi;
  pi.x(p.x() * vd->getScale());
  pi.y(p.y() * vd->getScale());
  vd->points.push_back(pi);
}

void Voronoi::addSegment(const Voronoi::segment_type &s) {
  Voronoi::point_type pil, pih;
  pil.x(low(s).x() * vd->getScale());
  pil.y(low(s).y() * vd->getScale());
  pih.x(high(s).x() * vd->getScale());
  pih.y(high(s).y() * vd->getScale());
  vd->segments.push_back(segment_type(pil, pih));
}

long Voronoi::numPoints() const {
  return vd->points.size();
}

long Voronoi::numSegments() const {
  return vd->segments.size();
}

long Voronoi::numCells() const {
  return vd->num_cells();
}

long Voronoi::numEdges() const {
  return vd->num_edges();
}

long Voronoi::numVertices() const {
  return vd->num_vertices();
}

void Voronoi::construct()
{
  vd->clear();
  construct_voronoi(vd->points.begin(), vd->points.end(), vd->segments.begin(), vd->segments.end(), (voronoi_diagram_type*)vd);
  vd->reIndex();
}

void Voronoi::colorExterior(const Voronoi::diagram_type::edge_type *edge, std::size_t colorValue) {
  if (edge->color()) {
    // end recursion
    return;
  }
  edge->color(colorValue);
  edge->twin()->color(colorValue);
  auto v = edge->vertex1();
  if (v == NULL || !edge->is_primary()) {
    return;
  }
  v->color(colorValue);
  auto e = v->incident_edge();
  do {
    colorExterior(e, colorValue);
    e = e->rot_next();
  } while (e != v->incident_edge());
}

void Voronoi::colorExterior(Voronoi::color_type color) {
  for (diagram_type::const_edge_iterator it = vd->edges().begin(); it != vd->edges().end(); ++it) {
    if (it->is_infinite()) {
      colorExterior(&(*it), color);
    }
  }
}

void Voronoi::colorTwins(Voronoi::color_type color) {
  for (diagram_type::const_edge_iterator it = vd->edges().begin(); it != vd->edges().end(); ++it) {
    if (!it->color()) {
      auto twin = it->twin();
      if (!twin->color()) {
        twin->color(color);
      }
    }
  }
}

double Voronoi::diagram_type::angleOfSegment(int i, Voronoi::diagram_type::angle_map_t *angle) const {
  Voronoi::diagram_type::angle_map_t::const_iterator a = angle ? angle->find(i) : Voronoi::diagram_type::angle_map_t::const_iterator();
  if (!angle || a == angle->end()) {
    Voronoi::point_type p0 = low(segments[i]);
    Voronoi::point_type p1 = high(segments[i]);
    double ang = 0;
    if (p0.x() == p1.x()) {
      if (p0.y() < p1.y()) {
        ang = M_PI_2;
      } else {
        ang = -M_PI_2;
      }
    } else {
      ang = atan((p0.y() - p1.y()) / (p0.x() - p1.x()));
    }
    if (angle) {
      angle->insert(angle_map_t::value_type(i, ang));
    }
    return ang;
  }
  return a->second;
}

static bool pointsMatch(const Voronoi::point_type &p0, const Voronoi::point_type &p1) {
  return long(p0.x()) == long(p1.x()) && long(p0.y()) == long(p1.y());
}

bool Voronoi::diagram_type::segmentsAreConnected(int i, int j) const {
  return
       pointsMatch(low(segments[i]), low(segments[j]))
    || pointsMatch(low(segments[i]), high(segments[j]))
    || pointsMatch(high(segments[i]), low(segments[j]))
    || pointsMatch(high(segments[i]), high(segments[j]));
}

void Voronoi::colorColinear(Voronoi::color_type color, double degree) {
  double rad = degree * M_PI / 180;

  Voronoi::diagram_type::angle_map_t angle;
  int psize = vd->points.size();

  for (diagram_type::const_edge_iterator it = vd->edges().begin(); it != vd->edges().end(); ++it) {
    int i0 = it->cell()->source_index() - psize;
    int i1 = it->twin()->cell()->source_index() - psize;
    if (it->color() == 0
        && it->cell()->contains_segment()
        && it->twin()->cell()->contains_segment()
        && vd->segmentsAreConnected(i0, i1)) {
      double a0 = vd->angleOfSegment(i0, &angle);
      double a1 = vd->angleOfSegment(i1, &angle);
      double a = a0 - a1;
      if (a > M_PI_2) {
        a -= M_PI;
      } else if (a < -M_PI_2) {
        a += M_PI;
      }
      if (fabs(a) < rad) {
        it->color(color);
        it->twin()->color(color);
      }
    }
  }
}

void Voronoi::resetColor(Voronoi::color_type color) {
  for (auto it = vd->cells().begin(); it != vd->cells().end(); ++it) {
    if (color == 0 || it->color() == color) {
      it->color(0);
    }
  }
  for (auto it = vd->edges().begin(); it != vd->edges().end(); ++it) {
    if (it->color() == color) {
      it->color(0);
    }
  }
  for (auto it = vd->vertices().begin(); it != vd->vertices().end(); ++it) {
    if (it->color() == color) {
      it->color(0);
    }
  }
}
<|endoftext|>


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

#include "iceoryx_posh/internal/popo/building_blocks/chunk_distributor.hpp"
#include "iceoryx_posh/internal/popo/building_blocks/chunk_receiver.hpp"
#include "iceoryx_posh/internal/popo/building_blocks/chunk_sender.hpp"
#include "iceoryx_posh/internal/popo/building_blocks/locking_policy.hpp"
#include "iceoryx_posh/internal/popo/ports/base_port.hpp"
#include "iceoryx_posh/mepoo/mepoo_config.hpp"
#include "iceoryx_utils/cxx/generic_raii.hpp"

#include "test.hpp"

#include <chrono>
#include <stdlib.h>
#include <thread>

using namespace ::testing;
using namespace iox::popo;
using namespace iox::cxx;
using namespace iox::mepoo;
using namespace iox::posix;
using ::testing::Return;

struct DummySample
{
    uint64_t m_dummy{42};
};

static constexpr uint32_t NUM_CHUNKS_IN_POOL = 3 * iox::MAX_SUBSCRIBER_QUEUE_CAPACITY;
static constexpr uint32_t SMALL_CHUNK = 128;
static constexpr uint32_t CHUNK_META_INFO_SIZE = 256;
static constexpr size_t MEMORY_SIZE = NUM_CHUNKS_IN_POOL * (SMALL_CHUNK + CHUNK_META_INFO_SIZE);
alignas(64) static uint8_t g_memory[MEMORY_SIZE];
static constexpr uint32_t ITERATIONS = 10000;
static constexpr uint32_t MAX_NUMBER_QUEUES = 128;

struct ChunkDistributorConfig
{
    static constexpr uint32_t MAX_QUEUES = MAX_NUMBER_QUEUES;
    static constexpr uint64_t MAX_HISTORY_CAPACITY = iox::MAX_PUBLISHER_HISTORY;
};

struct ChunkQueueConfig
{
    static constexpr uint64_t MAX_QUEUE_CAPACITY = iox::MAX_SUBSCRIBER_QUEUE_CAPACITY;
};

using ChunkQueueData_t = ChunkQueueData<ChunkQueueConfig, ThreadSafePolicy>;
using ChunkDistributorData_t =
    ChunkDistributorData<ChunkDistributorConfig, ThreadSafePolicy, ChunkQueuePusher<ChunkQueueData_t>>;
using ChunkDistributor_t = ChunkDistributor<ChunkDistributorData_t>;
using ChunkQueuePopper_t = ChunkQueuePopper<ChunkQueueData_t>;
using ChunkSenderData_t =
    ChunkSenderData<iox::MAX_CHUNKS_ALLOCATED_PER_PUBLISHER_SIMULTANEOUSLY, ChunkDistributorData_t>;
using ChunkReceiverData_t = ChunkReceiverData<iox::MAX_CHUNKS_HELD_PER_SUBSCRIBER_SIMULTANEOUSLY, ChunkQueueData_t>;

class ChunkBuildingBlocks_IntegrationTest : public Test
{
  public:
    ChunkBuildingBlocks_IntegrationTest()
    {
        m_mempoolConfig.addMemPool({SMALL_CHUNK, NUM_CHUNKS_IN_POOL});
        m_memoryManager.configureMemoryManager(m_mempoolConfig, &m_memoryAllocator, &m_memoryAllocator);
    }
    virtual ~ChunkBuildingBlocks_IntegrationTest()
    {
        /// @note One chunk is on hold due to the fact that chunkSender and chunkDistributor hold last chunk
        EXPECT_THAT(m_memoryManager.getMemPoolInfo(0).m_usedChunks, Eq(1));
    }

    void SetUp()
    {
        m_chunkSender.tryAddQueue(&m_chunkQueueData);
        m_chunkDistributor.tryAddQueue(&m_chunkReceiverData);
    }
    void TearDown(){};

    void publish()
    {
        for (size_t i = 0; i < ITERATIONS; i++)
        {
            m_chunkSender.tryAllocate(sizeof(DummySample), iox::UniquePortId())
                .and_then([&](auto chunkHeader) {
                    auto sample = chunkHeader->payload();
                    new (sample) DummySample();
                    static_cast<DummySample*>(sample)->m_dummy = i;
                    m_chunkSender.send(chunkHeader);
                    m_sendCounter++;
                })
                .or_else([](AllocationError) {
                    // Errors shall never occur
                    FAIL();
                });

            /// Add some jitter to make thread breathe
            std::this_thread::sleep_for(std::chrono::nanoseconds(rand() % 100));
        }
        // Signal the next threads we're done
        m_publisherRun = false;
    }

    void forward()
    {
        uint64_t forwardCounter{0};
        bool finished{false};
        // this is to prevent a race condition on thread shutdown; there must be two consecutive empty pops after the
        // publish thread finished
        bool newChunkReceivedInLastIteration{true};
        while (!finished)
        {
            ASSERT_FALSE(m_popper.hasOverflown());

            m_popper.tryPop()
                .and_then([&](auto& chunk) {
                    auto dummySample = *reinterpret_cast<DummySample*>(chunk.getPayload());
                    // Check if monotonically increasing
                    EXPECT_THAT(dummySample.m_dummy, Eq(forwardCounter));
                    m_chunkDistributor.deliverToAllStoredQueues(chunk);
                    forwardCounter++;
                    newChunkReceivedInLastIteration = true;
                })
                .or_else([&] {
                    if (!m_publisherRun.load(std::memory_order_relaxed))
                    {
                        if (newChunkReceivedInLastIteration)
                        {
                            newChunkReceivedInLastIteration = false;
                        }
                        else
                        {
                            finished = true;
                        }
                    }
                });
        }
        // Signal the next threads we're done
        m_forwarderRun = false;
    }

    void subscribe()
    {
        bool finished{false};
        // this is to prevent a race condition on thread shutdown; there must be two consecutive empty pops after the
        // forward thread finished
        bool newChunkReceivedInLastIteration{true};

        while (!finished)
        {
            ASSERT_FALSE(m_chunkReceiver.hasOverflown());

            m_chunkReceiver.tryGet()
                .and_then([&](iox::cxx::optional<const iox::mepoo::ChunkHeader*>& maybeChunkHeader) {
                    if (maybeChunkHeader.has_value())
                    {
                        auto chunkHeader = maybeChunkHeader.value();
                        auto dummySample = *reinterpret_cast<DummySample*>(chunkHeader->payload());
                        // Check if monotonically increasing
                        EXPECT_THAT(dummySample.m_dummy, Eq(m_receiveCounter));
                        m_receiveCounter++;
                        m_chunkReceiver.release(chunkHeader);
                        newChunkReceivedInLastIteration = true;
                    }
                    else if (!m_forwarderRun.load(std::memory_order_relaxed))
                    {
                        if (newChunkReceivedInLastIteration)
                        {
                            newChunkReceivedInLastIteration = false;
                        }
                        else
                        {
                            finished = true;
                        }
                    }
                })
                .or_else([](ChunkReceiveError) {
                    // Errors shall never occur
                    FAIL();
                });
        }
    }

    iox::cxx::GenericRAII m_uniqueRouDiId{[] { iox::popo::internal::setUniqueRouDiId(0); },
                                          [] { iox::popo::internal::unsetUniqueRouDiId(); }};
    uint64_t m_sendCounter{0};
    uint64_t m_receiveCounter{0};
    std::atomic<bool> m_publisherRun{true};
    std::atomic<bool> m_forwarderRun{true};

    // Memory objects
    Allocator m_memoryAllocator{g_memory, MEMORY_SIZE};
    MePooConfig m_mempoolConfig;
    MemoryManager m_memoryManager;

    // Objects used by publishing thread
    ChunkSenderData_t m_chunkSenderData{&m_memoryManager};
    ChunkSender<ChunkSenderData_t> m_chunkSender{&m_chunkSenderData};

    // Objects used by forwarding thread
    ChunkDistributorData_t m_chunkDistributorData;
    ChunkDistributor_t m_chunkDistributor{&m_chunkDistributorData};
    ChunkQueueData_t m_chunkQueueData{
        iox::cxx::VariantQueueTypes::FiFo_SingleProducerSingleConsumer}; // SoFi intentionally not used
    ChunkQueuePopper_t m_popper{&m_chunkQueueData};

    // Objects used by subscribing thread
    ChunkReceiverData_t m_chunkReceiverData{
        iox::cxx::VariantQueueTypes::FiFo_SingleProducerSingleConsumer}; // SoFi intentionally not used
    ChunkReceiver<ChunkReceiverData_t> m_chunkReceiver{&m_chunkReceiverData};
};

TEST_F(ChunkBuildingBlocks_IntegrationTest, TwoHopsThreeThreadsNoSoFi)
{
    std::thread subscribingThread(&ChunkBuildingBlocks_IntegrationTest::subscribe, this);
    std::thread forwardingThread(&ChunkBuildingBlocks_IntegrationTest::forward, this);
    std::thread publishingThread(&ChunkBuildingBlocks_IntegrationTest::publish, this);

    if (publishingThread.joinable())
    {
        publishingThread.join();
    }

    if (forwardingThread.joinable())
    {
        forwardingThread.join();
    }

    if (subscribingThread.joinable())
    {
        subscribingThread.join();
    }

    EXPECT_EQ(m_sendCounter, m_receiveCounter);
}
<commit_msg>iox-#408 another try to make ChunkBuildingBlocks_IntegrationTest more robust<commit_after>// Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "iceoryx_posh/internal/popo/building_blocks/chunk_distributor.hpp"
#include "iceoryx_posh/internal/popo/building_blocks/chunk_receiver.hpp"
#include "iceoryx_posh/internal/popo/building_blocks/chunk_sender.hpp"
#include "iceoryx_posh/internal/popo/building_blocks/locking_policy.hpp"
#include "iceoryx_posh/internal/popo/ports/base_port.hpp"
#include "iceoryx_posh/mepoo/mepoo_config.hpp"
#include "iceoryx_utils/cxx/generic_raii.hpp"

#include "test.hpp"

#include <chrono>
#include <stdlib.h>
#include <thread>

using namespace ::testing;
using namespace iox::popo;
using namespace iox::cxx;
using namespace iox::mepoo;
using namespace iox::posix;
using ::testing::Return;

struct DummySample
{
    uint64_t m_dummy{42};
};

static constexpr uint32_t NUM_CHUNKS_IN_POOL = 9 * iox::MAX_SUBSCRIBER_QUEUE_CAPACITY;
static constexpr uint32_t SMALL_CHUNK = 128;
static constexpr uint32_t CHUNK_META_INFO_SIZE = 256;
static constexpr size_t MEMORY_SIZE = NUM_CHUNKS_IN_POOL * (SMALL_CHUNK + CHUNK_META_INFO_SIZE);
alignas(64) static uint8_t g_memory[MEMORY_SIZE];
static constexpr uint32_t ITERATIONS = 10000;
static constexpr uint32_t MAX_NUMBER_QUEUES = 128;

struct ChunkDistributorConfig
{
    static constexpr uint32_t MAX_QUEUES = MAX_NUMBER_QUEUES;
    static constexpr uint64_t MAX_HISTORY_CAPACITY = iox::MAX_PUBLISHER_HISTORY;
};

struct ChunkQueueConfig
{
    static constexpr uint64_t MAX_QUEUE_CAPACITY = NUM_CHUNKS_IN_POOL / 3;
};

using ChunkQueueData_t = ChunkQueueData<ChunkQueueConfig, ThreadSafePolicy>;
using ChunkDistributorData_t =
    ChunkDistributorData<ChunkDistributorConfig, ThreadSafePolicy, ChunkQueuePusher<ChunkQueueData_t>>;
using ChunkDistributor_t = ChunkDistributor<ChunkDistributorData_t>;
using ChunkQueuePopper_t = ChunkQueuePopper<ChunkQueueData_t>;
using ChunkSenderData_t =
    ChunkSenderData<iox::MAX_CHUNKS_ALLOCATED_PER_PUBLISHER_SIMULTANEOUSLY, ChunkDistributorData_t>;
using ChunkReceiverData_t = ChunkReceiverData<iox::MAX_CHUNKS_HELD_PER_SUBSCRIBER_SIMULTANEOUSLY, ChunkQueueData_t>;

class ChunkBuildingBlocks_IntegrationTest : public Test
{
  public:
    ChunkBuildingBlocks_IntegrationTest()
    {
        m_mempoolConfig.addMemPool({SMALL_CHUNK, NUM_CHUNKS_IN_POOL});
        m_memoryManager.configureMemoryManager(m_mempoolConfig, &m_memoryAllocator, &m_memoryAllocator);
    }
    virtual ~ChunkBuildingBlocks_IntegrationTest()
    {
        /// @note One chunk is on hold due to the fact that chunkSender and chunkDistributor hold last chunk
        EXPECT_THAT(m_memoryManager.getMemPoolInfo(0).m_usedChunks, Eq(1));
    }

    void SetUp()
    {
        m_chunkSender.tryAddQueue(&m_chunkQueueData);
        m_chunkDistributor.tryAddQueue(&m_chunkReceiverData);
    }
    void TearDown(){};

    void publish()
    {
        for (size_t i = 0; i < ITERATIONS; i++)
        {
            m_chunkSender.tryAllocate(sizeof(DummySample), iox::UniquePortId())
                .and_then([&](auto chunkHeader) {
                    auto sample = chunkHeader->payload();
                    new (sample) DummySample();
                    static_cast<DummySample*>(sample)->m_dummy = i;
                    m_chunkSender.send(chunkHeader);
                    m_sendCounter++;
                })
                .or_else([](AllocationError) {
                    // Errors shall never occur
                    FAIL();
                });

            /// Add some jitter to make thread breathe
            std::this_thread::sleep_for(std::chrono::nanoseconds(rand() % 100));
        }
        // Signal the next threads we're done
        m_publisherRun = false;
    }

    void forward()
    {
        uint64_t forwardCounter{0};
        bool finished{false};
        // this is to prevent a race condition on thread shutdown; there must be two consecutive empty pops after the
        // publish thread finished
        bool newChunkReceivedInLastIteration{true};
        while (!finished)
        {
            ASSERT_FALSE(m_popper.hasOverflown());

            m_popper.tryPop()
                .and_then([&](auto& chunk) {
                    auto dummySample = *reinterpret_cast<DummySample*>(chunk.getPayload());
                    // Check if monotonically increasing
                    EXPECT_THAT(dummySample.m_dummy, Eq(forwardCounter));
                    m_chunkDistributor.deliverToAllStoredQueues(chunk);
                    forwardCounter++;
                    newChunkReceivedInLastIteration = true;
                })
                .or_else([&] {
                    if (!m_publisherRun.load(std::memory_order_relaxed))
                    {
                        if (newChunkReceivedInLastIteration)
                        {
                            newChunkReceivedInLastIteration = false;
                        }
                        else
                        {
                            finished = true;
                        }
                    }
                });
        }
        // Signal the next threads we're done
        m_forwarderRun = false;
    }

    void subscribe()
    {
        bool finished{false};
        // this is to prevent a race condition on thread shutdown; there must be two consecutive empty pops after the
        // forward thread finished
        bool newChunkReceivedInLastIteration{true};

        while (!finished)
        {
            ASSERT_FALSE(m_chunkReceiver.hasOverflown());

            m_chunkReceiver.tryGet()
                .and_then([&](iox::cxx::optional<const iox::mepoo::ChunkHeader*>& maybeChunkHeader) {
                    if (maybeChunkHeader.has_value())
                    {
                        auto chunkHeader = maybeChunkHeader.value();
                        auto dummySample = *reinterpret_cast<DummySample*>(chunkHeader->payload());
                        // Check if monotonically increasing
                        EXPECT_THAT(dummySample.m_dummy, Eq(m_receiveCounter));
                        m_receiveCounter++;
                        m_chunkReceiver.release(chunkHeader);
                        newChunkReceivedInLastIteration = true;
                    }
                    else if (!m_forwarderRun.load(std::memory_order_relaxed))
                    {
                        if (newChunkReceivedInLastIteration)
                        {
                            newChunkReceivedInLastIteration = false;
                        }
                        else
                        {
                            finished = true;
                        }
                    }
                })
                .or_else([](ChunkReceiveError) {
                    // Errors shall never occur
                    FAIL();
                });
        }
    }

    iox::cxx::GenericRAII m_uniqueRouDiId{[] { iox::popo::internal::setUniqueRouDiId(0); },
                                          [] { iox::popo::internal::unsetUniqueRouDiId(); }};
    uint64_t m_sendCounter{0};
    uint64_t m_receiveCounter{0};
    std::atomic<bool> m_publisherRun{true};
    std::atomic<bool> m_forwarderRun{true};

    // Memory objects
    Allocator m_memoryAllocator{g_memory, MEMORY_SIZE};
    MePooConfig m_mempoolConfig;
    MemoryManager m_memoryManager;

    // Objects used by publishing thread
    ChunkSenderData_t m_chunkSenderData{&m_memoryManager};
    ChunkSender<ChunkSenderData_t> m_chunkSender{&m_chunkSenderData};

    // Objects used by forwarding thread
    ChunkDistributorData_t m_chunkDistributorData;
    ChunkDistributor_t m_chunkDistributor{&m_chunkDistributorData};
    ChunkQueueData_t m_chunkQueueData{
        iox::cxx::VariantQueueTypes::FiFo_SingleProducerSingleConsumer}; // SoFi intentionally not used
    ChunkQueuePopper_t m_popper{&m_chunkQueueData};

    // Objects used by subscribing thread
    ChunkReceiverData_t m_chunkReceiverData{
        iox::cxx::VariantQueueTypes::FiFo_SingleProducerSingleConsumer}; // SoFi intentionally not used
    ChunkReceiver<ChunkReceiverData_t> m_chunkReceiver{&m_chunkReceiverData};
};

TEST_F(ChunkBuildingBlocks_IntegrationTest, TwoHopsThreeThreadsNoSoFi)
{
    std::thread subscribingThread(&ChunkBuildingBlocks_IntegrationTest::subscribe, this);
    std::thread forwardingThread(&ChunkBuildingBlocks_IntegrationTest::forward, this);
    std::thread publishingThread(&ChunkBuildingBlocks_IntegrationTest::publish, this);

    if (publishingThread.joinable())
    {
        publishingThread.join();
    }

    if (forwardingThread.joinable())
    {
        forwardingThread.join();
    }

    if (subscribingThread.joinable())
    {
        subscribingThread.join();
    }

    EXPECT_EQ(m_sendCounter, m_receiveCounter);
}
<|endoftext|>