#include "w3cs_rain_quad.h"

#include <float.h>
#include <limits.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#define RAIN_POSITION_STEP 0.015625f
#define RAIN_PIXEL_EPSILON 0.000001

struct projected_point {
    double x;
    double y;
    double depth;
    uint8_t planes;
};

static uint16_t load_u16(const uint8_t *source)
{
    uint16_t value;
    memcpy(&value, source, sizeof(value));
    return value;
}

static uint32_t load_u32(const uint8_t *source)
{
    uint32_t value;
    memcpy(&value, source, sizeof(value));
    return value;
}

static float load_f32(const uint8_t *source)
{
    float value;
    memcpy(&value, source, sizeof(value));
    return value;
}

static void store_u16(uint8_t *target, uint16_t value)
{
    memcpy(target, &value, sizeof(value));
}

static void store_f32(uint8_t *target, float value)
{
    memcpy(target, &value, sizeof(value));
}

/* Force one binary32 rounding per operation on i686/x87. The browser mirrors
 * this sequence with Math.fround after every operation. */
static float float_add(float left, float right)
{
    volatile float result = left + right;
    return result;
}

static float float_subtract(float left, float right)
{
    volatile float result = left - right;
    return result;
}

static float float_multiply(float left, float right)
{
    volatile float result = left * right;
    return result;
}

static bool valid_vertex_size(uint32_t vertex_bytes)
{
    return vertex_bytes != 0u
        && vertex_bytes % W3CS_RAIN_QUAD_BYTES == 0u;
}

size_t w3cs_rain_quad_body_capacity(uint32_t vertex_bytes)
{
    uint64_t quads;
    uint64_t per_lane;
    uint64_t total;
    if (!valid_vertex_size(vertex_bytes))
        return 0u;
    quads = vertex_bytes / W3CS_RAIN_QUAD_BYTES;
    per_lane = 1u + (quads * W3CS_RAIN_QUAD_MAX_WIDTH + 7u) / 8u;
    total = W3CS_RAIN_QUAD_BODY_HEADER_SIZE
        + W3CS_RAIN_QUAD_LANES * per_lane;
    return total <= SIZE_MAX ? (size_t)total : 0u;
}

static bool projection_is_valid(
    const struct w3cs_rain_quad_projection *projection)
{
    unsigned index;
    if (!projection || !isfinite(projection->viewport_x)
            || !isfinite(projection->viewport_y)
            || !isfinite(projection->viewport_width)
            || !isfinite(projection->viewport_height)
            || !(projection->viewport_width > 0.0f)
            || !(projection->viewport_height > 0.0f))
        return false;
    for (index = 0; index < 16u; ++index)
        if (!isfinite(projection->object_to_clip[index]))
            return false;
    return true;
}

static bool project_point(const float point[3],
                          const struct w3cs_rain_quad_projection *projection,
                          struct projected_point *result)
{
    const float *m = projection->object_to_clip;
    double x = point[0];
    double y = point[1];
    double z = point[2];
    double clip_x = x * m[0] + y * m[4] + z * m[8] + m[12];
    double clip_y = x * m[1] + y * m[5] + z * m[9] + m[13];
    double clip_z = x * m[2] + y * m[6] + z * m[10] + m[14];
    double clip_w = x * m[3] + y * m[7] + z * m[11] + m[15];
    uint8_t planes;
    if (!isfinite(clip_x) || !isfinite(clip_y) || !isfinite(clip_z)
            || !isfinite(clip_w) || !(clip_w > 0.000001))
        return false;
    planes = (uint8_t)((clip_x >= -clip_w ? 1u : 0u)
        | (clip_x <= clip_w ? 2u : 0u)
        | (clip_y >= -clip_w ? 4u : 0u)
        | (clip_y <= clip_w ? 8u : 0u)
        | (clip_z >= 0.0 ? 16u : 0u)
        | (clip_z <= clip_w ? 32u : 0u));
    result->x = projection->viewport_x
        + (clip_x / clip_w + 1.0) * 0.5 * projection->viewport_width;
    result->y = projection->viewport_y
        + (1.0 - clip_y / clip_w) * 0.5 * projection->viewport_height;
    result->depth = clip_z / clip_w;
    result->planes = planes;
    return isfinite(result->x) && isfinite(result->y)
        && isfinite(result->depth);
}

static bool attributes_are_exact(const uint8_t *vertices,
                                 uint32_t vertex_bytes)
{
    static const uint32_t expected_uv[4][2] = {
        {0x00000000u, 0x00000000u},
        {0x00000000u, 0x3f800000u},
        {0x3f800000u, 0x00000000u},
        {0x3f800000u, 0x3f800000u}
    };
    uint32_t vertex_count = vertex_bytes / W3CS_RAIN_QUAD_STRIDE;
    uint32_t vertex;
    for (vertex = 0; vertex < vertex_count; ++vertex) {
        const uint8_t *bytes = vertices
            + (size_t)vertex * W3CS_RAIN_QUAD_STRIDE;
        unsigned corner = vertex & 3u;
        if (!isfinite(load_f32(bytes)) || !isfinite(load_f32(bytes + 4u))
                || !isfinite(load_f32(bytes + 8u))
                || load_u32(bytes + 12u) != 0x00000000u
                || load_u32(bytes + 16u) != 0x00000000u
                || load_u32(bytes + 20u) != 0x3f800000u
                || load_u32(bytes + 24u) != 0xff649696u
                || load_u32(bytes + 28u) != expected_uv[corner][0]
                || load_u32(bytes + 32u) != expected_uv[corner][1])
            return false;
    }
    return true;
}

static void store_exact_attributes(uint8_t *vertex, unsigned corner)
{
    static const uint32_t template_words[4][6] = {
        {0u, 0u, 0x3f800000u, 0xff649696u, 0u, 0u},
        {0u, 0u, 0x3f800000u, 0xff649696u, 0u, 0x3f800000u},
        {0u, 0u, 0x3f800000u, 0xff649696u, 0x3f800000u, 0u},
        {0u, 0u, 0x3f800000u, 0xff649696u, 0x3f800000u, 0x3f800000u}
    };
    memcpy(vertex + 12u, template_words[corner], 24u);
}

static long round_half_away_from_zero(double value)
{
    return value < 0.0 ? (long)ceil(value - 0.5)
                       : (long)floor(value + 0.5);
}

static bool quantize(float value, float origin, float step,
                     int16_t *quantum, float *reconstructed)
{
    double quotient = ((double)value - origin) / step;
    long rounded;
    float scaled;
    if (!isfinite(quotient) || quotient <= -32768.5
            || quotient >= 32767.5)
        return false;
    rounded = round_half_away_from_zero(quotient);
    if (rounded < INT16_MIN || rounded > INT16_MAX)
        return false;
    *quantum = (int16_t)rounded;
    scaled = float_multiply((float)*quantum, step);
    *reconstructed = float_add(origin, scaled);
    return isfinite(*reconstructed);
}

static bool build_candidate(const uint8_t *source, uint32_t vertex_bytes,
                            unsigned drop_bits, int16_t *lanes,
                            uint8_t *candidate, float origin[3])
{
    uint32_t quad_count = vertex_bytes / W3CS_RAIN_QUAD_BYTES;
    float step = ldexpf(RAIN_POSITION_STEP, (int)drop_bits);
    uint32_t quad;
    unsigned axis;
    for (axis = 0; axis < 3u; ++axis)
        origin[axis] = load_f32(source + axis * 4u);
    for (quad = 0; quad < quad_count; ++quad) {
        const uint8_t *quad_source = source
            + (size_t)quad * W3CS_RAIN_QUAD_BYTES;
        uint8_t *quad_target = candidate
            + (size_t)quad * W3CS_RAIN_QUAD_BYTES;
        float basis[3][3];
        float reconstructed_basis[3][3];
        float positions[4][3];
        for (axis = 0; axis < 3u; ++axis) {
            float p00 = load_f32(quad_source + axis * 4u);
            float p10 = load_f32(quad_source
                                 + 2u * W3CS_RAIN_QUAD_STRIDE
                                 + axis * 4u);
            float p01 = load_f32(quad_source + W3CS_RAIN_QUAD_STRIDE
                                 + axis * 4u);
            basis[0][axis] = p00;
            basis[1][axis] = float_subtract(p10, p00);
            basis[2][axis] = float_subtract(p01, p00);
        }
        for (unsigned part = 0; part < 3u; ++part) {
            for (axis = 0; axis < 3u; ++axis) {
                unsigned lane = part * 3u + axis;
                float lane_origin = part == 0u ? origin[axis] : 0.0f;
                int16_t quantum;
                if (!quantize(basis[part][axis], lane_origin, step,
                              &quantum, &reconstructed_basis[part][axis]))
                    return false;
                lanes[(size_t)lane * quad_count + quad] = quantum;
            }
        }
        for (axis = 0; axis < 3u; ++axis) {
            positions[0][axis] = reconstructed_basis[0][axis];
            positions[1][axis] = float_add(positions[0][axis],
                                           reconstructed_basis[2][axis]);
            positions[2][axis] = float_add(positions[0][axis],
                                           reconstructed_basis[1][axis]);
            positions[3][axis] = float_subtract(
                float_add(positions[1][axis], positions[2][axis]),
                positions[0][axis]);
        }
        for (unsigned corner = 0; corner < 4u; ++corner) {
            uint8_t *vertex = quad_target
                + corner * W3CS_RAIN_QUAD_STRIDE;
            for (axis = 0; axis < 3u; ++axis)
                store_f32(vertex + axis * 4u, positions[corner][axis]);
            store_exact_attributes(vertex, corner);
        }
    }
    return true;
}

static bool measure_candidate(const uint8_t *source,
                              const uint8_t *candidate,
                              uint32_t vertex_bytes,
                              const struct w3cs_rain_quad_projection *projection,
                              double maximum_combined,
                              struct w3cs_rain_quad_result *result)
{
    double depth_scale = 0.5 * fmax(projection->viewport_width,
                                   projection->viewport_height);
    double maximum_xy = 0.0;
    double maximum_depth = 0.0;
    double observed_combined = 0.0;
    uint32_t vertex_count = vertex_bytes / W3CS_RAIN_QUAD_STRIDE;
    uint32_t vertex;
    for (vertex = 0; vertex < vertex_count; ++vertex) {
        float source_point[3];
        float candidate_point[3];
        struct projected_point left;
        struct projected_point right;
        double dx;
        double dy;
        double xy;
        double depth;
        double combined;
        for (unsigned axis = 0; axis < 3u; ++axis) {
            source_point[axis] = load_f32(source
                + (size_t)vertex * W3CS_RAIN_QUAD_STRIDE + axis * 4u);
            candidate_point[axis] = load_f32(candidate
                + (size_t)vertex * W3CS_RAIN_QUAD_STRIDE + axis * 4u);
        }
        if (!project_point(source_point, projection, &left)
                || !project_point(candidate_point, projection, &right)
                || left.planes != right.planes
                || (left.planes & 0x30u) != 0x30u
                || (right.planes & 0x30u) != 0x30u)
            return false;
        dx = left.x - right.x;
        dy = left.y - right.y;
        xy = hypot(dx, dy);
        depth = fabs(left.depth - right.depth) * depth_scale;
        combined = hypot(xy, depth);
        if (!isfinite(combined)
                || combined > maximum_combined + RAIN_PIXEL_EPSILON)
            return false;
        if (xy > maximum_xy) maximum_xy = xy;
        if (depth > maximum_depth) maximum_depth = depth;
        if (combined > observed_combined) observed_combined = combined;
    }
    if (result) {
        result->maximum_xy_error_px = (float)maximum_xy;
        result->maximum_depth_error_px = (float)maximum_depth;
        result->maximum_combined_error_px = (float)observed_combined;
    }
    return true;
}

static uint32_t zigzag_encode(int16_t value)
{
    int32_t wide = value;
    return wide < 0 ? (uint32_t)(-(int64_t)wide * 2 - 1)
                    : (uint32_t)wide * 2u;
}

static int16_t zigzag_decode(uint32_t value)
{
    int32_t magnitude = (int32_t)(value >> 1u);
    int32_t decoded = (value & 1u) ? -magnitude - 1 : magnitude;
    return (int16_t)decoded;
}

static uint8_t required_width(uint32_t value)
{
    uint8_t width = 0;
    while (value) {
        ++width;
        value >>= 1u;
    }
    return width;
}

static size_t encoded_lanes_size(const int16_t *lanes, uint32_t quad_count)
{
    size_t size = 0;
    unsigned lane;
    for (lane = 0; lane < W3CS_RAIN_QUAD_LANES; ++lane) {
        uint32_t maximum = 0;
        uint32_t quad;
        for (quad = 0; quad < quad_count; ++quad) {
            uint32_t value = zigzag_encode(
                lanes[(size_t)lane * quad_count + quad]);
            if (value > maximum) maximum = value;
        }
        size += 1u + ((size_t)quad_count * required_width(maximum) + 7u) / 8u;
    }
    return size;
}

static size_t encode_lanes(const int16_t *lanes, uint32_t quad_count,
                           uint8_t *output)
{
    size_t offset = 0;
    unsigned lane;
    for (lane = 0; lane < W3CS_RAIN_QUAD_LANES; ++lane) {
        uint32_t maximum = 0;
        uint32_t quad;
        uint8_t width;
        size_t bytes;
        uint64_t bit_offset = 0;
        for (quad = 0; quad < quad_count; ++quad) {
            uint32_t value = zigzag_encode(
                lanes[(size_t)lane * quad_count + quad]);
            if (value > maximum) maximum = value;
        }
        width = required_width(maximum);
        bytes = ((size_t)quad_count * width + 7u) / 8u;
        output[offset++] = width;
        if (bytes) memset(output + offset, 0, bytes);
        for (quad = 0; quad < quad_count; ++quad) {
            uint32_t value = zigzag_encode(
                lanes[(size_t)lane * quad_count + quad]);
            unsigned bit;
            for (bit = 0; bit < width; ++bit)
                if (value & (1u << bit)) {
                    uint64_t target = bit_offset + bit;
                    output[offset + (size_t)(target >> 3u)] |=
                        (uint8_t)(1u << (target & 7u));
                }
            bit_offset += width;
        }
        offset += bytes;
    }
    return offset;
}

bool w3cs_rain_quad_encode_body(
    const uint8_t *vertices,
    uint32_t vertex_bytes,
    const struct w3cs_rain_quad_projection *projection,
    float maximum_combined_error_px,
    unsigned max_drop_bits,
    uint8_t *body,
    size_t body_capacity,
    size_t *body_size,
    uint8_t *reconstructed,
    size_t reconstructed_capacity,
    struct w3cs_rain_quad_result *result)
{
    uint32_t quad_count;
    int16_t *lanes = NULL;
    uint8_t *candidate = NULL;
    uint8_t *encoded = NULL;
    struct w3cs_rain_quad_result chosen_result;
    size_t maximum_capacity;
    size_t encoded_size = 0;
    int bits;
    bool selected = false;
    if (!body_size)
        return false;
    maximum_capacity = w3cs_rain_quad_body_capacity(vertex_bytes);
    *body_size = maximum_capacity;
    if (!body)
        return maximum_capacity != 0u;
    if (!maximum_capacity || !vertices || !reconstructed
            || reconstructed_capacity < vertex_bytes
            || max_drop_bits > 14u || !projection_is_valid(projection)
            || !isfinite(maximum_combined_error_px)
            || maximum_combined_error_px < 0.0f
            || !attributes_are_exact(vertices, vertex_bytes))
        return false;
    quad_count = vertex_bytes / W3CS_RAIN_QUAD_BYTES;
    if ((uint64_t)quad_count * W3CS_RAIN_QUAD_LANES
            > SIZE_MAX / sizeof(*lanes))
        return false;
    lanes = (int16_t *)malloc((size_t)quad_count * W3CS_RAIN_QUAD_LANES
                              * sizeof(*lanes));
    candidate = (uint8_t *)malloc(vertex_bytes);
    encoded = (uint8_t *)malloc(maximum_capacity);
    if (!lanes || !candidate || !encoded)
        goto done;
    memset(&chosen_result, 0, sizeof(chosen_result));
    chosen_result.quad_count = quad_count;
    for (bits = (int)max_drop_bits; bits >= 0; --bits) {
        float origin[3];
        struct w3cs_rain_quad_result measured = {0};
        measured.quad_count = quad_count;
        if (!build_candidate(vertices, vertex_bytes, (unsigned)bits, lanes,
                             candidate, origin)
                || !measure_candidate(vertices, candidate, vertex_bytes,
                                      projection,
                                      maximum_combined_error_px, &measured))
            continue;
        encoded_size = W3CS_RAIN_QUAD_BODY_HEADER_SIZE
            + encoded_lanes_size(lanes, quad_count);
        if (encoded_size > maximum_capacity)
            goto done;
        memset(encoded, 0, W3CS_RAIN_QUAD_BODY_HEADER_SIZE);
        store_u16(encoded, W3CS_RAIN_QUAD_MARKER);
        encoded[2] = W3CS_RAIN_QUAD_VERSION;
        encoded[3] = (uint8_t)bits;
        for (unsigned axis = 0; axis < 3u; ++axis) {
            store_f32(encoded + 4u + axis * 4u, origin[axis]);
        }
        if (encode_lanes(lanes, quad_count,
                         encoded + W3CS_RAIN_QUAD_BODY_HEADER_SIZE)
                != encoded_size - W3CS_RAIN_QUAD_BODY_HEADER_SIZE)
            goto done;
        chosen_result = measured;
        chosen_result.position_drop_bits = (uint8_t)bits;
        selected = true;
        break;
    }
    if (!selected || body_capacity < encoded_size)
        goto done;
    memcpy(body, encoded, encoded_size);
    memcpy(reconstructed, candidate, vertex_bytes);
    *body_size = encoded_size;
    if (result) *result = chosen_result;
done:
    free(encoded);
    free(candidate);
    free(lanes);
    return selected && body_capacity >= encoded_size;
}

static bool decode_lanes(const uint8_t *body, size_t body_size,
                         uint32_t quad_count, int16_t *lanes)
{
    size_t offset = W3CS_RAIN_QUAD_BODY_HEADER_SIZE;
    unsigned lane;
    for (lane = 0; lane < W3CS_RAIN_QUAD_LANES; ++lane) {
        uint8_t width;
        uint64_t bits;
        size_t bytes;
        uint32_t maximum = 0;
        uint32_t quad;
        if (offset >= body_size)
            return false;
        width = body[offset++];
        if (width > W3CS_RAIN_QUAD_MAX_WIDTH)
            return false;
        bits = (uint64_t)quad_count * width;
        bytes = (size_t)((bits + 7u) / 8u);
        if (bytes > body_size - offset)
            return false;
        if ((bits & 7u) && bytes) {
            uint8_t valid = (uint8_t)((1u << (bits & 7u)) - 1u);
            if (body[offset + bytes - 1u] & (uint8_t)~valid)
                return false;
        }
        for (quad = 0; quad < quad_count; ++quad) {
            uint32_t code = 0;
            uint64_t bit_offset = (uint64_t)quad * width;
            unsigned bit;
            for (bit = 0; bit < width; ++bit) {
                uint64_t source = bit_offset + bit;
                if (body[offset + (size_t)(source >> 3u)]
                        & (uint8_t)(1u << (source & 7u)))
                    code |= 1u << bit;
            }
            if (code > maximum) maximum = code;
            lanes[(size_t)lane * quad_count + quad] = zigzag_decode(code);
        }
        if (required_width(maximum) != width)
            return false;
        offset += bytes;
    }
    return offset == body_size;
}

bool w3cs_rain_quad_decode_body(
    const uint8_t *body,
    size_t body_size,
    uint32_t vertex_bytes,
    uint8_t *vertices,
    size_t vertex_capacity,
    unsigned *position_drop_bits)
{
    uint32_t quad_count;
    int16_t *lanes = NULL;
    uint8_t *decoded = NULL;
    float origin[3];
    float step;
    unsigned bits;
    bool ok = false;
    if (!body || !valid_vertex_size(vertex_bytes)
            || body_size < W3CS_RAIN_QUAD_BODY_HEADER_SIZE
                    + W3CS_RAIN_QUAD_LANES
            || !vertices || vertex_capacity < vertex_bytes
            || load_u16(body) != W3CS_RAIN_QUAD_MARKER
            || body[2] != W3CS_RAIN_QUAD_VERSION || body[3] > 14u)
        return false;
    for (unsigned axis = 0; axis < 3u; ++axis) {
        origin[axis] = load_f32(body + 4u + axis * 4u);
        if (!isfinite(origin[axis]))
            return false;
    }
    bits = body[3];
    step = ldexpf(RAIN_POSITION_STEP, (int)bits);
    quad_count = vertex_bytes / W3CS_RAIN_QUAD_BYTES;
    if ((uint64_t)quad_count * W3CS_RAIN_QUAD_LANES
            > SIZE_MAX / sizeof(*lanes))
        return false;
    lanes = (int16_t *)malloc((size_t)quad_count * W3CS_RAIN_QUAD_LANES
                              * sizeof(*lanes));
    decoded = (uint8_t *)malloc(vertex_bytes);
    if (!lanes || !decoded || !decode_lanes(body, body_size, quad_count, lanes))
        goto done;
    for (uint32_t quad = 0; quad < quad_count; ++quad) {
        float basis[3][3];
        float positions[4][3];
        uint8_t *quad_target = decoded
            + (size_t)quad * W3CS_RAIN_QUAD_BYTES;
        for (unsigned part = 0; part < 3u; ++part) {
            for (unsigned axis = 0; axis < 3u; ++axis) {
                unsigned lane = part * 3u + axis;
                float lane_origin = part == 0u ? origin[axis] : 0.0f;
                float scaled = float_multiply(
                    (float)lanes[(size_t)lane * quad_count + quad], step);
                basis[part][axis] = float_add(lane_origin, scaled);
            }
        }
        for (unsigned axis = 0; axis < 3u; ++axis) {
            positions[0][axis] = basis[0][axis];
            positions[1][axis] = float_add(positions[0][axis], basis[2][axis]);
            positions[2][axis] = float_add(positions[0][axis], basis[1][axis]);
            positions[3][axis] = float_subtract(
                float_add(positions[1][axis], positions[2][axis]),
                positions[0][axis]);
        }
        for (unsigned corner = 0; corner < 4u; ++corner) {
            uint8_t *vertex = quad_target
                + corner * W3CS_RAIN_QUAD_STRIDE;
            for (unsigned axis = 0; axis < 3u; ++axis)
                store_f32(vertex + axis * 4u, positions[corner][axis]);
            store_exact_attributes(vertex, corner);
        }
    }
    memcpy(vertices, decoded, vertex_bytes);
    if (position_drop_bits) *position_drop_bits = bits;
    ok = true;
done:
    free(decoded);
    free(lanes);
    return ok;
}
