meCoffee_in_CPP/lib/meCoffee_controller_logic/src/oscillator_adjustment.cpp

#include "oscillator_adjustment.h"

namespace meCoffee {

OscillatorAdjustmentResult adjustOscillator(uint8_t qualifiedEventCount, int16_t oldOffset, uint8_t baselineByte,
                                            uint8_t oldFractionPeriod) {
    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;
}

}  // namespace meCoffee