/**
 * Predicate to check expected distance between two segments
 * @param  aSegA the first #SEG
 * @param  aSegB the second #SEG
 * @param  aExp  expected distance
 * @return       does the distance calculated agree?
 */
bool SegDistanceCorrect( const SEG& aSegA, const SEG& aSegB, int aExp )
{
    const int AtoB = aSegA.Distance( aSegB );
    const int BtoA = aSegB.Distance( aSegA );

    bool ok = ( AtoB == aExp ) && ( BtoA == aExp );

    if( AtoB != BtoA )
    {
        std::stringstream ss;
        ss << "Segment distance is not the same in both directions: expected " << aExp << ", got "
           << AtoB << " & " << BtoA;
        BOOST_TEST_INFO( ss.str() );
    }
    else if( !ok )
    {
        std::stringstream ss;
        ss << "Distance incorrect: expected " << aExp << ", got " << AtoB;
        BOOST_TEST_INFO( ss.str() );
    }

    // Sanity check: the collision should be consistent with the distance
    ok = ok && SegCollideCorrect( aSegA, aSegB, 0, aExp == 0 );

    return ok;
}
bool SHAPE_ARC::Collide( const SEG& aSeg, int aClearance ) const
{
    int minDist = aClearance + m_width / 2;
    auto centerDist = aSeg.Distance( m_pc );
    auto p1 = GetP1();

    if( centerDist < minDist )
        return true;

    auto ab = (aSeg.B - aSeg.A );
    auto ac = ( m_pc - aSeg.A );

    auto lenAbSq = ab.SquaredEuclideanNorm();

    auto lambda = (double) ac.Dot( ab ) / (double) lenAbSq;


    if( lambda >= 0.0 && lambda <= 1.0 )
    {
        VECTOR2I p;

        p.x = (double) aSeg.A.x * lambda + (double) aSeg.B.x * (1.0 - lambda);
        p.y = (double) aSeg.A.y * lambda + (double) aSeg.B.y * (1.0 - lambda);

        auto p0pdist = ( m_p0 - p ).EuclideanNorm();

        if( p0pdist < minDist )
            return true;

        auto p1pdist = ( p1 - p ).EuclideanNorm();

        if( p1pdist < minDist )
            return true;
    }

    auto p0dist = aSeg.Distance( m_p0 );

    if( p0dist > minDist )
        return true;

    auto p1dist = aSeg.Distance( p1 );

    if( p1dist > minDist )
        return false;


    return true;
}
static VECTOR2I pushoutForce( const SHAPE_CIRCLE& aA, const SEG& aB, int aClearance )
{
    VECTOR2I f( 0, 0 );

    const VECTOR2I c = aA.GetCenter();
    const VECTOR2I nearest = aB.NearestPoint( c );

    const int r = aA.GetRadius();

    int dist = ( nearest - c ).EuclideanNorm();
    int min_dist = aClearance + r;

    if( dist < min_dist )
    {
        for( int corr = 0; corr < 5; corr++ )
        {
            f = ( aA.GetCenter() - nearest ).Resize( min_dist - dist + corr );

            if( aB.Distance( c + f ) >= min_dist )
                break;
        }
    }

    return f;
}
int SHAPE_LINE_CHAIN::Split( const VECTOR2I& aP )
{
    int ii = -1;
    int min_dist = 2;

    ii = Find( aP );

    if( ii >= 0 )
        return ii;

    for( int s = 0; s < SegmentCount(); s++ )
    {
        const SEG seg = CSegment( s );
        int dist = seg.Distance( aP );

        // make sure we are not producing a 'slightly concave' primitive. This might happen
        // if aP lies very close to one of already existing points.
        if( dist < min_dist && seg.A != aP && seg.B != aP )
        {
            min_dist = dist;
            ii = s;
        }
    }

    if( ii >= 0 )
    {
        m_points.insert( m_points.begin() + ii + 1, aP );

        return ii + 1;
    }

    return -1;
}
/**
 * Predicate to check expected distance between a segment and a point
 * @param  aSegA the segment
 * @param  aVec  the vector (point)
 * @param  aExp  expected distance
 * @return       does the distance calculated agree?
 */
bool SegVecDistanceCorrect( const SEG& aSeg, const VECTOR2I& aVec, int aExp )
{
    const int dist = aSeg.Distance( aVec );

    bool ok = ( dist == aExp );

    if( !ok )
    {
        std::stringstream ss;
        ss << "Distance incorrect: expected " << aExp << ", got " << dist;
        BOOST_TEST_INFO( ss.str() );
    }

    return ok;
}
bool SHAPE_LINE_CHAIN::PointOnEdge( const VECTOR2I& aP ) const
{
    if( SegmentCount() < 1 )
        return m_points[0] == aP;

    for( int i = 1; i < SegmentCount(); i++ )
    {
        const SEG s = CSegment( i );

        if( s.A == aP || s.B == aP )
            return true;

        if( s.Distance( aP ) <= 1 )
            return true;
    }

    return false;
}
int SHAPE_LINE_CHAIN::PathLength( const VECTOR2I& aP ) const
{
    int sum = 0;

    for( int i = 0; i < SegmentCount(); i++ )
    {
        const SEG seg = CSegment( i );
        int d = seg.Distance( aP );

        if( d <= 1 )
        {
            sum += ( aP - seg.A ).EuclideanNorm();
            return sum;
        }
        else
            sum += seg.Length();
    }

    return -1;
}
bool SHAPE_LINE_CHAIN::CheckClearance( const VECTOR2I& aP, const int aDist) const
{
    if( !PointCount() )
        return false;

    else if( PointCount() == 1 )
        return m_points[0] == aP;

    for( int i = 0; i < SegmentCount(); i++ )
    {
        const SEG s = CSegment( i );

        if( s.A == aP || s.B == aP )
            return true;

        if( s.Distance( aP ) <= aDist )
            return true;
    }

    return false;
}
int SHAPE_LINE_CHAIN::EdgeContainingPoint( const VECTOR2I& aPt, int aAccuracy ) const
{
    if( !PointCount() )
		return -1;

	else if( PointCount() == 1 )
    {
	    VECTOR2I dist = m_points[0] - aPt;
	    return ( hypot( dist.x, dist.y ) <= aAccuracy + 1 ) ? 0 : -1;
    }

    for( int i = 0; i < SegmentCount(); i++ )
    {
        const SEG s = CSegment( i );

        if( s.A == aPt || s.B == aPt )
            return i;

        if( s.Distance( aPt ) <= aAccuracy + 1 )
            return i;
    }

    return -1;
}