#include "heater_controller.h"

#include "diagnostic_queue.h"
#include "integer_arithmetic.h"
#include "numeric_conversion.h"
#include "program_memory.h"
#include "shot_control.h"
#include "telemetry_reports.h"

namespace meCoffee {

namespace {

constexpr int16_t kMinimumUsableAggregate = 100;
constexpr uint16_t kMinimumMappedAmplitude = 80;

int16_t signedSetpointForMode(const FirmwareState& state) {
    return state.alternateModeSelection == 0 ? state.settings.brewSetpoint : state.settings.steamSetpoint;
}

int32_t lowerAccumulatorBound(const SettingsValues& settings) {
    return signedDivide(100 * static_cast<int32_t>(settings.integralAccumulatorMinimum), settings.integralGain);
}

int32_t upperAccumulatorBound(const SettingsValues& settings) {
    return signedDivide(100 * static_cast<int32_t>(settings.integralAccumulatorMaximum), settings.integralGain);
}

int32_t clampAccumulatorCandidate(const SettingsValues& settings, int32_t candidate) {
    const int32_t lowerBound = lowerAccumulatorBound(settings);
    const int32_t afterLower = candidate > lowerBound ? candidate : lowerBound;
    const int32_t upperBound = upperAccumulatorBound(settings);
    return afterLower < upperBound ? afterLower : upperBound;
}

uint16_t mapDemandToAmplitudeCached(HeaterState* heater, uint16_t demand, uint16_t filteredPhase) {
    if (!heater->hasMappedAmplitude || heater->mappedDemand != demand || heater->mappedPhase != filteredPhase) {
        heater->mappedAmplitude = mapDemandToAmplitude(demand, filteredPhase);
        heater->mappedDemand = demand;
        heater->mappedPhase = filteredPhase;
        heater->hasMappedAmplitude = true;
    }
    return heater->mappedAmplitude;
}

void writeHeaterAmplitudeThroughWrapper(FirmwareState* state, uint16_t demand) {
    uint16_t mappedAmplitude = mapDemandToAmplitudeCached(&state->heater, demand, state->mainsPhase.filteredTicksPerHalfCycle);
    if (mappedAmplitude < kMinimumMappedAmplitude) {
        mappedAmplitude = 0;
    }
    setGroupAmplitude(&groupForRole(&state->outputChannels, OutputGroupRole::Boiler), mappedAmplitude);
}

void reportHeaterEnableChange(FirmwareState* state, bool shouldEnable) {
    enqueueDiagnosticProgramMemoryText(&state->diagnosticQueue,
                                       shouldEnable ? MECOFFEE_PROGRAM_TEXT("pid 65536 0 0 0") : MECOFFEE_PROGRAM_TEXT("pid 0 0 0 0"));
}

}  // namespace

uint16_t computeDemandFromTerms(int32_t proportional, int32_t integral, int32_t derivative, int32_t feedForward) {
    const uint32_t wrappedSum = static_cast<uint32_t>(proportional) + static_cast<uint32_t>(integral) +
                                static_cast<uint32_t>(derivative) + static_cast<uint32_t>(feedForward);
    const int32_t signedSum = static_cast<int32_t>(wrappedSum);
    int32_t clamped = signedSum;
    if (clamped < 0) {
        clamped = 0;
    } else if (clamped > 65535) {
        clamped = 65535;
    }
    return static_cast<uint16_t>(clamped >> 6);
}

uint16_t mapDemandToAmplitude(uint16_t demand, uint16_t filteredPhase) {
    const float scaledDemand = static_cast<float>(demand) * 3476.47998046875f;
    const uint32_t floorOfScaledDemand = convertFloatToUnsigned32(scaledDemand, nullptr);
    const int32_t first = static_cast<int16_t>(static_cast<uint16_t>(floorOfScaledDemand >> 10));
    const uint32_t demandWide = demand;
    const uint32_t second = (demandWide * demandWide * 2303u) >> 20;
    const uint32_t residual = static_cast<uint32_t>(first) - second - 2396u;
    const int32_t demandTimesResidual = static_cast<int32_t>(demandWide * residual);
    const uint32_t intermediate = static_cast<uint32_t>(signedDivideBy1024(demandTimesResidual)) + 1418u;
    const int32_t phaseTimesIntermediate = static_cast<int32_t>(static_cast<uint32_t>(filteredPhase) * intermediate);
    return static_cast<uint16_t>(signedDivide(phaseTimesIntermediate, 1608));
}

void initializeHeaterController(FirmwareState* state, uint32_t currentMilliseconds) {
    HeaterState& heater = state->heater;
    heater = HeaterState{};
    heater.integralAccumulator = kInitialIntegralAccumulator;
    heater.previousAggregate = kInitialPreviousAggregate;
    heater.periodTimerBaseline = currentMilliseconds;
}

void updateControllerPeriodTimer(FirmwareState* state, uint32_t foregroundSnapshotMilliseconds) {
    HeaterState& heater = state->heater;
    if (static_cast<uint32_t>(foregroundSnapshotMilliseconds - heater.periodTimerBaseline) > state->settings.controllerUpdatePeriod) {
        heater.isPeriodElapsed = true;
        heater.periodTimerBaseline = foregroundSnapshotMilliseconds;
    } else {
        heater.isPeriodElapsed = false;
    }
}

void resetIntegralAccumulator(FirmwareState* state) {
    const SettingsValues& settings = state->settings;
    const uint16_t setpoint = static_cast<uint16_t>(settings.brewSetpoint);
    const uint16_t measurement = static_cast<uint16_t>(state->sensor.firstChannelAggregate);
    const uint16_t difference = static_cast<uint16_t>(setpoint - measurement);
    const uint16_t span = static_cast<uint16_t>(settings.integralAccumulatorMaximum - settings.integralAccumulatorMinimum);
    uint32_t candidate = unsignedDivide(static_cast<uint32_t>(difference) * span, setpoint);
    candidate += settings.integralAccumulatorMinimum;
    candidate *= 100u;
    candidate = unsignedDivide(candidate, settings.integralGain);
    state->heater.integralAccumulator = clampAccumulatorCandidate(settings, static_cast<int32_t>(candidate));
}

void runHeaterController(FirmwareState* state, uint32_t foregroundSnapshotMilliseconds) {
    HeaterState& heater = state->heater;
    const SettingsValues& settings = state->settings;
    OutputGroup& boiler = groupForRole(&state->outputChannels, OutputGroupRole::Boiler);

    const int16_t aggregate = state->sensor.firstChannelAggregate;
    if (aggregate < kMinimumUsableAggregate) {
        setGroupEnabled(&boiler, false);
        return;
    }

    if (state->alternateModeSelection != 0 && heater.bypassSelector == 0) {
        setGroupAmplitude(&boiler, 0);
        const bool shouldEnable = aggregate < settings.steamSetpoint;
        if (shouldEnable != (boiler.isEnabled != 0)) {
            reportHeaterEnableChange(state, shouldEnable);
        }
        setGroupEnabled(&boiler, shouldEnable);
        return;
    }

    if (heater.isPeriodElapsed) {
        const int16_t setpoint = signedSetpointForMode(*state);
        const int16_t measurement = state->sensor.firstChannelAggregate;
        const int16_t error = static_cast<int16_t>(static_cast<uint16_t>(setpoint) - static_cast<uint16_t>(measurement));
        heater.proportionalTerm = static_cast<int32_t>(error) * static_cast<int32_t>(settings.proportionalGain);

        bool isIntegrationFrozen = false;
        if (heater.shotBeginMarker != 0) {
            if (heater.shotEndMarker == 0) {
                isIntegrationFrozen = true;
            } else if (foregroundSnapshotMilliseconds < heater.shotEndMarker + heater.retainedLockDuration) {
                isIntegrationFrozen = true;
            } else {
                heater.shotEndMarker = 0;
                heater.shotBeginMarker = 0;
            }
        }
        const uint16_t warmThreshold = static_cast<uint16_t>(static_cast<uint16_t>(setpoint) - (static_cast<uint16_t>(setpoint) >> 4));
        if (!isIntegrationFrozen && static_cast<uint16_t>(measurement) > warmThreshold) {
            const int32_t candidate = static_cast<int32_t>(static_cast<uint32_t>(heater.integralAccumulator) + static_cast<uint32_t>(error));
            heater.integralAccumulator = clampAccumulatorCandidate(settings, candidate);
        }
        heater.integralTerm = signedDivide(
            static_cast<int32_t>(static_cast<uint32_t>(settings.integralGain) * static_cast<uint32_t>(heater.integralAccumulator)), 100);

        const int16_t delta = static_cast<int16_t>(heater.previousAggregate - static_cast<uint16_t>(measurement));
        const int32_t derivativeBase = static_cast<int32_t>(delta) * static_cast<int32_t>(settings.derivativeGain);
        const uint32_t factor = (heater.shotBeginMarker != 0 && delta < 0) ? 2u : 1u;
        heater.derivativeTerm = static_cast<int32_t>(static_cast<uint32_t>(derivativeBase) * factor);
        heater.previousAggregate = static_cast<uint16_t>(measurement);

        heater.demand = computeDemandFromTerms(heater.proportionalTerm, heater.integralTerm, heater.derivativeTerm, heater.feedForwardTerm);
        enqueueDiagnosticCallback(&state->diagnosticQueue, reportHeaterTermsFrame);

        if (settings.continuousHeaterControlEnabled != 0) {
            writeHeaterAmplitudeThroughWrapper(state, heater.demand);
            setGroupEnabled(&boiler, true);
        } else {
            setGroupAmplitude(&boiler, 0);
        }
    }

    if (settings.continuousHeaterControlEnabled != 0) {
        const uint32_t sampleScale = state->sensor.firstRangeFlag == 0 ? 11000u : 22000u;
        const uint16_t continuousMeasurement = static_cast<uint16_t>((static_cast<uint32_t>(state->sensor.firstRing[0]) * sampleScale) >> 10);
        const int16_t setpoint = signedSetpointForMode(*state);
        const int16_t error = static_cast<int16_t>(static_cast<uint16_t>(setpoint) - continuousMeasurement);
        heater.proportionalTerm = static_cast<int32_t>(error) * static_cast<int32_t>(settings.proportionalGain);
        heater.demand = computeDemandFromTerms(heater.proportionalTerm, heater.integralTerm, heater.derivativeTerm, heater.feedForwardTerm);
        writeHeaterAmplitudeThroughWrapper(state, heater.demand);
    } else {
        const uint16_t onTime = static_cast<uint16_t>(5u * heater.demand);
        setGroupEnabled(&boiler, static_cast<uint32_t>(foregroundSnapshotMilliseconds - heater.periodTimerBaseline) < onTime);
    }

    if (isInputQualified(state->shotInput)) {
        const uint32_t shotDuration = foregroundSnapshotMilliseconds - state->shotInput.beginTimestamp;
        const uint16_t pressureDemand = computePressureDemand(settings, shotDuration);
        setGroupAmplitude(&groupForRole(&state->outputChannels, OutputGroupRole::Pump),
                          mapDemandToAmplitude(pressureDemand, state->mainsPhase.filteredTicksPerHalfCycle));
    }
}

void beginShotFeedForward(FirmwareState* state, uint32_t currentMilliseconds) {
    HeaterState& heater = state->heater;
    heater.shotBeginMarker = currentMilliseconds;
    heater.shotEndMarker = 0;
    const int32_t product = static_cast<int32_t>(65535u * static_cast<uint32_t>(state->settings.shotFeedForwardPercentage));
    const float quotient = static_cast<float>(product) / 100.0f;
    heater.feedForwardTerm = convertFloatToSigned32(quotient);
}

void endShotFeedForward(FirmwareState* state, uint32_t currentMilliseconds) {
    HeaterState& heater = state->heater;
    if (heater.shotBeginMarker != 0) {
        heater.shotEndMarker = currentMilliseconds;
        heater.retainedLockDuration = static_cast<uint32_t>(state->settings.shotFeedForwardLockDuration) * 1000u;
    }
    heater.feedForwardTerm = 0;
}

uint16_t computePressureDemand(const SettingsValues& settings, uint32_t durationMilliseconds) {
    const int16_t delta = static_cast<int16_t>(settings.pressureProfileEnd - settings.pressureProfileStart);
    const float denominator = static_cast<float>(settings.pressureProfileDuration) * 1000.0f;
    const float ratio = static_cast<float>(durationMilliseconds) / denominator;
    const float value = ratio * static_cast<float>(delta) + static_cast<float>(static_cast<int16_t>(settings.pressureProfileStart));
    const uint16_t scaledValue = static_cast<uint16_t>(convertFloatToUnsigned32(value, nullptr));
    const float scaled = static_cast<float>(scaledValue) * 1024.0f / 100.0f;
    if (scaled > 1023.0f) {
        return 1023;
    }
    return static_cast<uint16_t>(convertFloatToUnsigned32(scaled, nullptr));
}

}  // namespace meCoffee