#include "CgeScanline.h"

#define BEZIER_MAX_DEPTH 64

typedef struct {
    CgeScanlineEdge *edge;
    size_t edgeSize;
    size_t count;
} EdgeAddContext;

static void edgeAddLineTo(uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1,
                          void *userData) {
    EdgeAddContext *ctx = (EdgeAddContext *)userData;

    if (ctx->count >= ctx->edgeSize)
        return;
    ctx->count += CgeScanlineEdgeAdd(ctx->edge + ctx->count, x0, y0, x1, y1);
}

static void addVec(uint32_t a[2], uint32_t b[2], uint32_t c[2]) {
    c[0] = a[0] + b[0];
    c[1] = a[1] + b[1];
}

static void shiftVec(uint32_t a[2], uint32_t b[2], int val) {
    b[0] = a[0] >> val;
    b[1] = a[1] >> val;
}

static uint64_t distSq(uint32_t x, uint32_t y) {
    if (x & 0x80000000)
        x = -x;
    if (y & 0x80000000)
        y = -y;

    return ((uint64_t)x * x + (uint64_t)y * y);
}

static uint64_t flatnessCubic(uint32_t p[4][2]) {
    uint64_t f1, f2;
    f1 = distSq(p[0][0] - (p[1][0] << 1) + p[2][0],
                p[0][1] - (p[1][1] << 1) + p[2][1]);
    f2 = distSq(p[1][0] - (p[2][0] << 1) + p[3][0],
                p[1][1] - (p[2][1] << 1) + p[3][1]);
    return f1 > f2 ? f1 : f2;
}

static uint64_t flatnessQuad(uint32_t p[3][2]) {
    return distSq(p[0][0] - (p[1][0] << 1) + p[2][0],
                  p[0][1] - (p[1][1] << 1) + p[2][1]);
}

static void bezierCubicFlattenRec(uint32_t x0, uint32_t y0, uint32_t x1,
                                  uint32_t y1, uint32_t x2, uint32_t y2,
                                  uint32_t x3, uint32_t y3, uint64_t flatnessSq,
                                  int depth, CgeScanlineLineToCallback lineTo,
                                  void *userData) {
    uint32_t a[4][2] = {{x0, y0}, {x1, y1}, {x2, y2}, {x3, y3}};
    uint32_t b[3][2];
    uint32_t c[2][2];
    uint32_t d[2];

    if (depth < BEZIER_MAX_DEPTH && flatnessCubic(a) > flatnessSq) {
        addVec(a[0], a[1], b[0]);
        addVec(a[1], a[2], b[1]);
        addVec(a[2], a[3], b[2]);
        addVec(b[0], b[1], c[0]);
        addVec(b[1], b[2], c[1]);
        addVec(c[0], c[1], d);
        shiftVec(b[0], b[0], 1);
        shiftVec(b[1], b[1], 1);
        shiftVec(b[2], b[2], 1);
        shiftVec(c[0], c[0], 2);
        shiftVec(c[1], c[1], 2);
        shiftVec(d, d, 3);

        bezierCubicFlattenRec(a[0][0], a[0][1], b[0][0], b[0][1], c[0][0],
                              c[0][1], d[0], d[1], flatnessSq, depth + 1,
                              lineTo, userData);

        bezierCubicFlattenRec(d[0], d[1], c[1][0], c[1][1], b[2][0], b[2][1],
                              a[3][0], a[3][1], flatnessSq, depth + 1,
                              lineTo, userData);
    } else {
        lineTo(x0, y0, x3, y3, userData);
    }
}

static void bezierQuadFlattenRec(uint32_t x0, uint32_t y0, uint32_t x1,
                                 uint32_t y1, uint32_t x2, uint32_t y2,
                                 uint64_t flatnessSq, int depth,
                                 CgeScanlineLineToCallback lineTo,
                                 void *userData) {
    uint32_t a[3][2] = {{x0, y0}, {x1, y1}, {x2, y2}};
    uint32_t b[2][2];
    uint32_t c[2];

    if (depth < BEZIER_MAX_DEPTH && flatnessQuad(a) > flatnessSq) {
        addVec(a[0], a[1], b[0]);
        addVec(a[1], a[2], b[1]);
        addVec(b[0], b[1], c);
        shiftVec(b[0], b[0], 1);
        shiftVec(b[1], b[1], 1);
        shiftVec(c, c, 2);

        bezierQuadFlattenRec(a[0][0], a[0][1], b[0][0], b[0][1], c[0], c[1],
                             flatnessSq, depth + 1, lineTo, userData);
        bezierQuadFlattenRec(c[0], c[1], b[1][0], b[1][1], a[2][0], a[2][1],
                            flatnessSq, depth + 1, lineTo, userData);
    } else {
        lineTo(x0, y0, x2, y2, userData);
    }
}

void CgeScanlineBezierCubicFlatten(uint32_t x0, uint32_t y0, uint32_t x1,
                                   uint32_t y1, uint32_t x2, uint32_t y2,
                                   uint32_t x3, uint32_t y3, uint32_t flatness,
                                   CgeScanlineLineToCallback lineTo,
                                   void *userData) {
    uint64_t flatnessSq = (uint64_t)flatness * (uint64_t)flatness;
    if (flatnessSq < 1)
        flatnessSq = 1;
    bezierCubicFlattenRec(x0, y0, x1, y1, x2, y2, x3, y3,
                          flatnessSq, 0, lineTo, userData);
}

void CgeScanlineBezierQuadFlatten(uint32_t x0, uint32_t y0, uint32_t x1,
                                  uint32_t y1, uint32_t x2, uint32_t y2,
                                  uint32_t flatness,
                                  CgeScanlineLineToCallback lineTo,
                                  void *userData) {
    uint64_t flatnessSq = (uint64_t)flatness * (uint64_t)flatness;
    if (flatnessSq < 1)
        flatnessSq = 1;
    bezierQuadFlattenRec(x0, y0, x1, y1, x2, y2,
                         flatnessSq, 0, lineTo, userData);
}

size_t CgeScanlineBezierCubicEdges(CgeScanlineEdge *edge, size_t edgeSize,
                                   uint32_t x0, uint32_t y0, uint32_t x1,
                                   uint32_t y1, uint32_t x2, uint32_t y2,
                                   uint32_t x3, uint32_t y3,
                                   uint32_t flatness) {
    EdgeAddContext ctx;
    ctx.edge = edge;
    ctx.edgeSize = edgeSize;
    ctx.count = 0;

    CgeScanlineBezierCubicFlatten(x0, y0, x1, y1, x2, y2, x3, y3, flatness,
                                  edgeAddLineTo, &ctx);

    return ctx.count;
}

size_t CgeScanlineBezierQuadEdges(CgeScanlineEdge *edge, size_t edgeSize,
                                  uint32_t x0, uint32_t y0, uint32_t x1,
                                  uint32_t y1, uint32_t x2, uint32_t y2,
                                  uint32_t flatness) {
    EdgeAddContext ctx;
    ctx.edge = edge;
    ctx.edgeSize = edgeSize;
    ctx.count = 0;

    CgeScanlineBezierQuadFlatten(x0, y0, x1, y1, x2, y2, flatness,
                                 edgeAddLineTo, &ctx);
    return ctx.count;
}