/* Standard includes */

#include <math.h>

/* psLib includes */

#include "psMemory.h"

/* streakremove includes */

#include "Line.h"

/** Internal routine to swap double values */

void SwapDouble (double* first, double* second)
{
    double temp = *first;
    *first = *second;
    *second = temp;
}

/** Internal routine to swap integer values */

void SwapInt (int* first, int* second)
{
    int temp = *first;
    *first = *second;
    *second = temp;
}

/** Return if a point is inside the current line segment assuming the
    point and line are colinear                                         */

bool InLineSegment (Line* line, strkPt* tuple)
{
    // If the current line is not vertical, check to see if the point
    // is inside the two endpoints independent of direction
    
    if (line->begin.x != line->end.x)
    {
        return (tuple->x >= line->begin.x &&
                tuple->x <= line->end.x) ||
               (tuple->x >= line->end.x &&
                tuple->x <= line->begin.x);
    }
    return (tuple->y >= line->begin.y &&
            tuple->y <= line->end.y) ||
           (tuple->y >= line->end.y &&
            tuple->y <= line->begin.y);
}

double DistanceSquared (Line* line, double x, double y)
{
    // Define U as the vector from the line segment start to end
    // Define V as the vector from the line segment start to the tuple
    // Define W as the vector from the line segment end to the tuple

    double ux, uy, vx, vy, b, px, py;
    ux = line->end.x - line->begin.x;
    uy = line->end.y - line->begin.y;
    
    vx = x - line->begin.x;
    vy = y - line->begin.y;
    
    double u_u = ux * ux + uy * uy;                         // (u . u)
    double u_v = ux * vx + uy * vy;                         // (u . v)
    
    if (u_v <= 0) return vx * vx + vy * vy;                 // (v . v)
    if (u_u <= u_v)
    {
        double wx =  x - line->end.x;
        double wy =  y - line->end.y;
        return wx * wx + wy * wy;                           // (w . w)
    }
    
    // Compute P(b) is the base of the perpendicular dropped from tuple to
    // the line
    
    b = u_v / u_u;
    px = vx - b * ux;
    py = vy - b * uy;
    return px * px + py * py;                               // norm (p)
}

/** Return true if two lines intersect within their limits and in
    all cases including coplanar lines, set one or two points of
    intersection

    @param[in] first first line to compare for intercept
    @param[in] second second line to compare for intercept
    @param[out] tuple1 first intercept point if found
    @param[out] tuple2 second intercept point if lines are coplanar
    @param[in] inFirstSegment true if the line intercept must be within
                              the bounds of the first line. False if the
                              first line should be considered as infinite
    @param[in] inSecondSegment true if the line intercept must be within
                               the bounds of the first line. False if the
                               second line should be considered as infinite
    @return the number of intercept points                                  */

int LineIntercept (Line* first, Line* second, strkPt* tuple1, strkPt* tuple2,
                   bool inFirstSegment, bool inSecondSegment)
{
    double len1, len2, w2x, w2y, t0, t1;
    double dx1 = first->end.x - first->begin.x;
    double dy1 = first->end.y - first->begin.y;
    double dx2 = second->end.x - second->begin.x;
    double dy2 = second->end.y - second->begin.y;
    double wx  = first->begin.x - second->begin.x;
    double wy  = first->begin.y - second->begin.y;
    
    // Calculate the perpendicular product, dt
    
    double dt = dx1 * dy2 - dy1 * dx2;
    
    // Are the lines parallel?
    
    if (fabs (dt) <= 1e-8)
    {
        // Check to see if they are overlap
        
        if (dx1 * wy - dy1 * wx != 0 || dx2 * wy - dy2 * wx != 0) return 0;
        
        // The line are coplanar, so check to see if they are degenerate
        
        len1 = dx1 * dx1 + dy1 * dy1;
        len2 = dx2 * dx2 + dy2 * dy2;
        
        // First, check to see if both lines are points and if they are
        // distinct tuples
        
        if (len1 == 0 && len2 == 0)
        {
            if (first->begin.x != second->begin.x ||
                first->begin.y != second->begin.y) return 0;
            *tuple1 = first->begin;
            return 1;
        }
        
        // Check to see if the first line is a point and is inside the
        // second line
        
        if (len1 == 0)
        {
            if (InLineSegment (second, &first->begin))
                *tuple1 = first->begin;
            else
                return 0;
        }
        
        // Check to see if the second line is a point and is inside the
        // first line
        
        if (len2 == 0)
        {
            if (InLineSegment (first, &second->begin))
                *tuple1 = second->begin;
            else
                return 0;
        }
        
        // Since both lines are coplanar and have length, search for
        // overlap tuples
        
        w2x = first->end.x - second->begin.x;
        w2y = first->end.y - second->begin.y;
        
        t0 = (dx2 != 0) ? wx  / dx2 : wy  / dy2;
        t1 = (dx2 != 0) ? w2x / dx2 : w2y / dy2;
        
        if (t0 > t1) SwapDouble (&t0, &t1);
        if ((inFirstSegment || inSecondSegment) && (t0 > 1 || t1 < 0))
        {
            return 0;                       // No overlap
        }
        t0 = (t0 < 0) ? 0 : t0;             // Clip to min 0
        t1 = (t1 > 1) ? 1 : t1;             // Clip to max 1
        
        // Set the first tuple of intersection
        
        tuple1->x = second->begin.x + t0 * dx2;
        tuple1->y = second->begin.y + t0 * dy2;
        
        // Check to see if the intersection is a single tuple
        
        if (t0 == t1) return 1;
        
        // Otherwise, set the second tuple of intersection as well
        
        tuple2->x = second->begin.x + t1 * dx2;
        tuple2->y = second->begin.y + t1 * dy2;
        return 2;
    }
    
    // The segments are skew and may intersect in a tuple.
    // Get the intersect parameter for the first line
    
    double i1 = (dx2 * wy - dy2 * wx) / dt;
    if (inFirstSegment && (i1 < 0 || i1 > 1)) return 0;
    
    // Get the intersect parameter for the second line

    double i2 = (dx1 * wy - dy1 * wx) / dt;
    if (inSecondSegment && (i2 < 0 || i2 > 1)) return 0;

    tuple1->x = first->begin.x + i1 * dx1;
    tuple1->y = first->begin.y + i1 * dy1;
    return 1;
}

/** Clip the line between (0,0) and the specified upper bound

    @param[in,out] line line to be clipped within the bounds
    @param[in] numCols maximum X (columns) for the line
    @param[in] numRows maximum Y (rows) for the line
    @return true if line overlaps the clip boundaries           */

bool LineClip (Line *line, int numCols, int numRows)
{
    unsigned int i, found = 0;
    Line boundLine, clipLine;
    strkPt tuple1, tuple2, vertices[4];
    vertices[0].x = 0;       vertices[0].y = 0;
    vertices[1].x = numCols; vertices[1].y = 0;
    vertices[2].x = numCols; vertices[2].y = numRows;
    vertices[3].x = 0;       vertices[3].y = numRows;

    for (i = 0; i < 4 && found < 2; ++i)
    {
        boundLine.begin = vertices[i];
        boundLine.end   = vertices[(i + 1) % 4];
        if (LineIntercept (line, &boundLine, &tuple1, &tuple2, false, true))
        {
            if (found == 0)
            {
                clipLine.begin = tuple1;
                ++found;
            }
            else if (tuple1.x != clipLine.begin.x || 
                     tuple1.y != clipLine.begin.y)
            {
                clipLine.end = tuple1;
                ++found;
            }
        }
    }
    
    // If two endpoints are found, clip the line
    
    if (found > 1)
    {
        if (clipLine.begin.x <= clipLine.end.x)
        {
            line->begin = clipLine.begin;
            line->end   = clipLine.end;
        }
        else
        {
            line->begin = clipLine.end;
            line->end   = clipLine.begin;
        }
    }
    return found > 1;
}

/** Clip the line between (minX,minY) and (maxX,maxY)

    @param[in,out] line line to be clipped within the bounds
    @param[in] minX minimum X (columns) for the line
    @param[in] minY minimum Y (rows) for the line
    @param[in] maxX maximum X (columns) for the line
    @param[in] maxY maximum Y (rows) for the line
    @return true if line overlaps the clip boundaries           */

bool LineClipFull (Line *line, int minX, int minY, int maxX, int maxY)
{
    unsigned int i, found = 0;
    Line boundLine, clipLine;
    strkPt tuple1, tuple2, vertices[4];
    vertices[0].x = minX; vertices[0].y = minY;
    vertices[1].x = maxX; vertices[1].y = minY;
    vertices[2].x = maxX; vertices[2].y = maxY;
    vertices[3].x = minX; vertices[3].y = maxY;

    for (i = 0; i < 4 && found < 2; ++i)
    {
        boundLine.begin = vertices[i];
        boundLine.end   = vertices[(i + 1) % 4];
        if (LineIntercept (line, &boundLine, &tuple1, &tuple2, false, true))
        {
            if (found == 0)
            {
                clipLine.begin = tuple1;
                ++found;
            }
            else if (tuple1.x != clipLine.begin.x || 
                     tuple1.y != clipLine.begin.y)
            {
                clipLine.end = tuple1;
                ++found;
            }
        }
    }
    
    // If two endpoints are found, clip the line
    
    if (found > 1)
    {
        if (clipLine.begin.x <= clipLine.end.x)
        {
            line->begin = clipLine.begin;
            line->end   = clipLine.end;
        }
        else
        {
            line->begin = clipLine.end;
            line->end   = clipLine.begin;
        }
    }
    return found > 1;
}

/** Move a line by the specified X and Y offsets
    @param[in,out] line Line to move by X and Y offsets
    @param[in] xOffset X shift applied to both endpoints
    @param[in] yOffset Y shift applied to both endpoints        */

void LineMove (Line *line, double xOffset, double yOffset)
{
    line->begin.x += xOffset;
    line->begin.y += yOffset;
    line->end.x   += xOffset;
    line->end.y   += yOffset;
}

/** Return the maximum bounds between the line endpoints and
    current bounds
    @param[in] line Line endpoints to compare
    @param[in,out] xMin minimum X value to update
    @param[in,out] xMax maximum X value to update
    @param[in,out] yMin minimum Y value to update
    @param[in,out] yMax maximum Y value to update               */

void MaxBounds (Line *line, int *xMin, int *xMax,  int *yMin, int *yMax)
{
    if (line->begin.x < *xMin) *xMin = (int) floor (line->begin.x);
    if (line->end.x   < *xMin) *xMin = (int) floor (line->end.x);
    if (line->begin.y < *yMin) *yMin = (int) floor (line->begin.y);
    if (line->end.y   < *yMin) *yMin = (int) floor (line->end.y);

    if (line->begin.x > *xMax) *xMax = (int) ceil (line->begin.x);
    if (line->end.x   > *xMax) *xMax = (int) ceil (line->end.x);
    if (line->begin.y > *yMax) *yMax = (int) ceil (line->begin.y);
    if (line->end.y   > *yMax) *yMax = (int) ceil (line->end.y);
}

/** Map a line to an image for its specified width and store as
    a list of pixel positions

    @param[out] pixels list of PixelPos pointers corresponding
                       based on the line settings
    @param[in] line Line to map to pixels   
    @param[in] numCols maximum X (columns) for the line
    @param[in] numRows maximum Y (rows) for the line            */

void PixelsFromLine (StreakPixels* pixels, Line *line, int numCols, int numRows)
{
    Line offsetLine;
    PixelPos *pixel;
    double slope, xOffset, yOffset, xMid, yMid;
    int x, y, xBegin = numCols, yBegin = numRows, xEnd = 0, yEnd = 0;

    // Extract the endpoints
    
    double x1 = line->begin.x;
    double y1 = line->begin.y;
    double x2 = line->end.x;
    double y2 = line->end.y;
    
    // Determine the width and height of the line
    
    double dx = x2 - x1;
    double dy = y2 - y1;
    double dr = sqrt (dx * dx + dy * dy);
    double halfWidth  = line->width / 2.0;
    if (!dr) return;

    // Compute the intercepts of line width bounds and determine maximum
    // bounds in each axis

    xOffset = -halfWidth * dy / dr;
    yOffset =  halfWidth * dx / dr;

    offsetLine = *line;
    LineMove (&offsetLine, xOffset, yOffset);
    if (LineClip (&offsetLine, numCols, numRows))
        MaxBounds (&offsetLine, &xBegin, &xEnd, &yBegin, &yEnd);

    offsetLine = *line;
    LineMove (&offsetLine, -xOffset, -yOffset);
    if (LineClip (&offsetLine, numCols, numRows))
        MaxBounds (&offsetLine, &xBegin, &xEnd, &yBegin, &yEnd);

    // Step point by point based on the dominate axis
    
    if (fabs (dx) > fabs (dy))
    {
        // ---------------------
        // horizontal(ish) lines
        // ---------------------
        // If line is back to front, reorder endpoints and recompute
        // the line width and height
        
        if (x1 > x2)
        {
            SwapDouble (&x1, &x2);
            SwapDouble (&y1, &y2);
            dx = x2 - x1;
            dy = y2 - y1;
        }

        // Compute the slope of the line
        
        slope = dy / dx;
        
        // Compute the x and y offsets for the line width extent

        if (xBegin > xEnd) 
            SwapInt (&xBegin, &xEnd);
        else
            ++xEnd;
        if (xBegin < 0) xBegin = 0;
        if (xEnd > numCols) xEnd = numCols;

        yMid = y1 + slope * (xBegin - x1);
        yOffset = fabs (halfWidth * dr / dx);

        for (x = xBegin; x != xEnd; ++x)
        {
            yBegin = (int) floor (yMid - yOffset);
            yEnd   = (int) ceil  (yMid + yOffset) + 1;
            for (y = yBegin; y != yEnd; ++y)
            {
                if (y >= 0 && y < numRows)
                {
                    psImageSet(pixels, x, y, 1);
                }
            }
            yMid += slope;
        }
    }
    else
    {       
        // -------------------
        // vertical(ish) lines
        // -------------------
        // If line is back to front, reorder endpoints and recompute
        // the line width and height
        
        if (y1 > y2)
        {
            SwapDouble (&x1, &x2);
            SwapDouble (&y1, &y2);
            dx = x2 - x1;
            dy = y2 - y1;
        }
        
        // Compute the slope of the line
        
        slope = dx / dy;
        
        // Compute the x and y offsets for the line width extent

        if (yBegin > yEnd)
            SwapInt (&yBegin, &yEnd);
        else
            ++yEnd;
        if (yBegin < 0) yBegin = 0;
        if (yEnd > numRows) yEnd = numRows;

        xMid = x1 + slope * (yBegin - y1);
        xOffset = fabs (halfWidth * dr / dy);
        
        for (y = yBegin; y != yEnd; ++y)
        {
            xBegin = (int) floor (xMid - xOffset);
            xEnd   = (int) ceil  (xMid + xOffset) + 1;
            for (x = xBegin; x != xEnd; ++x)
            {
                if (x >=0 && x < numCols)
                {
                    psImageSet(pixels, x, y, 1);
                }
            }
            xMid += slope;
        }
    }                                       // if (fabs (dx) > fabs (dy))
}
