#ifndef INC_MATHUTIL
#define INC_MATHUTIL

#include <algorithm>
#include <functional>
#include <cstring>

#include "jc_voronoi.h"

double distance(const jcv_point *p1, const jcv_point *p2)
{
    return sqrt((p1->x - p2->x) * (p1->x - p2->x) + (p1->y - p2->y) * (p1->y - p2->y));
}

// ----------------------- INTEGRATION -----------------------
// Functions pertaining to integrating a function of pixel position on convex
// polygonal domains, that is, integrating some function f which depends only on
// the integer part of position.
// -----------------------------------------------------------

// Integrate over a horizontal line
double integrateHline(double x1, double x2, int y, const std::function<double(int, int)> &f)
{
    if(x1 > x2)
        std::swap(x1, x2);
    
    int ix1 = x1 + 1, ix2 = x2;
    
    if(x2 < ix1)
        return f(x1, y) * (x2 - x1);
    
    // Integrate from x1 to ix1
    double acc = f(x1, y) * (ix1 - x1);
    // Integrate from ix1 to ix2
    for(int x = ix1; x < ix2; ++x)
        acc += f(x, y);
    // Integrate from ix2 to x2
    acc += f(x2, y) * (x2 - ix2);
    
    return acc;
}

// Uncomment and integrate the constant 1 function to test the correctness of the results
// #define TRI_CHECK_AREA

double triangleArea(double x1, double y1, double x2, double y2, double x3, double y3)
{
    return std::abs(((x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1))) / 2;
}

// Integrate over a triangle
// Gives the exact result up to machine precision (0% error)
double integrateTriangle(const jcv_point &v1, const jcv_point &v2, const jcv_point &v3,
    const std::function<double(int, int)> &f)
{
    static auto minComp = [](const jcv_point &a, const jcv_point &b) { return a.y < b.y ? &a : &b; };
    static auto maxComp = [](const jcv_point &a, const jcv_point &b) { return a.y > b.y ? &a : &b; };
    
    double acc = 0;
    const jcv_point *p1, *p2, *p3;
    
    // Y sorting
    p1 = minComp(v1, *minComp(v2, v3));
    p3 = maxComp(v1, *maxComp(v2, v3));
    p2 = &v1 - p1 + &v2 - p3 + &v3; // 3-way exclusion
    
    // Edge slopes
    double dx12 = p2->y != p1->y ? (p2->x - p1->x) / (p2->y - p1->y) : 0,
        dx13 = p3->y != p1->y ? (p3->x - p1->x) / (p3->y - p1->y) : 0,
        dx23 = p3->y != p2->y ? (p3->x - p2->x) / (p3->y - p2->y) : 0;
    
    // Make it so that x1 <= x2 at all times
    double x1 = p1->x, x2 = x1, dx1 = std::min(dx12, dx13), dx2 = dx12 + dx13 - dx1,
        y = p1->y;
    
    // Integrate over trapezoid (x1, y1), (x2, y1), (x1 + dx1 * (y2 - y1), y2), (x2 + dx2 * (y2 - y1), y2).
    // Uses dx1 and dx2 and modifies x1 and x2, but DOES NOT MODIFY Y.
    // It is assumed that for all x ; the function y -> f(x, y) is constant when y1 <= y < y2.
    auto integrateSameY = [&](double y1, double y2)
    {
        double acc = 0;
        while(y1 < y2 - 1e-3)
        {
            // Calculate the Y of the next intersection with an integer X coordinate,
            // capped at y2
            int nx1 = dx1 < 0 ? x1 : x1 + 1, nx2 = dx2 < 0 ? x2 : x2 + 1;
            // Avoid getting stuck if x1 or x2 are already integers
            nx1 -= x1 == nx1;
            nx2 -= x2 == nx2;
            
            double candidate1 = (nx1 - x1) / dx1, candidate2 = (nx2 - x2) / dx2;
            bool which = candidate1 < candidate2;
            
            double calc_dy = std::min(candidate1, candidate2),
                dy = std::min(calc_dy, y2 - y1);
            bool kept = dy == calc_dy;
            
            if(calc_dy <= 0)
            {
                std::cout << "ERR (val) : calc_dy = " << calc_dy << std::endl;
                std::cout << x1 << ", " << x2 << ", " << dx1 << ", " << dx2 << std::endl;
                std::cout << y1 << ", " << y2 << ", " << y2 - y1 << std::endl;
                std::cout << nx1 << ", " << nx2 << std::endl;
                exit(1);
            }
            // Bottom values
            double bx1 = x1 + dx1 * dy, bx2 = x2 + dx2 * dy, by = y1 + dy;
            // Make sure a center piece exists ... (might not happen with very
            // slim triangles)
            if(x1 <= bx2 && x2 >= bx1)
            {
                // Center piece
                acc += integrateHline(std::max(x1, bx1), std::min(x2, bx2), y1, f) * dy;
                // Left piece
                acc += f(std::min(x1, bx1), y1) * std::abs(x1 - bx1) * dy / 2;
                // Right piece
                acc += f(std::min(x2, bx2), y1) * std::abs(x2 - bx2) * dy / 2;
            }
            else
            {
                // In this case, the entire thing is inside a single pixel
                // Multiply the constant function's value by the quad area
                acc += f(x1, y1) * (triangleArea(x1, y1, x2, y1, bx1, by)
                    + triangleArea(x2, y1, bx1, by, bx2, by));
            }
            
            x1 = bx1;
            x2 = bx2;
            if(kept)
                *(which ? &x1 : &x2) = which ? nx1 : nx2;
            y1 += calc_dy;
        }
        return acc;
    };
    
    // Integrate in straight lines from p1->y to p2->y (top half)
    int ny = y + 1;
    if(p1->y != p2->y)
    {
        if(p2->y <= ny)
            acc += integrateSameY(y, p2->y);
        else
        {
            acc += integrateSameY(y, ny);
            
            for(; ny < (int)p2->y; ny++)
                acc += integrateSameY(ny, ny + 1);
            
            acc += integrateSameY(ny, p2->y);
        }
        
        // Snapping
        y = p2->y;
        if(dx12 > dx13)
        {
            x1 = p1->x + dx1 * (p2->y - p1->y);
            x2 = p2->x;
            dx2 = dx23;
        }
        else
        {
            x1 = p2->x;
            x2 = p1->x + dx2 * (p2->y - p1->y);
            dx1 = dx23;
        }
    }
    else
    {
        if(p2->x > p1->x)
        {
            x1 = p1->x;
            x2 = p2->x;
            dx1 = dx13;
            dx2 = dx23;
        }
        else
        {
            x1 = p2->x;
            x2 = p1->x;
            dx1 = dx23;
            dx2 = dx13;
        }
    }
    
    
    // Integrate from p2->y to p3->y (bottom half)
    ny = y + 1;
    if(p3->y <= ny)
        acc += integrateSameY(y, p3->y);
    else
    {
        acc += integrateSameY(y, ny);
        
        for(; ny < (int)p3->y; ny++)
            acc += integrateSameY(ny, ny + 1);
        
        acc += integrateSameY(ny, p3->y);
    }
    
#ifdef TRI_CHECK_AREA
    {
        double realArea = triangleArea(p1->x, p1->y, p2->x, p2->y, p3->x, p3->y);
        double relativeError = (acc - realArea) * 100 / realArea;
        if(relativeError > 3)
        {
            std::cout << "Area discrepancy with triangle (" << p1->x << ", " << p1->y << "), ("
                << p2->x << ", " << p2->y << "), ("
                << p3->x << ", " << p3->y << ") : " << acc << " instead of " << realArea
                << " (" << relativeError << "%)" << std::endl;
        }
        else
            std::cout << "Area is ok" << std::endl;
    }
#endif

    return acc;
}

// Integrate over a cell
// Triangle integration approach
double integrateCell(const jcv_site *site, const std::function<double(int, int)> &f)
{
    double acc = 0;
    
    const jcv_graphedge *edge = site->edges;
    while(edge)
    {
        // Integrate over triangle (site->p, edge->pos[0], edge->pos[1])
        acc += integrateTriangle(site->p, edge->pos[0], edge->pos[1], f);
        
        edge = edge->next;
    }
    
    return acc;
}

// ----------------------- CENTROIDS -----------------------
// Functions pertaining to calculating the weighted centroids of convex polygonal
// domains, that is, integrating the function f : x, y -> w(int(x), int(y)) * (x, y)
// where w is some weighting function dependent only on the integer part of position.
// ---------------------------------------------------------

// Integrate over a rectangle where w doesn't change as y1 <= y < y2.
// Accumulates into a given point instead of returning anything.
void integratePosSmallRect(double x1, double x2, double y1, double y2, const std::function<double(int, int)> &w, jcv_point &r)
{
    double dy = y2 - y1, dy2 = dy * (y2 + y1);
    
    // Integrate over a rectangle where w doesn't change at all
    auto smallInteg = [&](double x1, double x2)
    {
        double factor = w(x1, y1);
        double dx = x2 - x1;
        r.x += dx * (x2 + x1) * dy * factor / 2;
        r.y += dx * dy2 * factor / 2;
    };
    
    if(x1 > x2)
        std::swap(x1, x2);
    
    int ix1 = x1 + 1, ix2 = x2;
    
    if(x2 < ix1)
    {
        smallInteg(x1, x2);
        return;
    }
    
    // Integrate from x1 to ix1
    smallInteg(x1, ix1);
    // Integrate from ix1 to ix2
    for(int x = ix1; x < ix2; ++x)
        smallInteg(x, x + 1);
    // Integrate from ix2 to x2
    smallInteg(ix2, x2);
}

// Integrate over a triangle, accumulating into a given point.
// Simultaneously integrates the weight function and returns the result.
// Gives the exact result up to machine precision (0% error)
double integrateWeightedPosTriangle(const jcv_point &v1, const jcv_point &v2, const jcv_point &v3,
    const std::function<double(int, int)> &w, jcv_point &r)
{
    static auto minComp = [](const jcv_point &a, const jcv_point &b) { return a.y < b.y ? &a : &b; };
    static auto maxComp = [](const jcv_point &a, const jcv_point &b) { return a.y > b.y ? &a : &b; };
    
    double acc = 0;
    const jcv_point *p1, *p2, *p3;
    
    // Y sorting
    p1 = minComp(v1, *minComp(v2, v3));
    p3 = maxComp(v1, *maxComp(v2, v3));
    p2 = &v1 - p1 + &v2 - p3 + &v3; // 3-way exclusion
    
    // Edge slopes
    double dx12 = p2->y != p1->y ? (p2->x - p1->x) / (p2->y - p1->y) : 0,
        dx13 = p3->y != p1->y ? (p3->x - p1->x) / (p3->y - p1->y) : 0,
        dx23 = p3->y != p2->y ? (p3->x - p2->x) / (p3->y - p2->y) : 0;
    
    // Make it so that x1 <= x2 at all times
    double x1 = p1->x, x2 = x1, dx1 = std::min(dx12, dx13), dx2 = dx12 + dx13 - dx1,
        y = p1->y;
    
    // Integrate over trapezoid (x1, y1), (x2, y1), (x1 + dx1 * (y2 - y1), y2), (x2 + dx2 * (y2 - y1), y2).
    // Uses dx1 and dx2 and modifies x1 and x2, but DOES NOT MODIFY Y.
    // It is assumed that for all x ; the function y -> w(x, y) is constant when y1 <= y < y2.
    auto integrateSameY = [&](double y1, double y2)
    {
        while(y1 < y2 - 1e-3)
        {
            // Calculate the Y of the next intersection with an integer X coordinate,
            // capped at y2
            int nx1 = dx1 < 0 ? x1 : x1 + 1, nx2 = dx2 < 0 ? x2 : x2 + 1;
            // Avoid getting stuck if x1 or x2 are already integers
            nx1 -= x1 == nx1;
            nx2 -= x2 == nx2;
            
            double candidate1 = (nx1 - x1) / dx1, candidate2 = (nx2 - x2) / dx2;
            bool which = candidate1 < candidate2;
            
            double calc_dy = std::min(candidate1, candidate2),
                dy = std::min(calc_dy, y2 - y1);
            bool kept = dy == calc_dy;
            
            if(calc_dy <= 0)
            {
                std::cout << "ERR (pos) : calc_dy = " << calc_dy << std::endl;
                std::cout << "x1 = " << x1 << ", x2 = " << x2 << ", dx1 = " << dx1 << ", dx2 = " << dx2 << std::endl;
                std::cout << "y1 = " << y1 << ", y2 = " << y2 << ", dy = " << y2 - y1 << std::endl;
                std::cout << "nx1 = " << nx1 << ", nx2 = " << nx2 << std::endl;
                std::cout << "Drawing triangle (" << p1->x << ", " << p1->y << "), ("
                    << p2->x << ", " << p2->y << "), (" << p3->x << ", " << p3->y << ")" << std::endl;
                exit(1);
            }
            // Bottom values
            double bx1 = x1 + dx1 * dy, bx2 = x2 + dx2 * dy, by = y1 + dy;
            // Make sure a center piece exists ... (might not happen with very
            // slim triangles)
            if(x1 <= bx2 && x2 >= bx1)
            {
                // Center piece
                acc += integrateHline(std::max(x1, bx1), std::min(x2, bx2), y1, w) * dy;
                integratePosSmallRect(std::max(x1, bx1), std::min(x2, bx2), y1, by, w, r);
                // Left piece
                double factor = w(std::min(x1, bx1), y1), area = std::abs(x1 - bx1) * dy / 2;
                acc += factor * area;
                r.x += factor * (dx1 < 0 ? x1 * 2 + bx1 : x1 + bx1 * 2) * area / 3;
                r.y += factor * (dx1 < 0 ? y1 + by * 2 : y1 * 2 + by) * area / 3;
                // Right piece
                factor = w(std::min(x2, bx2), y1), area = std::abs(x2 - bx2) * dy / 2;
                acc += factor * area;
                r.x += factor * (dx2 > 0 ? x2 * 2 + bx2 : x2 + bx2 * 2) * area / 3;
                r.y += factor * (dx2 > 0 ? y1 + by * 2 : y1 * 2 + by) * area / 3;
            }
            else
            {
                // In this case, the entire thing is inside a single pixel
                // Multiply the constant function's value by the quad area
                double t1area = triangleArea(x1, y1, x2, y1, bx1, by),
                    t2area = triangleArea(x2, y1, bx1, by, bx2, by),
                    factor = w(x1, y1);
                acc += factor * (t1area + t2area);
                r.x += ((x1 + x2 + bx1) * t1area + (x2 + bx1 + bx2) * t2area) * factor / 3;
                r.y += ((y1 + y1 + by) * t1area + (y1 + by + by) * t2area) * factor / 3;
            }
            
            x1 = bx1;
            x2 = bx2;
            if(kept)
                *(which ? &x1 : &x2) = which ? nx1 : nx2;
            y1 += calc_dy;
        }
    };
    
    // Integrate in straight lines from p1->y to p2->y (top half)
    int ny = y + 1;
    if(p1->y != p2->y)
    {
        if(p2->y <= ny)
            integrateSameY(y, p2->y);
        else
        {
            integrateSameY(y, ny);
            
            for(; ny < (int)p2->y; ny++)
                integrateSameY(ny, ny + 1);
            
            integrateSameY(ny, p2->y);
        }
        
        // Snapping
        y = p2->y;
        if(dx12 > dx13)
        {
            x1 = p1->x + dx1 * (p2->y - p1->y);
            x2 = p2->x;
            dx2 = dx23;
        }
        else
        {
            x1 = p2->x;
            x2 = p1->x + dx2 * (p2->y - p1->y);
            dx1 = dx23;
        }
    }
    else
    {
        if(p2->x > p1->x)
        {
            x1 = p1->x;
            x2 = p2->x;
            dx1 = dx13;
            dx2 = dx23;
        }
        else
        {
            x1 = p2->x;
            x2 = p1->x;
            dx1 = dx23;
            dx2 = dx13;
        }
    }
    
    
    // Integrate from p2->y to p3->y (bottom half)
    ny = y + 1;
    if(p3->y <= ny)
        integrateSameY(y, p3->y);
    else
    {
        integrateSameY(y, ny);
        
        for(; ny < (int)p3->y; ny++)
            integrateSameY(ny, ny + 1);
        
        integrateSameY(ny, p3->y);
    }
    
    return acc;
}

// Calculates the centroid of a cell given a weight function varying at the pixel level.
// Writes the result in a given point and returns the integral of the weight function.
// Triangle integration approach
double cellWeightedCentroid(const jcv_site *site, const std::function<double(int, int)> &w, jcv_point &r)
{
    double weight_acc = 0;
    r.x = r.y = 0;
    
    const jcv_graphedge *edge = site->edges;
    while(edge)
    {
        // Integrate over triangle (site->p, edge->pos[0], edge->pos[1])
        weight_acc += integrateWeightedPosTriangle(site->p, edge->pos[0], edge->pos[1], w, r);
        
        edge = edge->next;
    }
    
    r.x /= weight_acc;
    r.y /= weight_acc;
    
    return weight_acc;
}

#endif
