#import "Basic";
#import "Math";
#import,file "testkit.jai";

TIMESTEP: float = 1.0 / 60.0;
DEBUGGING :: true;

xy :: (x: int, y: int) -> Vector2 {
    return xy(cast(float)x, cast(float)y);
}
normalize :: (using v: *Vector2) -> float {
    sq := sqrt(x*x + y*y);

    factor := 1.0 / sq;
    x *= factor;
    y *= factor;

    return sq;
}

local_to_world :: (using b: Body, local_point: Vector2) -> Vector2 {
    return rotate(local_point, angle) + pos;
}
world_to_local :: (using b: Body, world_point: Vector2) -> Vector2 {
    // world = rotate(local, angle) + pos
    // world - pos = rotate(local, angle)
    // rotate(world - pos, -angle) = local
    return rotate(world_point - pos, -angle);
}

normalize :: (v: Vector2) -> Vector2 #must {
    normalize(*v);
    return v;
}
normalize_or_zero :: inline (using _v: Vector2) -> Vector2 #must {
    v := _v;
    normalize_or_zero(*v);
    return v;
}
length_squared :: inline (a: Vector2) -> float {
    return a.x*a.x + a.y*a.y;
}
negative :: (v: Vector2) -> Vector2 #must {
    return xy(-v.x, -v.y);
}
cross :: (a: Vector2, b: Vector2) -> float {
    return a.x * b.y - a.y * b.x;
}
hit_impulse :: (body_pos: Vector2, mouse_pos: Vector2) -> Vector2 {
    return .{0, -15.0};
    //return normalize(body_pos - mouse_pos) * 15.0;
}
ShapeType :: enum {
    Circle;
    Rectangle;
};

Shape :: struct {
    type: ShapeType;
    offset: Vector2; // in local space of body
    radius: float; // only valid on circle
    halfsize: Vector2; // only valid on rectangle
    invmass: float = 1.0;
    inv_moment_of_inertia: float;
}

Body :: struct {
    pos  , vel      , force : Vector2; // pos is in world space
    angle, angle_vel, torque: float;
    elasticity: float; // 1 is bouncy, 0 is absorbant

    shape: Shape;
}

point_query :: (bodies: [] Body, world_point: Vector2) -> *Body {
    for *bodies {
        if length_squared(world_to_local(it, world_point)) < it.shape.radius*it.shape.radius return it;
    }
    return null;
}

Contact :: struct {
    point_a: Vector2;
    point_b: Vector2;

    normal: Vector2;
    separation: float; // positive when non-penetrating, negative when penetrating
    time_of_impact: float;

    body_a: *Body;
    body_b: *Body;
};

// returns world point
center_of_mass :: (using b: Body) -> Vector2 {
    return b.pos;
}
apply_impulse_linear :: (using b: *Body, impulse: Vector2) {
    if shape.invmass != 0.0 {
        vel += impulse * shape.invmass;
    }
}
apply_impulse_angular :: (using b: *Body, angle_impulse: float) {
    if shape.inv_moment_of_inertia != 0.0 {
        angle_vel += angle_impulse * shape.inv_moment_of_inertia;
    }
}

apply_impulse :: (using b: *Body, world_point: Vector2, impulse: Vector2) {
    apply_impulse_linear(b, impulse);
    r: Vector2 = center_of_mass(b) - world_point;
    dL: float = cross(r, impulse);
    apply_impulse_angular(b, dL);
}

intersect :: (a: *Body, b: *Body, c: *Contact) -> bool {
    c.body_a = a;
    c.body_b = b;

    ab := b.pos - a.pos;
    c.normal = normalize_or_zero(ab);

    assert(a.shape.type == .Circle);
    assert(b.shape.type == .Circle);

    c.point_a = a.pos + c.normal * a.shape.radius;
    c.point_b = b.pos - c.normal * b.shape.radius;


    radius_ab: float = a.shape.radius + b.shape.radius;
    lengthsqr: float = length_squared(ab);

    if lengthsqr <= (radius_ab * radius_ab) {
        return true;
    } else {
        return false;
    }
}

resolve :: (using c: Contact) {
    // collision impulse
    elasticity: float = body_a.elasticity * body_b.elasticity;
    vab: Vector2 = body_a.vel - body_b.vel;
    dot_product: float = dot(vab, normal);
    impulse_j_magnitude: float = -(1.0 + elasticity) * dot_product / (body_a.shape.invmass + body_b.shape.invmass);
    impulse_j: Vector2 = normal * impulse_j_magnitude;
    apply_impulse_linear(body_a, impulse_j);
    apply_impulse_linear(body_b, -impulse_j);

    // move outside eachother, center of mass of both bodies stays the same, but separate them.
    ta: float = body_a.shape.invmass / (body_a.shape.invmass + body_b.shape.invmass);
    tb: float = body_b.shape.invmass / (body_a.shape.invmass + body_b.shape.invmass);

    ds: Vector2 = point_b - point_a;
    body_a.pos += ds * ta;
    body_b.pos -= ds * tb;
}

draw_body :: (d: DrawingSettings, using b: Body) {
    if shape.type == {
        case .Circle;
        points: int = 32;
        if shape.radius > 100.0 points = 128;
        drawing_circle_at: Vector2 = local_to_world(b, shape.offset);
        for 0..points {
            theta: float = (it/cast(float)points) * PI*2.0;
            theta_next: float = (it+1)/cast(float)points * PI*2.0;
            from := drawing_circle_at + .{cos(theta),sin(theta)} * shape.radius;
            to := drawing_circle_at + .{cos(theta_next),sin(theta_next)} * shape.radius;
            line(d, from, to);
        }
        pip(d, pos);

        case .Rectangle;
        using b.shape;
        facing_to_right := rotate(xy(halfsize.x,0.0), angle);
        facing_to_up    := rotate(xy(0.0,halfsize.y), angle);

        upper_right := pos + facing_to_right + facing_to_up;
        upper_left  := pos - facing_to_right + facing_to_up;
        lower_left  := pos - facing_to_right - facing_to_up;
        lower_right := pos + facing_to_right - facing_to_up;

        line(d, upper_left, upper_right);
        line(d, upper_right, lower_right);
        line(d, lower_right, lower_left);
        line(d, lower_left, upper_left);
    }
}


circledemo :: () {
    init();

    bodies: [..]Body;
    array_add(*bodies, .{pos = .{0.0, 0.0}, shape = .{type = .Circle, radius = 1.0}, elasticity = 0.5});
    array_add(*bodies, .{pos = .{0.0, -1003.0}, shape = .{type = .Circle, radius = 1000.0, invmass = 0.0}, elasticity = 1.0}); // ground sphere
    array_add(*bodies, .{pos = .{3.0, 0.0}, shape = .{type = .Circle, radius = 1.0}, elasticity = 0.3});

    unprocessed_time: float = 0.0;
    while !quit {
        frame_start();
        defer frame_end();
        // process physics
        unprocessed_time += dt;
        {
            dt: string = "do not use, physics processes with TIMESTEP not dt";
            while unprocessed_time > TIMESTEP
            {
                defer unprocessed_time -= TIMESTEP;
                
                if get_time() < 0.5
                {
                    //apply_force_at_point(*rects[0], xy(3, 0), rects[0].pos + xy(-0.1, 0.03));
                }
                // iterating by index so can do a sort of iteration that avoids double checking
                // collisions between bodies. This is important because if intersections are double
                // tested then they may have operated on shapes which were just separated but not
                // moved by velocity
                for 0..bodies.count-1 {
                    a := *bodies[it];
                    // calculate moment of inertia
                    if a.shape.type == {
                        case .Circle;
                        a.shape.inv_moment_of_inertia = 1.0/(PI * pow(a.shape.radius, 4.0) / 4.0);

                        case;
                        assert(false);
                    }

                    // gravity must be impulse for some reason
                    apply_impulse_linear(a, .{0.0, -9.81 * (1.0 / a.shape.invmass) * TIMESTEP});

                    // collisions
                    for it+1..bodies.count-1 {
                        b := *bodies[it];
                        contact: Contact;
                        if intersect(a, b, *contact) {
                            resolve(contact);
                        }
                    }
                }

                for * bodies {
                    defer { it.force = .{}; it.torque = 0.0; }

                    it.vel += (it.force * it.shape.invmass) * TIMESTEP;
                    it.pos += it.vel * TIMESTEP;
                    it.angle_vel += (it.torque * it.shape.inv_moment_of_inertia) * TIMESTEP;
                    it.angle += it.angle_vel * TIMESTEP;
                }
            }
        }

        b := bodies[0];
        pip(.{}, local_to_world(b, world_to_local(b, mouse_screen())));

        hovering: *Body = point_query(bodies, screen_to_world(mouse_screen()));
        for * bodies {
            d: DrawingSettings = .{true, GREEN};
            if hovering == it {
                d.color = RED;
                vector(d, screen_to_world(mouse_screen()), hit_impulse(it.pos, screen_to_world(mouse_screen())) * TIMESTEP);
            }
            draw_body(d, it);
        }

        for Input.events_this_frame {
            if it.type == {
                case .KEYBOARD;
                if it.key_code == .MOUSE_BUTTON_LEFT {
                    panning = cast(bool)it.key_pressed;
                    to_tap := point_query(bodies, screen_to_world(mouse_screen()));
                    if to_tap != null apply_impulse(to_tap, mouse_world(), hit_impulse(to_tap.pos, mouse_screen()));
                }
            }
        }
    }
}

main :: () { circledemo(); }