#include "foster_student.h"

void foster_init(FosterStudent* fs, float t_critical)
{
    if (!fs) return;
    
    for (int i = 0; i < FS_WINDOW_SIZE; i++) {
        fs->window[i] = 0.0f;
    }
    
    fs->t_critical = t_critical;
    fs->mse = 1.0f;
    fs->index = 0;
    fs->count = 0;
    fs->ready = false;
}

void foster_set_mse(FosterStudent* fs, float mse)
{
    if (!fs || mse <= 0.0f) return;
    fs->mse = mse;
}

void foster_add_sample(FosterStudent* fs, float value)
{
    if (!fs) return;
    
    fs->window[fs->index] = value;
    fs->index = (fs->index + 1) % FS_WINDOW_SIZE;
    
    if (fs->count < FS_WINDOW_SIZE) {
        fs->count++;
        if (fs->count == FS_WINDOW_SIZE) {
            fs->ready = true;
        }
    }
}

uint8_t foster_is_ready(FosterStudent* fs)
{
    return fs && fs->ready;
}

uint8_t foster_check(FosterStudent* fs)
{
    if (!fs || !fs->ready) {
        return 0xFF;
    }
    
    float first = fs->window[(fs->index) % FS_WINDOW_SIZE];
    float max_val = first;
    float min_val = first;
    
    uint8_t m = 0;
    uint8_t l = 0;
    float d_sum = 0.0f;
    
    for (int i = 0; i < FS_WINDOW_SIZE; i++) {
        float val = fs->window[(fs->index + i) % FS_WINDOW_SIZE];
        
        if (val > max_val) {
            max_val = val;
            m = 1;
        } else {
            m = 0;
        }
        
        if (val < min_val) {
            min_val = val;
            l = 1;
        } else {
            l = 0;
        }
        
        d_sum += (float)(m - l);
    }
    
    if (d_sum < 0.0f) d_sum = -d_sum;
    
    if (fs->mse > 0.0f) {
        float t_value = d_sum / fs->mse;
      
        fs->t_current = t_value;
        
        return (t_value > fs->t_critical) ? 1 : 0;
    }
    
    return 0xFF;
}