/*
 * FBKW.c
 *
 *  Created on: Nov 20, 2024
 *      Author: herli
 */
#include <id.h>
#include "FBKW.h"
#include <stdint.h>
#include <math.h>
#include "injection.h"
#include "toothed_wheel.h"
#include "pre_injection.h"
#include "temperature.h"

float FBKW_DEMAND = 0.0;
float FBKW_DC = 5;
float FBKW_FEEDBACK = 0.0;

float B_FB_KW = 0.0;

float DEMAND_filtered = 0.0;
extern float B_PHIAD;

uint8_t CKP_PULSE_AVAILABLE = 0;
uint8_t FBKW_PID_OPEN = 0; //estaba en 0 antes, igual asi no se peta el pid si no recibe avance

void UpdatePWM(TIM_HandleTypeDef* pwmHandle, uint32_t pwmChannel, uint16_t period_on, uint16_t period){

	if(period_on > period)
	{
		period_on = period;
	}
	pwmHandle->Instance->ARR = period;
	pwmHandle->Instance->CCR2 = period_on;

}
/* =======================================================================
 * VP44 reverse-engineered pipeline bridge
 * -----------------------------------------------------------------------
 * The pipeline in pwm.c consumes 9 integer inputs through a getter vtable.
 * Each getter below converts from the project's native float/int variables
 * into the integer units the pipeline expects. Getters marked TODO return
 * zero until the correct source variable is wired up.
 *
 * Pipeline output `rt.pwm_duty` is a 12-bit (0..4095) duty command; it is
 * converted to a percentage and fed to UpdateFBKW_MODULATION for htim4/CH2.
 * =======================================================================
 */

static pwm_runtime_t        fbkw_rt;
static pwm_input_getters_t  fbkw_getters;

#define CF_KW 85.3333
#define CF_TMS 8.388
#define CF_ME 32
#define CF_T_M 16
#define CF_T_N 4368
#define CF_VOLT 54

/* ckp_in (0x02f8): sensed plunger position — fed by the CKP-derived
 * feedback. FBKW_FEEDBACK is already the plunger-position estimate in
 * this project. */
static int16_t fbkw_get_ckp_in(void *ctx) {
    (void)ctx;
    return (int16_t)(FBKW_FEEDBACK*CF_KW);
}

/* rpm (0x0040): engine RPM as uint16. */
static uint16_t fbkw_get_rpm(void *ctx) {
    (void)ctx;
    float r = RPM;
    if (r < 0.0f) r = 0.0f;
    if (r > 65535.0f) r = 65535.0f;
    return (uint16_t)(r*CF_TMS); //this is the value that uses the original, teeths/ms so 3degree n /ms.
}

/* angle_dec_cmd (0x0042): CAN angle-decrease command.
 * TODO: wire to the real source when available (was CAN-delivered on VP44). */
static int16_t fbkw_get_angle_dec_cmd(void *ctx) {
    (void)ctx;
    return (int16_t)(B_PHIAD*CF_KW);
}

/* inj_qty_demand (0x0044): injection quantity demand.
 * TODO: wire to project's injection-qty variable if/when exposed. */
static int16_t fbkw_get_inj_qty_demand(void *ctx) {
    (void)ctx;
    return (uint16_t)(ME*CF_ME);
}


/* b_fb_kw (0x02b4): KW plunger-feedback demand (baseline setpoint term).
 * Same semantic meaning as the project's B_FB_KW global. */
static int16_t fbkw_get_b_fb_kw(void *ctx) {
    (void)ctx;
    return (int16_t)(B_FB_KW*CF_KW);
}

/* state_130 (0x0130): CAN-delivered open/closed-loop discriminant.
 * TODO: wire when the corresponding CAN signal is exposed. Using
 * FBKW_PID_OPEN as a provisional mapping (negative = open-loop sentinel). */
static int16_t fbkw_get_state_130(void *ctx) {
    (void)ctx;
    return 0;
    //return (int16_t)(CKP_PULSE_AVAILABLE ? 0 : -1);

}

/* supply_voltage (0x0142): battery/supply voltage.
 * TODO: wire to the project's VBAT ADC reading. */
static uint16_t fbkw_get_supply_voltage(void *ctx) {
    (void)ctx;
    return (uint16_t)(736);//13.5 * its factor 54
}

/* temperature (0x0146): temperature input.
 * Temp is already a project-wide temperature variable (temperature.h). */
static int16_t fbkw_get_temperature(void *ctx) {
    (void)ctx;
    return (int16_t)(Temp*CF_T_M+CF_T_N);
}

void FBKW_init(void) {
    fbkw_getters.ckp_in         = fbkw_get_ckp_in;
    fbkw_getters.rpm            = fbkw_get_rpm;
    fbkw_getters.angle_dec_cmd  = fbkw_get_angle_dec_cmd;
    fbkw_getters.inj_qty_demand = fbkw_get_inj_qty_demand;
    fbkw_getters.b_fb_kw        = fbkw_get_b_fb_kw;
    fbkw_getters.state_130      = fbkw_get_state_130;
    fbkw_getters.supply_voltage = fbkw_get_supply_voltage;
    fbkw_getters.temperature    = fbkw_get_temperature;
    fbkw_getters.ctx            = NULL;

    pwm_init(&fbkw_rt, &pwm_cal_rom, &pwm_flash_rom, &fbkw_getters);
}

uint8_t last_ckp = 1;
void FBKW_service(void) {
    /* Signal a reset edge to the supervisor when CKP pulses are not yet
     * available — matches the VP44 boot/CKP-lost reset behavior. */
    if (!CKP_PULSE_AVAILABLE) {
    	if(last_ckp){
            //fbkw_rt.system_flags_110 = 0x30;
            fbkw_rt.system_flags_110 |= 0x20u;   // set ONLY bit 5 (OL gate). Leave other bits alone.


#if defined(T06211)
#elif defined(T06301)
            fbkw_rt.mode_flags = 1;//muy importantes estas mierdas para que en open loop vaya como toca.
#endif
    	}
        fbkw_rt.reset_flag = 1;
    }else{
    	if(!last_ckp){
            fbkw_rt.system_flags_110 &= ~0x20u;   // clear ONLY bit 5 (OL gate). Leave other bits alone.

#if defined(T06211)
            fbkw_rt.pi_open_loop_flag = 0x00; //0x2000
#endif
    	}
    }

    last_ckp = CKP_PULSE_AVAILABLE;

    pwm_service(&fbkw_rt);

    UpdatePWM(&htim4, TIM_CHANNEL_2, fbkw_rt.pwm_on_time, fbkw_rt.pwm_period);
    FBKW_DC = ((float)fbkw_rt.pwm_duty) * (100.0f / 4095.0f);
    FBKW_DEMAND = fbkw_rt.target_5e * (3.0f / 256.0f);
}
void FBKW_CKP_ISR(void) {
    pwm_ckp_isr(&fbkw_rt);
}

const pwm_runtime_t *FBKW_pipeline_runtime(void) {
    return &fbkw_rt;
}