#include <Preferences.h>
#include <math.h>
#include "calibration.h"
#include "sensor_node_config.h"

namespace {
Preferences preferences;
float angleDelta(float a, float b) {
    float delta = fmodf(a - b + 540.0f, 360.0f) - 180.0f;
    return delta;
}
float normalizeAngle(float angle) {
    return fmodf(angle + 540.0f, 360.0f) - 180.0f;
}
void saveCalibration() {
    preferences.begin("sensor_cal", false);
    preferences.putBool("level_valid", levelCalibration.valid);
    preferences.putFloat("pitch_zero", levelCalibration.pitchOffsetDegrees);
    preferences.putFloat("roll_zero", levelCalibration.rollOffsetDegrees);
    preferences.end();
}
}

LevelCalibrationState levelCalibration;

void initLevelCalibration() {
    preferences.begin("sensor_cal", true);
    levelCalibration.valid = preferences.getBool("level_valid", false);
    levelCalibration.pitchOffsetDegrees = preferences.getFloat("pitch_zero", 0);
    levelCalibration.rollOffsetDegrees = preferences.getFloat("roll_zero", 0);
    preferences.end();
}

bool startLevelCalibration(const ImuState& imu) {
    if (!imu.online || !imu.valid || millis() - imu.lastUpdateMs > SENSOR_STALE_MS) return false;
    levelCalibration.active = true;
    levelCalibration.startedMs = millis();
    levelCalibration.lastSampleMs = 0;
    levelCalibration.sampleCount = 0;
    levelCalibration.pitchSum = 0;
    levelCalibration.rollSinSum = 0;
    levelCalibration.rollCosSum = 0;
    levelCalibration.firstPitch = imu.pitchDegrees;
    levelCalibration.firstRoll = imu.rollDegrees;
    return true;
}

bool updateLevelCalibration(const ImuState& imu, String& result) {
    if (!levelCalibration.active) return false;
    if (!imu.valid || millis() - imu.lastUpdateMs > SENSOR_STALE_MS) {
        levelCalibration.active = false;
        result = "imu_unavailable";
        return true;
    }
    if (imu.lastUpdateMs == levelCalibration.lastSampleMs) return false;
    levelCalibration.lastSampleMs = imu.lastUpdateMs;

    if (fabsf(imu.pitchDegrees - levelCalibration.firstPitch) > LEVEL_CALIBRATION_MAX_MOVEMENT_DEGREES ||
        fabsf(angleDelta(imu.rollDegrees, levelCalibration.firstRoll)) > LEVEL_CALIBRATION_MAX_MOVEMENT_DEGREES) {
        levelCalibration.active = false;
        result = "vehicle_moved";
        return true;
    }

    levelCalibration.pitchSum += imu.pitchDegrees;
    levelCalibration.rollSinSum += sinf(imu.rollDegrees * DEG_TO_RAD);
    levelCalibration.rollCosSum += cosf(imu.rollDegrees * DEG_TO_RAD);
    levelCalibration.sampleCount++;

    if (millis() - levelCalibration.startedMs < LEVEL_CALIBRATION_DURATION_MS) return false;
    if (levelCalibration.sampleCount < LEVEL_CALIBRATION_MIN_SAMPLES) {
        levelCalibration.active = false;
        result = "insufficient_samples";
        return true;
    }

    levelCalibration.pitchOffsetDegrees = levelCalibration.pitchSum / levelCalibration.sampleCount;
    levelCalibration.rollOffsetDegrees = atan2f(levelCalibration.rollSinSum, levelCalibration.rollCosSum) * RAD_TO_DEG;
    levelCalibration.valid = true;
    levelCalibration.active = false;
    saveCalibration();
    result = "complete";
    return true;
}

void clearLevelCalibration() {
    levelCalibration = LevelCalibrationState();
    saveCalibration();
}

float calibratedPitchDegrees(const ImuState& imu) {
    return levelCalibration.valid ? imu.pitchDegrees - levelCalibration.pitchOffsetDegrees : imu.pitchDegrees;
}

float calibratedRollDegrees(const ImuState& imu) {
    return levelCalibration.valid ? normalizeAngle(imu.rollDegrees - levelCalibration.rollOffsetDegrees) : imu.rollDegrees;
}