#ifndef CAR_PHYSICS_HPP
#define CAR_PHYSICS_HPP

#include "godot_cpp/classes/collision_shape3d.hpp"
#include "godot_cpp/templates/hash_set.hpp"
#include "godot_cpp/variant/rid.hpp"
#include <godot_cpp/classes/curve.hpp>
#include <godot_cpp/classes/physics_direct_body_state3d.hpp>
#include <godot_cpp/classes/rigid_body3d.hpp>

namespace godot {
//! Subclass Sandbox for car physics. Uses _integrate_forces and local_velocity to abstract the behaviour of a physical car.
class CarPhysics : public RigidBody3D {
    GDCLASS(CarPhysics, RigidBody3D);
    //! Registers functions and properties
    static void _bind_methods();
public:
    //! Enable _integrate_forces, fetch child nodes, and setup contact monitoring.
    virtual void _enter_tree() override;

    //! Fixed-interval process function. Split into process_oversteer and process_understeer
    virtual void _physics_process(double delta_time) override;
    //! Process the current_oversteer member variable
    void process_oversteer(double delta_time);
    //! Process the current_understeer member variable
    void process_understeer(double delta_time);
    //! Custom force integrator. Split into integrate_steering, integrate_engine_acceleration and integrate_oversteer. After calling other integrate_\* functions, applies local_velocity to state.velocity.
    virtual void _integrate_forces(PhysicsDirectBodyState3D *state) override;
protected:
    //! Integrate steering into angular and local x velocities. As a side-effect this also applies sliding resistance and sliding recovery. Also detects crashes to avoid glitchy behaviour when accelerating into a wall.
    void integrate_steering(PhysicsDirectBodyState3D *state);
    //! Apply velocity along local z. Accelerates towards value returned by get_true_target_speed at get_current_acceleration m/s^2
    void integrate_engine_acceleration(PhysicsDirectBodyState3D *state);
    //! Integrate oversteering to local_velocity to allow drifting.
    void integrate_oversteer(PhysicsDirectBodyState3D *state);
    //! Detect that an object started colliding with the wheels.
    void on_body_shape_entered(RID body_rid, Node *node, int body_shape_index, int local_shape_index);
    //! Detect that an object lost contact with the wheels.
    void on_body_shape_exited(RID body_rid, Node *node, int body_shape_index, int local_shape_index);
    //! evaluate the oversteer curve with a speed, taking *_x_scale into account
    float evaluate_oversteer_curve(float speed) const;
    //! evaluate the understeer curve with a speed, taking *_x_scale into account
    float evaluate_understeer_curve(float speed) const;

    //! Returns the maximum speed modified by under/oversteer
    float get_true_target_speed() const;
    //! Returns true if there is at least one contact with either of the wheels.
    bool is_grounded() const;
    //! Returns acceleration, taking into account braking, throttle, and handbrake.
    float get_current_acceleration() const;
public:
    //! Public getter for current local velocity
    Vector3 get_local_velocity() const;
    //! transform the current local_velocity to world coordinates
    Vector3 local_to_world_velocity() const;
    //! transform the current world velocity to local coordinates
    Vector3 world_to_local_velocity() const;
    //! The current forward velocity (local_velocity.z)
    float get_current_speed() const;
    //! The target speed
    void set_target_speed(float target);
    //! The target speed
    float get_target_speed() const;
    //! The current steering input
    void set_current_steering(float steering);
    //! The current steering input
    float get_current_steering() const;
    //! True if the hand-brake is currently active
    void set_brake(bool value);
    //! True if the hand-brake is currently active
    bool get_brake() const;
    //! The base curve used for oversteering
    void set_oversteer_curve(Ref<Curve> curve);
    //! The base curve used for oversteering
    Ref<Curve> get_oversteer_curve() const;
    //! The amount of speed represented by x=1 on the oversteer curve.
    void set_oversteer_curve_x_scale(float scale);
    //! The amount of speed represented by x=1 on the oversteer curve.
    float get_oversteer_curve_x_scale() const;
    //! The base curve used for understeering
    void set_understeer_curve(Ref<Curve> curve);
    //! The base curve used for understeering
    Ref<Curve> get_understeer_curve() const;
    //! The amount of speed represented by x=1 on the understeer curve.
    void set_understeer_curve_x_scale(float scale);
    //! The amount of speed represented by x=1 on the understeer curve.
    float get_understeer_curve_x_scale() const;
    //! The base engine acceleration of this car.
    void set_acceleration(float value);
    //! The base engine acceleration of this car.
    float get_acceleration() const;
    //! The base engine braking force of this car.
    void set_engine_brake_force(float value);
    //! The base engine braking force of this car.
    float get_engine_brake_force() const;
    //! The amount of braking force applied by the handbrake.
    void set_handbrake_force(float value);
    //! The amount of braking force applied by the handbrake.
    float get_handbrake_force() const;
    //! The modifier applied to oversteering when the handbrake is active.
    void set_handbrake_oversteer(float value);
    //! The modifier applied to oversteering when the handbrake is active.
    float get_handbrake_oversteer() const;
    //! The speed at which the car will return to regular traction after under- or oversteering.
    void set_traction_recovery_speed(float value);
    //! The speed at which the car will return to regular traction after under- or oversteering.
    float get_traction_recovery_speed() const;
    //! The maximum sideways speed that can be reached while drifting.
    void set_max_slide_speed(float value);
    //! The maximum sideways speed that can be reached while drifting.
    float get_max_slide_speed() const;
    //! The sideways acceleration applied when drifting.
    void set_slide_speed_acceleration(float value);
    //! The sideways acceleration applied when drifting.
    float get_slide_speed_acceleration() const;
    //! Deceleration applied to forward speed when oversteering.
    void set_oversteer_speed_penalty(float value);
    //! Deceleration applied to forward speed when oversteering.
    float get_oversteer_speed_penalty() const;
    //! Modifier applied to brake force when oversteering.
    void set_oversteer_brake_penalty(float value);
    //! Modifier applied to brake force when oversteering.
    float get_oversteer_brake_penalty() const;
    //! Modifier applied to brake force when oversteering.
    void set_oversteer_steering_speed(float value);
    //! Base amount of steering applied while oversteering.
    float get_oversteer_steering_speed() const;
    //! Deceleration applied to sideways velocity.
    void set_slide_resistance(float value);
    //! Deceleration applied to sideways velocity.
    float get_slide_resistance() const;
    //! Target amount of velocity towards the centre of a turn while steering.
    void set_steering_inward_speed(float value);
    //! Target amount of velocity towards the centre of a turn while steering.
    float get_steering_inward_speed() const;
private:
    //! Acceleration at the end of the integration step of the last frame.
    Vector3 last_velocity{0.f, 0.f, 0.f};
    //! Velocity relative to the local transform.
    Vector3 local_velocity{0.f, 0.f, 0.f};
    //! Target value for local_velocity.z.
    float target_speed{0.f};
    //! Current steering input.
    float current_steering{0.f};
    //! While true, the handbrake is on.
    bool brake{false};

    Ref<Curve> oversteer_curve{};
    float oversteer_curve_x_scale{50.f};
    Ref<Curve> understeer_curve{};
    float understeer_curve_x_scale{50.f};
    float acceleration{20.f};
    float engine_brake_force{30.f};
    float handbrake_force{40.f};
    float handbrake_oversteer{1.f};
    float traction_recovery_speed{1.f};
    float max_slide_speed{20.f};
    float slide_speed_acceleration{0.05f};
    float oversteer_speed_penalty{5.f};
    float oversteer_brake_penalty{0.25f};
    float oversteer_steering_speed{0.7f};
    float understeer_speed_penalty{3.f};
    float slide_resistance{20.f};
    float steering_inward_speed{0.1f};

    HashSet<Node*> grounded_objects{};

    CollisionShape3D *front_wheels{nullptr};
    CollisionShape3D *back_wheels{nullptr};

    float current_understeer{0.f};
    float current_oversteer{0.f};
};
}

#endif // !CAR_PHYSICS_HPP
