/* Moving-flipper collision response at 1000:7ed9. */

#include "tdkpin_flippers.h"

#include "tdkpin_arithmetic.h"
#include "tdkpin_collision_records.h"
#include "tdkpin_gameplay_helpers.h"
#include "tdkpin_real48.h"
#include "tdkpin_sound.h"

enum {
    COLLISION_RECORDS_BASE = 0x095b,
    COLLISION_RECORD_BYTES = 0x53,
    RECORD_RADIUS = 0x22,
    RECORD_RESPONSE_NORMAL = 0x28,
    COLLISION_SEARCH_RADIUS = 54000,
    COLLISION_RESPONSE_RADIUS = 44000,
};

typedef struct {
    int32_t radius;
    int32_t pivot_x;
    int32_t pivot_y;
    int32_t unused_negative_point_x;
    int32_t unused_positive_point_x;
    int32_t negative_edge_x;
    int32_t negative_edge_y;
    int32_t positive_edge_x;
    int32_t positive_edge_y;
    uint16_t response_record;
} FlipperCollisionGeometry;

static const BorlandReal48 g_real48_one_tenth =
    {{0x7d, 0xcd, 0xcc, 0xcc, 0xcc, 0x4c}};
static const BorlandReal48 g_real48_one_half =
    {{0x80, 0, 0, 0, 0, 0}};
static const BorlandReal48 g_real48_two =
    {{0x82, 0, 0, 0, 0, 0}};
static const BorlandReal48 g_real48_two_fifths =
    {{0x7f, 0xcd, 0xcc, 0xcc, 0xcc, 0x4c}};
static const BorlandReal48 g_real48_one =
    {{0x81, 0, 0, 0, 0, 0}};
static const BorlandReal48 g_real48_thousand =
    {{0x8a, 0, 0, 0, 0, 0x7a}};

static Win16FarPtr caller_receiver(uint16_t caller_bp)
{
    uint16_t offset = win16_read_stack_u16((uint16_t)(caller_bp + 6));
    uint16_t selector = win16_read_stack_u16((uint16_t)(caller_bp + 8));
    return win16_make_far_pointer(selector, offset);
}

static Win16FarPtr collision_record(uint16_t id)
{
    return win16_dgroup_pointer((uint16_t)(
        COLLISION_RECORDS_BASE + id * COLLISION_RECORD_BYTES));
}

static int32_t record_i32(uint16_t id, uint16_t offset)
{
    return (int32_t)win16_read_u32(
        win16_far_add_offset(collision_record(id), offset));
}

static BorlandReal48 record_real48(uint16_t id, uint16_t offset)
{
    BorlandReal48 value;
    Win16FarPtr source = win16_far_add_offset(collision_record(id), offset);
    for (uint16_t index = 0; index < sizeof(value.bytes); index++) {
        value.bytes[index] = win16_read_u8(
            win16_far_add_offset(source, index));
    }
    return value;
}

static int32_t add_wrap_i32(int32_t left, int32_t right)
{
    return (int32_t)((uint32_t)left + (uint32_t)right);
}

static int32_t subtract_wrap_i32(int32_t left, int32_t right)
{
    return (int32_t)((uint32_t)left - (uint32_t)right);
}

static int32_t negate_wrap_i32(int32_t value)
{
    return (int32_t)(0u - (uint32_t)value);
}

static int32_t absolute_wrap_i32(int32_t value)
{
    return value < 0 ? negate_wrap_i32(value) : value;
}

static int32_t divide_by_thousand(int32_t value)
{
    return borland_divide_i32(value, 1000).quotient;
}

static int32_t indeterminate_i32(void)
{
    uint32_t low = win16_indeterminate_u16();
    uint32_t high = win16_indeterminate_u16();
    return (int32_t)(low | (high << 16));
}

static BorlandReal48 negate_real48(BorlandReal48 value)
{
    if (value.bytes[0] != 0) {
        value.bytes[5] ^= 0x80;
    }
    return value;
}

static FlipperCollisionGeometry load_flipper_geometry(
    int32_t delta, uint16_t flipper)
{
    FlipperCollisionGeometry geometry;
    if (flipper == 1) {
        geometry.radius = borland_real48_round_to_i32(
            record_real48(65, RECORD_RADIUS));
        geometry.pivot_x = subtract_wrap_i32(
            record_i32(65, offsetof(TdkpinCollisionRecord, point1_x_milli)),
            10000);
        geometry.pivot_y = record_i32(
            65, offsetof(TdkpinCollisionRecord, point1_y_milli));
        geometry.unused_negative_point_x = record_i32(
            66, offsetof(TdkpinCollisionRecord, point1_x_milli));
        geometry.negative_edge_x = add_wrap_i32(
            record_i32(66, offsetof(TdkpinCollisionRecord, point2_x_milli)),
            20000);
        geometry.negative_edge_y = record_i32(
            66, offsetof(TdkpinCollisionRecord, point2_y_milli));
        geometry.unused_positive_point_x = record_i32(
            68, offsetof(TdkpinCollisionRecord, point2_x_milli));
        geometry.positive_edge_x = add_wrap_i32(
            record_i32(68, offsetof(TdkpinCollisionRecord, point1_x_milli)),
            20000);
        geometry.positive_edge_y = record_i32(
            68, offsetof(TdkpinCollisionRecord, point1_y_milli));
        geometry.response_record = delta == 1 ? 68 : 66;
    } else if (flipper == 2) {
        geometry.radius = borland_real48_round_to_i32(
            record_real48(80, RECORD_RADIUS));
        geometry.pivot_x = add_wrap_i32(
            record_i32(80, offsetof(TdkpinCollisionRecord, point1_x_milli)),
            10000);
        geometry.pivot_y = record_i32(
            80, offsetof(TdkpinCollisionRecord, point1_y_milli));
        geometry.unused_negative_point_x = record_i32(
            83, offsetof(TdkpinCollisionRecord, point1_x_milli));
        geometry.negative_edge_x = subtract_wrap_i32(
            record_i32(83, offsetof(TdkpinCollisionRecord, point1_x_milli)),
            20000);
        geometry.negative_edge_y = record_i32(
            83, offsetof(TdkpinCollisionRecord, point1_y_milli));
        geometry.unused_positive_point_x = record_i32(
            81, offsetof(TdkpinCollisionRecord, point1_x_milli));
        geometry.positive_edge_x = subtract_wrap_i32(
            record_i32(81, offsetof(TdkpinCollisionRecord, point2_x_milli)),
            20000);
        geometry.positive_edge_y = record_i32(
            81, offsetof(TdkpinCollisionRecord, point2_y_milli));
        geometry.response_record = delta == 1 ? 81 : 83;
    } else {
        geometry = (FlipperCollisionGeometry){
            .radius = indeterminate_i32(),
            .pivot_x = indeterminate_i32(),
            .pivot_y = indeterminate_i32(),
            .unused_negative_point_x = indeterminate_i32(),
            .unused_positive_point_x = indeterminate_i32(),
            .negative_edge_x = indeterminate_i32(),
            .negative_edge_y = indeterminate_i32(),
            .positive_edge_x = indeterminate_i32(),
            .positive_edge_y = indeterminate_i32(),
            .response_record = win16_indeterminate_u16(),
        };
    }
    geometry.pivot_y = add_wrap_i32(
        geometry.pivot_y,
        borland_multiply_i32(delta, geometry.radius));
    (void)geometry.unused_negative_point_x;
    (void)geometry.unused_positive_point_x;
    return geometry;
}

static int32_t cross_product_for_edge(
    int32_t edge_x,
    int32_t edge_y,
    int32_t pivot_x,
    int32_t pivot_y,
    int32_t ball_x,
    int32_t ball_y)
{
    int32_t edge_from_ball_x = divide_by_thousand(
        subtract_wrap_i32(edge_x, ball_x));
    int32_t edge_from_pivot_y = divide_by_thousand(
        subtract_wrap_i32(edge_y, pivot_y));
    int32_t edge_from_ball_y = divide_by_thousand(
        subtract_wrap_i32(edge_y, ball_y));
    int32_t edge_from_pivot_x = divide_by_thousand(
        subtract_wrap_i32(edge_x, pivot_x));
    return subtract_wrap_i32(
        borland_multiply_i32(edge_from_ball_x, edge_from_pivot_y),
        borland_multiply_i32(edge_from_ball_y, edge_from_pivot_x));
}

static int32_t radial_distance_milli(
    int32_t ball_x, int32_t ball_y, int32_t pivot_x, int32_t pivot_y)
{
    BorlandReal48 dx = borland_real48_divide(
        borland_i32_to_real48(subtract_wrap_i32(ball_x, pivot_x)),
        g_real48_thousand);
    BorlandReal48 dy = borland_real48_divide(
        borland_i32_to_real48(subtract_wrap_i32(ball_y, pivot_y)),
        g_real48_thousand);
    BorlandReal48 squared = borland_real48_add(
        borland_real48_square(dx), borland_real48_square(dy));
    return borland_real48_round_to_i32(
        borland_real48_multiply(
            borland_real48_sqrt(squared), g_real48_thousand));
}

static bool point_in_moving_flipper(
    const FlipperCollisionGeometry *geometry,
    int32_t delta,
    uint16_t flipper,
    uint16_t mode,
    int32_t ball_x,
    int32_t ball_y,
    int32_t *distance)
{
    int32_t first_cross;
    int32_t second_cross;
    if (delta == -1) {
        first_cross = cross_product_for_edge(
            geometry->negative_edge_x,
            subtract_wrap_i32(geometry->negative_edge_y, 43000),
            geometry->pivot_x,
            geometry->pivot_y,
            ball_x,
            ball_y);
        second_cross = cross_product_for_edge(
            geometry->negative_edge_x,
            add_wrap_i32(geometry->negative_edge_y, 12000),
            geometry->pivot_x,
            geometry->pivot_y,
            ball_x,
            ball_y);
    } else {
        first_cross = cross_product_for_edge(
            geometry->positive_edge_x,
            subtract_wrap_i32(geometry->positive_edge_y, 12000),
            geometry->pivot_x,
            geometry->pivot_y,
            ball_x,
            ball_y);
        second_cross = cross_product_for_edge(
            geometry->positive_edge_x,
            add_wrap_i32(geometry->positive_edge_y, 43000),
            geometry->pivot_x,
            geometry->pivot_y,
            ball_x,
            ball_y);
    }

    int32_t absolute_x = absolute_wrap_i32(
        subtract_wrap_i32(ball_x, geometry->pivot_x));
    int32_t absolute_y = absolute_wrap_i32(
        subtract_wrap_i32(ball_y, geometry->pivot_y));
    if (absolute_x >= COLLISION_SEARCH_RADIUS ||
        absolute_y >= COLLISION_SEARCH_RADIUS) {
        return false;
    }
    *distance = radial_distance_milli(
        ball_x, ball_y, geometry->pivot_x, geometry->pivot_y);
    if (*distance > COLLISION_SEARCH_RADIUS || mode != 0) {
        return false;
    }
    if (flipper == 1) {
        return first_cross > -1000 && second_cross < 1000 &&
               ball_x > geometry->pivot_x;
    }
    if (flipper == 2) {
        return first_cross < 1000 && second_cross > -1000 &&
               ball_x < geometry->pivot_x;
    }
    return false;
}

static int32_t collision_penetration(
    const FlipperCollisionGeometry *geometry,
    int32_t delta,
    int32_t ball_x,
    int32_t ball_y)
{
    int32_t collision_y;
    if (delta == -1) {
        int32_t edge_dx = absolute_wrap_i32(subtract_wrap_i32(
            geometry->negative_edge_x, geometry->pivot_x));
        if (borland_real48_compare(
                borland_i32_to_real48(edge_dx), g_real48_one_tenth) > 0) {
            collision_y = add_wrap_i32(
                subtract_wrap_i32(
                    geometry->pivot_y,
                    absolute_wrap_i32(subtract_wrap_i32(
                        ball_x, geometry->pivot_x))),
                5000);
        } else {
            collision_y = subtract_wrap_i32(
                geometry->negative_edge_y, 43000);
        }
    } else {
        int32_t edge_dx = subtract_wrap_i32(
            geometry->positive_edge_x, geometry->pivot_x);
        if (borland_real48_compare(
                borland_i32_to_real48(absolute_wrap_i32(edge_dx)),
                g_real48_one_tenth) > 0) {
            int32_t numerator = borland_multiply_i32(
                subtract_wrap_i32(ball_x, geometry->pivot_x),
                subtract_wrap_i32(
                    add_wrap_i32(geometry->positive_edge_y, 43000),
                    geometry->pivot_y));
            int32_t interpolation = borland_real48_round_to_i32(
                borland_real48_divide(
                    borland_i32_to_real48(numerator),
                    borland_i32_to_real48(edge_dx)));
            collision_y = subtract_wrap_i32(
                geometry->pivot_y, interpolation);
        } else {
            collision_y = add_wrap_i32(
                geometry->positive_edge_y, 43000);
        }
    }

    int32_t penetration = subtract_wrap_i32(collision_y, ball_y);
    if (penetration < 0) {
        return negate_wrap_i32(
            borland_multiply_i32(penetration, delta));
    }
    return 0;
}

static void apply_flipper_response(
    Win16FarPtr receiver,
    const FlipperCollisionGeometry *geometry,
    int32_t delta,
    uint16_t flipper,
    int32_t ball_x,
    int32_t ball_y,
    int32_t distance,
    int32_t penetration,
    uint16_t caller_bp)
{
    int32_t normal_x_integer = borland_multiply_i32(
        subtract_wrap_i32(ball_x, geometry->pivot_x), delta);
    int32_t normal_y_integer = borland_multiply_i32(
        add_wrap_i32(
            subtract_wrap_i32(ball_y, geometry->pivot_y), 4000),
        delta);
    BorlandReal48 normal_x = borland_i32_to_real48(normal_x_integer);
    BorlandReal48 normal_y = borland_i32_to_real48(normal_y_integer);
    if (flipper == 2) {
        normal_x = negate_real48(normal_x);
        normal_y = negate_real48(normal_y);
    }

    int32_t velocity_x = (int32_t)win16_read_u32(
        win16_far_add_offset(receiver, 0x0baa));
    int32_t velocity_y = (int32_t)win16_read_u32(
        win16_far_add_offset(receiver, 0x0bae));
    BorlandReal48 response_radius =
        borland_i32_to_real48(COLLISION_RESPONSE_RADIUS);

    /* 1000:94ce-9553 computes this projection and then deliberately clears it. */
    BorlandReal48 discarded_projection_real = borland_real48_divide(
        borland_real48_multiply(
            borland_real48_add(
                borland_real48_multiply(
                    borland_i32_to_real48(velocity_y), normal_y),
                borland_real48_multiply(
                    borland_i32_to_real48(velocity_x), normal_x)),
            g_real48_one_half),
        response_radius);
    (void)borland_real48_round_to_i32(discarded_projection_real);
    int32_t normal_projection = 0;

    int32_t tangent_projection = borland_real48_round_to_i32(
        borland_real48_divide(
            borland_real48_subtract(
                borland_real48_multiply(
                    borland_i32_to_real48(velocity_x), normal_y),
                borland_real48_multiply(
                    borland_i32_to_real48(velocity_y), normal_x)),
            response_radius));

    BorlandReal48 gain = borland_real48_add(
        borland_real48_multiply(
            borland_real48_sqrt(borland_real48_divide(
                borland_i32_to_real48(distance), response_radius)),
            g_real48_two),
        g_real48_two_fifths);
    BorlandReal48 record_response = record_real48(
        geometry->response_record, RECORD_RESPONSE_NORMAL);
    tangent_projection = add_wrap_i32(
        borland_real48_round_to_i32(
            borland_real48_multiply(
                borland_i32_to_real48(tangent_projection),
                borland_real48_add(g_real48_one, record_response))),
        borland_real48_round_to_i32(
            borland_real48_multiply(
                borland_i32_to_real48((int32_t)win16_read_u32(
                    win16_far_add_offset(receiver, 0x56))),
                gain)));

    int32_t delta_velocity_x = borland_real48_round_to_i32(
        borland_real48_divide(
            borland_real48_subtract(
                borland_real48_multiply(
                    borland_i32_to_real48(normal_projection), normal_x),
                borland_real48_multiply(
                    borland_i32_to_real48(tangent_projection), normal_y)),
            response_radius));
    velocity_x = add_wrap_i32(velocity_x, delta_velocity_x);
    win16_write_u32(receiver, 0x0baa, (uint32_t)velocity_x);

    int32_t delta_velocity_y = borland_real48_round_to_i32(
        borland_real48_divide(
            borland_real48_add(
                borland_real48_multiply(
                    borland_i32_to_real48(tangent_projection), normal_x),
                borland_real48_multiply(
                    borland_i32_to_real48(normal_projection), normal_y)),
            response_radius));
    velocity_y = add_wrap_i32(velocity_y, delta_velocity_y);
    win16_write_u32(receiver, 0x0bae, (uint32_t)velocity_y);

    int32_t movement_x = borland_real48_round_to_i32(
        borland_real48_divide(
            borland_i32_to_real48(
                borland_multiply_i32(penetration, velocity_x)),
            borland_i32_to_real48(velocity_y)));
    tdkpin_move_ball_and_render(
        caller_bp, movement_x, penetration);
}

void tdkpin_move_flipper_collision_geometry(
    uint16_t caller_bp,
    int32_t delta,
    uint16_t flipper,
    uint16_t mode)
{
    tdkpin_play_sound_if_not_tilted(21);
    Win16FarPtr receiver = caller_receiver(caller_bp);
    FlipperCollisionGeometry geometry =
        load_flipper_geometry(delta, flipper);
    uint16_t ball_count = win16_read_u8(
        win16_far_add_offset(receiver, 0x61));
    for (uint16_t ball = 1; ball <= ball_count; ball++) {
        if (win16_read_u8(win16_far_add_offset(receiver, 0x61)) > 1) {
            tdkpin_load_ball_slot(caller_bp, (uint16_t)(ball - 1u));
        }
        int32_t ball_x = (int32_t)win16_read_u32(
            win16_dgroup_pointer(0x07d3));
        int32_t ball_y = (int32_t)win16_read_u32(
            win16_dgroup_pointer(0x07d7));
        int32_t distance = 0;
        if (point_in_moving_flipper(
                &geometry,
                delta,
                flipper,
                mode,
                ball_x,
                ball_y,
                &distance)) {
            int32_t penetration = collision_penetration(
                &geometry, delta, ball_x, ball_y);
            apply_flipper_response(
                receiver,
                &geometry,
                delta,
                flipper,
                ball_x,
                ball_y,
                distance,
                penetration,
                caller_bp);
        }
        if (win16_read_u8(win16_far_add_offset(receiver, 0x61)) > 1) {
            tdkpin_save_ball_slot(caller_bp, (uint16_t)(ball - 1u));
        }
    }
}