#pragma once
#ifndef LIB_INTER
#define LIB_INTER

#ifdef __cplusplus
extern "C" {
#endif

/////////////////////////////////////////////////////
//
// ENUMS & STRUCTS
//
/////////////////////////////////////////////////////

typedef struct lib_inter_interpolator {
    zpl_vec3 start;
    zpl_vec3 target;
    zpl_vec3 error;
    f64 start_time;
    f64 finish_time;
    f32 last_alpha;
    f32 interp_time;
};

#ifdef __cplusplus
}
#endif

/////////////////////////////////////////////////////
//
// DECL
//
/////////////////////////////////////////////////////

lib_inter_interpolator*     lib_inter_create_iterpolator();
void                        lib_inter_destroy_interpolator(lib_inter_interpolator*);

zpl_vec3                    lib_inter_interpolate(lib_inter_interpolator*, zpl_vec3);
zpl_vec3                    lib_inter_set_target(lib_inter_interpolator*, zpl_vec3);

/////////////////////////////////////////////////////
//
// IMPLEMENTATION
//
/////////////////////////////////////////////////////

#ifdef LIB_INTER_IMPLEMENTATION

lib_inter_interpolator* lib_inter_create_iterpolator() {
    return (lib_inter_interpolator*)zpl_malloc(sizeof(lib_inter_interpolator));
}

void lib_inter_destroy_interpolator(lib_inter_interpolator* interpolator) {
    if (interpolator) {
        zpl_free(zpl_heap(), interpolator);
        interpolator = NULL;
    }
}

zpl_vec3 lib_inter_interpolate(lib_inter_interpolator* interp, zpl_vec3 current_val) {
    if (interp->finish_time > 0.0f) {
        f64 current_time = zpl_time_now();
        f32 alpha = zpl_unlerp(current_time, interp->start_time, interp->finish_time);

        // Don't let it to overcompensate the error
        alpha = zpl_clamp(alpha, 0.0f, 1.0f);

        // Get the current error portion to compensate
        f32 current_alpha = alpha - interp->last_alpha;
        interp->last_alpha = alpha;

        zpl_vec3 compensation;
        zpl_vec3_lerp(&compensation, zpl_vec3f_zero(), interp->error, current_alpha);

        // If we finished compensating the error, finish it for the next pulse
        if (alpha == 1.0f) {
            interp->finish_time = 0;
        }

        zpl_vec3 compensated;
        zpl_vec3_add(&compensated, current_val, compensation);
        return compensated;
    }
}

zpl_vec3 lib_inter_set_target(lib_inter_interpolator* interp, zpl_vec3 current_val, zpl_vec3 target_val) {

    interp->start = current_val;
    interp->target = target_val;

    zpl_vec3 sub_error;
    zpl_vec3_sub(&sub_error, target_val, current_val);
    interp->error = sub_error;

    auto error_mag = zpl_vec3_mag(interp->error);
    if (error_mag > 0.1f) {
        interp->start = target_val;
    }

    // Apply the error over 250ms (i.e. 2/5 per 100ms )
    zpl_vec3_mul(&interp->error, interp->error, zpl_lerp(0.25f, 1.0f, zpl_clamp01(zpl_unlerp(interp->interp_time, 0.1f, 0.4f))));

    interp->start_time  = zpl_time_now();
    interp->finish_time = interp->start_time + interp->interp_time;
    interp->last_alpha  = 0.0f;
}

#endif

#endif