meCoffee_in_CPP/lib/meCoffee_controller_logic/src/oscillator_adjustment.cpp

#include "oscillator_adjustment.h"

#if defined(MECOFFEE_RUST_OSCILLATOR)
extern "C" uint32_t mecoffee_rust_adjust_oscillator(uint8_t events, int16_t offset,
                                                 uint8_t baseline, uint8_t period);
#endif

namespace meCoffee {

OscillatorAdjustmentResult adjustOscillator(uint8_t qualifiedEventCount, int16_t oldOffset, uint8_t baselineByte,
                                            uint8_t oldFractionPeriod) {
#if defined(MECOFFEE_RUST_OSCILLATOR)
    const uint32_t packed = mecoffee_rust_adjust_oscillator(qualifiedEventCount, oldOffset, baselineByte, oldFractionPeriod);
    OscillatorAdjustmentResult result;
    result.newOffset = static_cast<int16_t>(static_cast<uint16_t>(packed));
    result.newFractionPeriod = static_cast<uint8_t>(packed >> 16);
    result.calibrationValue = static_cast<uint8_t>(packed >> 24);
    result.shouldWriteCalibration = result.calibrationValue != 0;
    return result;
#else
    OscillatorAdjustmentResult result;
    result.newOffset = oldOffset;
    result.newFractionPeriod = oldFractionPeriod;
    result.shouldWriteCalibration = false;
    result.calibrationValue = 0;

    uint8_t selectedPeriod;
    if (qualifiedEventCount < 105) {
        selectedPeriod = 100;
    } else if (qualifiedEventCount >= 116) {
        selectedPeriod = 120;
    } else {
        return result;
    }
    result.newFractionPeriod = selectedPeriod;

    uint16_t wrappedOffset = static_cast<uint16_t>(oldOffset);
    if (qualifiedEventCount < selectedPeriod) {
        wrappedOffset = static_cast<uint16_t>(wrappedOffset - 1);
    } else if (qualifiedEventCount > selectedPeriod) {
        wrappedOffset = static_cast<uint16_t>(wrappedOffset + 1);
    }
    int16_t clampedOffset = static_cast<int16_t>(wrappedOffset);
    if (clampedOffset < -8) {
        clampedOffset = -8;
    } else if (clampedOffset > 8) {
        clampedOffset = 8;
    }
    result.newOffset = clampedOffset;

    if (clampedOffset != oldOffset) {
        uint8_t calibrationValue = static_cast<uint8_t>(baselineByte + static_cast<uint8_t>(clampedOffset));
        if ((calibrationValue & 0x80) == 0) {
            calibrationValue = 128;
        }
        result.shouldWriteCalibration = true;
        result.calibrationValue = calibrationValue;
    }
    return result;
#endif
}

}  // namespace meCoffee