/*
 * mppt.c
 *
 *  Created on: Jun 11, 2025
 *      Author: janik
 */


#include "mppt.h"
#include <math.h> // for fabsf

void MPPT_IncCond_Init(MPPTController *mppt,
                       float initial_iref,
                       float step,
                       float iref_min,
                       float iref_max,
					   float dv_min)
{
    mppt->last_vin = 0.0f;
    mppt->last_iin = 0.0f;
    mppt->iref = initial_iref;
    mppt->step = step;
    mppt->iref_min = iref_min;
    mppt->iref_max = iref_max;
    mppt->dv_min = dv_min;
    mppt->deadband = 0.005f;
}

float MPPT_IncCond_Update(MPPTController *mppt, float vin, float iin)
{
    float dv = vin - mppt->last_vin;
    float di = iin - mppt->last_iin;

    // Skip if input voltage is essentially zero
    if (vin < 1e-2f)
        return mppt->iref;

    // Suppress update if both dv and di are below noise floor
    if (fabsf(dv) < mppt->dv_min && fabsf(di) < mppt->dv_min)
        return mppt->iref;

    /* dP = vin*di + iin*dv  (avoids dv division, works even when dv is small) */
    float dp = vin * di + iin * dv;
    float p = vin * iin; /* current power (mV*mA = nW scale) */

    /* Relative deadband: |dP| < 0.5% of P => near MPP */
    if (p > 1e3f && fabsf(dp) < mppt->deadband * fabsf(p)) {
        // At or near MPP — don't step
    } else if (dp > 0) {
        mppt->iref -= mppt->step;
    } else {
        mppt->iref += mppt->step;
    }

    // Clamp
    if (mppt->iref > mppt->iref_max) mppt->iref = mppt->iref_max;
    if (mppt->iref < mppt->iref_min) mppt->iref = mppt->iref_min;

    // Save state
    mppt->last_vin = vin;
    mppt->last_iin = iin;

    return mppt->iref;
}