module sim

import math

pub const default_rope_length = 0.25
pub const default_bearing_mass = 0.03
pub const default_magnet_spacing = 0.05
pub const default_magnet_height = 0.03
pub const default_magnet_strength = 10.0
pub const default_gravity = 4.9

@[params]
pub struct SimParams {
pub:
    rope_length     f64 = default_rope_length
    bearing_mass    f64 = default_bearing_mass
    magnet_spacing  f64 = default_magnet_spacing
    magnet_height   f64 = default_magnet_height
    magnet_strength f64 = default_magnet_strength
    gravity         f64 = default_gravity
}

pub fn sim_params(params SimParams) SimParams {
    return SimParams{
        ...params
    }
}

pub fn (params SimParams) get_rope_vector(state SimState) Vector3D {
    rope_origin := vector(z: params.rope_length)

    return state.position + rope_origin.scale(-1)
}

pub fn (params SimParams) get_forces_sum(state SimState) Vector3D {
    // force due to gravity
    f_gravity := params.get_grav_force(state)

    // force due to magnets
    f_magnet1 := params.get_magnet1_force(state)
    f_magnet2 := params.get_magnet2_force(state)
    f_magnet3 := params.get_magnet3_force(state)

    mut f_passive := vector(x: 0.0, y: 0.0, z: 0.0)
    for force in [f_gravity, f_magnet1, f_magnet2, f_magnet3] {
        f_passive = f_passive + force
    }

    // force due to tension of the rope
    f_tension := params.get_tension_force(state, f_passive)

    return f_passive + f_tension
}

pub fn (params SimParams) get_grav_force(state SimState) Vector3D {
    return vector(z: -params.bearing_mass * params.gravity)
}

pub fn (params SimParams) get_magnet_position(theta f64) Vector3D {
    return vector(
        x: math.cos(theta) * params.magnet_spacing
        y: math.sin(theta) * params.magnet_spacing
        z: -params.magnet_height
    )
}

pub fn (params SimParams) get_magnet_force(theta f64, state SimState) Vector3D {
    magnet_position := params.get_magnet_position(theta)
    mut diff := magnet_position + state.position.scale(-1)
    distance_squared := diff.norm_squared()
    diff = diff.scale(1.0 / math.sqrt(distance_squared))
    return diff.scale(params.magnet_strength / distance_squared)
}

pub fn (params SimParams) get_magnet_dist(theta f64, state SimState) f64 {
    return (params.get_magnet_position(theta) + state.position.scale(-1)).norm()
}

pub fn (params SimParams) get_magnet1_force(state SimState) Vector3D {
    return params.get_magnet_force(0.0 * math.pi / 3.0, state)
}

pub fn (params SimParams) get_magnet2_force(state SimState) Vector3D {
    return params.get_magnet_force(2.0 * math.pi / 3.0, state)
}

pub fn (params SimParams) get_magnet3_force(state SimState) Vector3D {
    return params.get_magnet_force(4.0 * math.pi / 3.0, state)
}

pub fn (params SimParams) get_tension_force(state SimState, f_passive Vector3D) Vector3D {
    rope_vector := params.get_rope_vector(state)
    rope_vector_norm := rope_vector.scale(1.0 / rope_vector.norm())
    return rope_vector_norm.scale(-1.0 * (rope_vector_norm * f_passive))
}