// math for graphics utility routines and RC, from FreePCB

#include <vector>

#include <cmath>
#include <float.h>
#include <limits.h>

#include <fctsys.h>

#include <PolyLine.h>
#include <math_for_graphics.h>

static bool InRange( double x, double xi, double xf );

double Distance( double x1, double y1, double x2, double y2 )
{
    return hypot( x1 - x2, y1 - y2 );
}

/**
 * Function TestLineHit
 * test for hit on line segment i.e. a point within a given distance from segment
 * @param x, y = cursor coords
 * @param xi,yi,xf,yf = the end-points of the line segment
 * @param dist = maximum distance for hit
 * return true if dist < distance between the point and the segment
 */
bool TestLineHit( int xi, int yi, int xf, int yf, int x, int y, double dist )
{
    double dd;

    // test for vertical or horizontal segment
    if( xf==xi )
    {
        // vertical segment
        dd = fabs( (double) (x - xi) );

        if( dd<dist && ( (yf>yi && y<yf && y>yi) || (yf<yi && y>yf && y<yi) ) )
            return true;
    }
    else if( yf==yi )
    {
        // horizontal segment
        dd = fabs( (double) (y - yi) );

        if( dd<dist && ( (xf>xi && x<xf && x>xi) || (xf<xi && x>xf && x<xi) ) )
            return true;
    }
    else
    {
        // oblique segment
        // find a,b such that (xi,yi) and (xf,yf) lie on y = a + bx
        double  b   = (double) (yf - yi) / (xf - xi);
        double  a   = (double) yi - b * xi;
        // find c,d such that (x,y) lies on y = c + dx where d=(-1/b)
        double  d   = -1.0 / b;
        double  c   = (double) y - d * x;
        // find nearest point to (x,y) on line segment (xi,yi) to (xf,yf)
        double  xp  = (a - c) / (d - b);
        double  yp  = a + b * xp;
        // find distance
        dd = sqrt( (x - xp) * (x - xp) + (y - yp) * (y - yp) );

        if( fabs( b )>0.7 )
        {
            // line segment more vertical than horizontal
            if( dd<dist && ( (yf>yi && yp<yf && yp>yi) || (yf<yi && yp>yf && yp<yi) ) )
                return 1;
        }
        else
        {
            // line segment more horizontal than vertical
            if( dd<dist && ( (xf>xi && xp<xf && xp>xi) || (xf<xi && xp>xf && xp<xi) ) )
                return true;
        }
    }

    return false;    // no hit
}


/* Function FindSegmentIntersections
 * find intersections between line segment (xi,yi) to (xf,yf)
 * and line segment (xi2,yi2) to (xf2,yf2)
 * returns true if intersection found
 */
bool FindSegmentIntersections( int xi, int yi, int xf, int yf,
                              int xi2, int yi2, int xf2, int yf2  )
{
    if( std::max( xi, xf ) < std::min( xi2, xf2 )
        || std::min( xi, xf ) > std::max( xi2, xf2 )
        || std::max( yi, yf ) < std::min( yi2, yf2 )
        || std::min( yi, yf ) > std::max( yi2, yf2 ) )
        return false;

    return TestForIntersectionOfStraightLineSegments( xi, yi, xf, yf,
                                                      xi2, yi2, xf2, yf2 );
}


/* Function FindLineSegmentIntersection
 * find intersection between line y = a + bx and line segment (xi,yi) to (xf,yf)
 * if b > DBL_MAX/10, assume vertical line at x = a
 * return false if no intersection or true if intersect
 * return coords of intersections in *x1, *y1, *x2, *y2
 * if no intersection, returns min distance in dist
 */
bool FindLineSegmentIntersection( double a, double b, int xi, int yi, int xf, int yf,
                                 double* x1, double* y1, double* x2, double* y2,
                                 double* dist )
{
    double  xx = 0, yy = 0; // Init made to avoid C compil "uninitialized" warning
    bool    bVert = false;

    if( b > DBL_MAX / 10.0 )
        bVert = true;

    if( xf != xi )      // non-vertical segment, get intersection
    {
        // horizontal or oblique straight segment
        // put into form y = c + dx;
        double  d   = (double) (yf - yi) / (double) (xf - xi);
        double  c   = yf - d * xf;

        if( bVert )
        {
            // if vertical line, easy
            if( InRange( a, xi, xf ) )
            {
                *x1 = a;
                *y1 = c + d * a;
                return 1;
            }
            else
            {
                if( dist )
                    *dist = std::min( abs( a - xi ), abs( a - xf ) );

                return false;
            }
        }

        if( fabs( b - d ) < 1E-12 )
        {
            // parallel lines
            if( dist )
            {
                *dist = GetPointToLineDistance( a, b, xi, xf );
            }

            return false;    // lines parallel
        }

        // calculate intersection
        xx  = (c - a) / (b - d);
        yy  = a + b * (xx);

        // see if intersection is within the line segment
        if( yf == yi )
        {
            // horizontal line
            if( (xx>=xi && xx>xf) || (xx<=xi && xx<xf) )
                return false;
        }
        else
        {
            // oblique line
            if( (xx>=xi && xx>xf) || (xx<=xi && xx<xf)
                || (yy>yi && yy>yf) || (yy<yi && yy<yf) )
                return false;
        }
    }
    else
    {
        // vertical line segment
        if( bVert )
            return false;

        xx  = xi;
        yy  = a + b * xx;

        if( (yy>=yi && yy>yf) || (yy<=yi && yy<yf) )
            return 0;
    }

    *x1 = xx;
    *y1 = yy;
    return true;
}


/*
 * Function TestForIntersectionOfStraightLineSegments
 * Test for intersection of line segments
 * If lines are parallel, returns false
 * If true, returns also intersection coords in x, y
 * if false, returns min. distance in dist (may be 0.0 if parallel)
 */
bool TestForIntersectionOfStraightLineSegments( int x1i, int y1i, int x1f, int y1f,
                                                int x2i, int y2i, int x2f, int y2f,
                                                int* x, int* y, double* d )
{
    double a, b, dist;

    // first, test for intersection
    if( x1i == x1f && x2i == x2f )
    {
        // both segments are vertical, can't intersect
    }
    else if( y1i == y1f && y2i == y2f )
    {
        // both segments are horizontal, can't intersect
    }
    else if( x1i == x1f && y2i == y2f )
    {
        // first seg. vertical, second horizontal, see if they cross
        if( InRange( x1i, x2i, x2f )
            && InRange( y2i, y1i, y1f ) )
        {
            if( x )
                *x = x1i;

            if( y )
                *y = y2i;

            if( d )
                *d = 0.0;

            return true;
        }
    }
    else if( y1i == y1f && x2i == x2f )
    {
        // first seg. horizontal, second vertical, see if they cross
        if( InRange( y1i, y2i, y2f )
            && InRange( x2i, x1i, x1f ) )
        {
            if( x )
                *x = x2i;

            if( y )
                *y = y1i;

            if( d )
                *d = 0.0;

            return true;
        }
    }
    else if( x1i == x1f )
    {
        // first segment vertical, second oblique
        // get a and b for second line segment, so that y = a + bx;
        b   = double( y2f - y2i ) / (x2f - x2i);
        a   = (double) y2i - b * x2i;

        double  x1, y1, x2, y2;
        int     test = FindLineSegmentIntersection( a, b, x1i, y1i, x1f, y1f,
                                                    &x1, &y1, &x2, &y2 );

        if( test )
        {
            if( InRange( y1, y1i, y1f ) && InRange( x1, x2i, x2f ) && InRange( y1, y2i, y2f ) )
            {
                if( x )
                    *x = (int) x1;

                if( y )
                    *y = (int) y1;

                if( d )
                    *d = 0.0;

                return true;
            }
        }
    }
    else if( y1i == y1f )
    {
        // first segment horizontal, second oblique
        // get a and b for second line segment, so that y = a + bx;
        b   = double( y2f - y2i ) / (x2f - x2i);
        a   = (double) y2i - b * x2i;

        double  x1, y1, x2, y2;
        int     test = FindLineSegmentIntersection( a, b, x1i, y1i, x1f, y1f,
                                                    &x1, &y1, &x2, &y2 );

        if( test )
        {
            if( InRange( x1, x1i, x1f ) && InRange( x1, x2i, x2f ) && InRange( y1, y2i, y2f ) )
            {
                if( x )
                    *x = (int) x1;

                if( y )
                    *y = (int) y1;

                if( d )
                    *d = 0.0;

                return true;
            }
        }
    }
    else if( x2i == x2f )
    {
        // second segment vertical, first oblique
        // get a and b for first line segment, so that y = a + bx;
        b   = double( y1f - y1i ) / (x1f - x1i);
        a   = (double) y1i - b * x1i;

        double  x1, y1, x2, y2;
        int     test = FindLineSegmentIntersection( a, b, x2i, y2i, x2f, y2f,
                                                    &x1, &y1, &x2, &y2 );

        if( test )
        {
            if( InRange( x1, x1i, x1f ) &&  InRange( y1, y1i, y1f ) && InRange( y1, y2i, y2f ) )
            {
                if( x )
                    *x = (int) x1;

                if( y )
                    *y = (int) y1;

                if( d )
                    *d = 0.0;

                return true;
            }
        }
    }
    else if( y2i == y2f )
    {
        // second segment horizontal, first oblique
        // get a and b for second line segment, so that y = a + bx;
        b   = double( y1f - y1i ) / (x1f - x1i);
        a   = (double) y1i - b * x1i;

        double  x1, y1, x2, y2;
        int     test = FindLineSegmentIntersection( a, b, x2i, y2i, x2f, y2f,
                                                    &x1, &y1, &x2, &y2 );

        if( test )
        {
            if( InRange( x1, x1i, x1f ) && InRange( y1, y1i, y1f ) )
            {
                if( x )
                    *x = (int) x1;

                if( y )
                    *y = (int) y1;

                if( d )
                    *d = 0.0;

                return true;
            }
        }
    }
    else
    {
        // both segments oblique
        if( long( y1f - y1i ) * (x2f - x2i) != long( y2f - y2i ) * (x1f - x1i) )
        {
            // not parallel, get a and b for first line segment, so that y = a + bx;
            b   = double( y1f - y1i ) / (x1f - x1i);
            a   = (double) y1i - b * x1i;

            double  x1, y1, x2, y2;
            int     test = FindLineSegmentIntersection( a, b, x2i, y2i, x2f, y2f,
                                                        &x1, &y1, &x2, &y2 );

            // both segments oblique
            if( test )
            {
                if( InRange( x1, x1i, x1f ) && InRange( y1, y1i, y1f ) )
                {
                    if( x )
                        *x = (int) x1;

                    if( y )
                        *y = (int) y1;

                    if( d )
                        *d = 0.0;

                    return true;
                }
            }
        }
    }

    // don't intersect, get shortest distance between each endpoint and the other line segment
    dist = GetPointToLineSegmentDistance( x1i, y1i, x2i, y2i, x2f, y2f );

    double  xx  = x1i;
    double  yy  = y1i;
    double  dd  = GetPointToLineSegmentDistance( x1f, y1f, x2i, y2i, x2f, y2f );

    if( dd < dist )
    {
        dist    = dd;
        xx      = x1f;
        yy      = y1f;
    }

    dd = GetPointToLineSegmentDistance( x2i, y2i, x1i, y1i, x1f, y1f );

    if( dd < dist )
    {
        dist    = dd;
        xx      = x2i;
        yy      = y2i;
    }

    dd = GetPointToLineSegmentDistance( x2f, y2f, x1i, y1i, x1f, y1f );

    if( dd < dist )
    {
        dist    = dd;
        xx      = x2f;
        yy      = y2f;
    }

    if( x )
        *x = (int) xx;

    if( y )
        *y = (int) yy;

    if( d )
        *d = dist;

    return false;
}


/* Function GetClearanceBetweenSegments
 * Get clearance between 2 segments
 * Returns coordinates of the closest point between these 2 segments in x, y
 * If clearance > max_cl, just returns max_cl+1 and doesn't return x,y
 */
int GetClearanceBetweenSegments( int x1i, int y1i, int x1f, int y1f, int w1,
                                 int x2i, int y2i, int x2f, int y2f, int w2,
                                 int max_cl, int* x, int* y )
{
    // check clearance between bounding rectangles
    int min_dist = max_cl + ( (w1 + w2) / 2 );

    if( std::min( x1i, x1f ) - std::max( x2i, x2f ) > min_dist )
        return max_cl+1;

    if( std::min( x2i, x2f ) - std::max( x1i, x1f ) > min_dist )
        return max_cl+1;

    if( std::min( y1i, y1f ) - std::max( y2i, y2f ) > min_dist )
        return max_cl+1;

    if( std::min( y2i, y2f ) - std::max( y1i, y1f ) > min_dist )
        return max_cl+1;

    int     xx, yy;
    double  dist;
    TestForIntersectionOfStraightLineSegments( x1i, y1i, x1f, y1f,
                                               x2i, y2i, x2f, y2f, &xx, &yy, &dist );
    int d = (int) dist - ( (w1 + w2) / 2 );
    if( d < 0 )
        d = 0;

    if( x )
        *x = xx;

    if( y )
        *y = yy;

    return d;
}


/* Function GetPointToLineDistance
 * Get min. distance from (x,y) to line y = a + bx
 * if b > DBL_MAX/10, assume vertical line at x = a
 * returns closest point on line in xpp, ypp
 */
double GetPointToLineDistance( double a, double b, int x, int y, double* xpp, double* ypp )
{
    if( b > DBL_MAX / 10 )
    {
        // vertical line
        if( xpp && ypp )
        {
            *xpp    = a;
            *ypp    = y;
        }

        return abs( a - x );
    }

    // find c,d such that (x,y) lies on y = c + dx where d=(-1/b)
    double  d   = -1.0 / b;
    double  c   = (double) y - d * x;

    // find nearest point to (x,y) on line through (xi,yi) to (xf,yf)
    double  xp  = (a - c) / (d - b);
    double  yp  = a + b * xp;

    if( xpp && ypp )
    {
        *xpp    = xp;
        *ypp    = yp;
    }

    // find distance
    return Distance( x, y, xp, yp );
}


/**
 * Function GetPointToLineSegmentDistance
 * Get distance between line segment and point
 * @param x,y = point
 * @param xi,yi Start point of the line segament
 * @param xf,yf End point of the line segment
 * @return the distance
 */
double GetPointToLineSegmentDistance( int x, int y, int xi, int yi, int xf, int yf )
{
    // test for vertical or horizontal segment
    if( xf==xi )
    {
        // vertical line segment
        if( InRange( y, yi, yf ) )
            return abs( x - xi );
        else
            return std::min( Distance( x, y, xi, yi ), Distance( x, y, xf, yf ) );
    }
    else if( yf==yi )
    {
        // horizontal line segment
        if( InRange( x, xi, xf ) )
            return abs( y - yi );
        else
            return std::min( Distance( x, y, xi, yi ), Distance( x, y, xf, yf ) );
    }
    else
    {
        // oblique segment
        // find a,b such that (xi,yi) and (xf,yf) lie on y = a + bx
        double  b   = (double) (yf - yi) / (xf - xi);
        double  a   = (double) yi - b * xi;

        // find c,d such that (x,y) lies on y = c + dx where d=(-1/b)
        double  d   = -1.0 / b;
        double  c   = (double) y - d * x;

        // find nearest point to (x,y) on line through (xi,yi) to (xf,yf)
        double  xp  = (a - c) / (d - b);
        double  yp  = a + b * xp;

        // find distance
        if( InRange( xp, xi, xf ) && InRange( yp, yi, yf ) )
            return Distance( x, y, xp, yp );
        else
            return std::min( Distance( x, y, xi, yi ), Distance( x, y, xf, yf ) );
    }
}


// test for value within range
bool InRange( double x, double xi, double xf )
{
    if( xf > xi )
    {
        if( x >= xi && x <= xf )
            return true;
    }
    else
    {
        if( x >= xf && x <= xi )
            return true;
    }

    return false;
}