/*
 * fuel_map.c — T06215 variant
 *
 * Thin wrapper that exposes the legacy host-app API
 * (init_FuelMap / FM_GET_PHIAD) on top of the reverse-engineered
 * T06215 phi pipeline. The original float trilinear interpolation
 * (5 fuel maps × RPM × ME plus a cosine empirical correction) has
 * been removed — the phi pipeline is now the sole angle-calculation
 * source.
 *
 * Mapping:
 *   FM_GET_PHIAD(RPM, ME, Temp) -> phi_outputs_t.angle_accumulator
 *                                  (RW52, "0052 accumulator")
 *                                  scaled to float degrees (/ 85.333).
 *
 * Getter scale factors mirror example_interface_getters.txt:
 *   RPM  raw = real_rpm × 8.388     (CF_TMS)
 *   ME   raw = real_me  × 32        (CF_ME)
 *   Temp raw = real_°C  × 16 + 4368 (CF_T_M, CF_T_N)
 */

#include "fuel_map.h"
#include "phi.h"
#include <stdint.h>
#include <stddef.h>

#define CF_TMS  8.388f
#define CF_ME   32.0f
#define CF_T_M  16.0f
#define CF_T_N  4368.0f
#define CF_KW   85.3333f
#define ANGLE_DEG_PER_RAW (1.0f / 85.333f)

/* Per-call input cache. FM_GET_PHIAD's float args land here so the
 * nullary phi getters can read them. */
static float s_rpm  = 0.0f;
static float s_me   = 0.0f;
static float s_temp = 0.0f;

/* Encapsulated phi runtime — opaque to the caller. */
static phi_state_t          s_phi_state;
static phi_cal_t            s_phi_cal;
static phi_input_getters_t  s_phi_getters;
static phi_outputs_t        s_phi_out;
static float               *s_phiad_out = NULL;

/* ──── Getters (nullary; T06215 vtable shape) ────────────────────────── */

static uint16_t get_rpm(void)              { return (uint16_t)(s_rpm  * CF_TMS); }
static int16_t  get_inj_qty_demand(void)   { return (int16_t) (s_me   * CF_ME); }
static int16_t  get_temperature(void)      { return (int16_t) (s_temp * CF_T_M + CF_T_N); }

/* angle-decrease CAN command. Default 0 matches sim_t06215_sweep.c.
 * To wire this to a host-app global, replace the body with e.g.:
 *     extern float B_PHIAD;
 *     return (int16_t)(B_PHIAD * CF_KW);
 */
static int16_t  get_angle_dec_cmd(void)    { return 0; }

/* Steady-state default: baseline tracks current RPM, so Δ-rpm = 0 and
 * the accel-comp branch contributes nothing. Override if modelling
 * transients. */
static int16_t  get_rpm_baseline(void)     { return (int16_t)(s_rpm * CF_TMS); }

static uint16_t get_rwc2(void)             { return 0; }       /* slow timing scratch */
static uint8_t  get_reset_gate_0226(void)  { return 0xFF; }    /* accel-comp enabled  */
static int16_t  get_dphi(void)             { return 0; }
static uint8_t  get_scratch_0103(void)     { return 0xFF; }    /* matches sim_t06215_sweep.c */
static uint8_t  get_scratch_0108(void)     { return 0xFF; }

/* ──── Public API ─────────────────────────────────────────────────────── */

void init_FuelMap(float *PHIAD) {
    s_phiad_out = PHIAD;

    s_phi_getters.get_rpm              = get_rpm;
    s_phi_getters.get_inj_qty_demand   = get_inj_qty_demand;
    s_phi_getters.get_temperature      = get_temperature;
    s_phi_getters.get_angle_dec_cmd    = get_angle_dec_cmd;
    s_phi_getters.get_rpm_baseline     = get_rpm_baseline;
    s_phi_getters.get_rwc2             = get_rwc2;
    s_phi_getters.get_reset_gate_0226  = get_reset_gate_0226;
    s_phi_getters.get_dphi             = get_dphi;
    s_phi_getters.get_scratch_0103     = get_scratch_0103;
    s_phi_getters.get_scratch_0108     = get_scratch_0108;

    s_phi_cal = phi_t06235_cal;
    phi_init(&s_phi_state, &s_phi_cal, &s_phi_getters);

    /* Pre-latch to skip the cranking-phase ramp (mirrors
     * sim_t06215_sweep.c:237-238). With this in place, the very first
     * phi_service call enters FUN_62a2 in Phase 2 and tein_valve_fault_guard
     * holds at cal_48 from boot. */
    s_phi_state.rt.tein_valve_fault_guard = s_phi_cal.cal_48;
    s_phi_state.rt.scratch_010f        = s_phi_state.rt.scratch_0108;
}
void Timer1_FM_ISR(){
	phi_tick_1khz(&s_phi_state, &s_phi_cal);
}

void TW_FM_ISR(uint8_t currentTooth, uint32_t ic){
	//phi_tick_1khz(&s_phi_state, &s_phi_cal);
}

extern float forceTemp;
float FM_GET_PHIAD(float RPM, float ME, float Temp) {
    s_rpm  = RPM;
    s_me   = ME;
    s_temp = Temp;

    phi_service(&s_phi_state, &s_phi_cal, &s_phi_out);

    float Z = (float)s_phi_out.angle_accumulator * ANGLE_DEG_PER_RAW;
    if (s_phiad_out) *s_phiad_out = Z;
    return Z;
}

