// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md

#include <log/log.hpp>

#include "PowerManager.hpp"

namespace sys
{
    PowerManager::PowerManager()
    {
        lowPowerControl = bsp::LowPowerMode::Create().value_or(nullptr);
        driverSEMC      = drivers::DriverSEMC::Create("ExternalRAM");
        cpuGovernor     = std::make_unique<CpuGovernor>();
    }

    PowerManager::~PowerManager()
    {}

    int32_t PowerManager::PowerOff()
    {
        return lowPowerControl->PowerOff();
    }

    int32_t PowerManager::Reboot()
    {
        return lowPowerControl->Reboot();
    }

    void PowerManager::UpdateCpuFrequency(uint32_t cpuLoad)
    {
        const auto currentCpuFreq        = lowPowerControl->GetCurrentFrequencyLevel();
        const auto minFrequencyRequested = cpuGovernor->GetMinimumFrequencyRequested();

        if (cpuLoad > frequencyShiftUpperThreshold && currentCpuFreq < bsp::CpuFrequencyHz::Level_6) {
            aboveThresholdCounter++;
            belowThresholdCounter = 0;
        }
        else if (cpuLoad < frequencyShiftLowerThreshold && currentCpuFreq > bsp::CpuFrequencyHz::Level_1) {
            belowThresholdCounter++;
            aboveThresholdCounter = 0;
        }
        else {
            ResetFrequencyShiftCounter();
        }

        if (aboveThresholdCounter >= maxAboveThresholdCount || minFrequencyRequested > currentCpuFreq) {
            ResetFrequencyShiftCounter();
            IncreaseCpuFrequency();
        }
        else {
            if (belowThresholdCounter >= maxBelowThresholdCount && currentCpuFreq > minFrequencyRequested) {
                ResetFrequencyShiftCounter();
                DecreaseCpuFrequency();
            }
        }
    }

    void PowerManager::IncreaseCpuFrequency() const
    {
        const auto freq      = lowPowerControl->GetCurrentFrequencyLevel();
        const auto oscSource = lowPowerControl->GetCurrentOscillatorSource();

        if (freq == bsp::CpuFrequencyHz::Level_1) {
            // switch osc source first
            if (oscSource == bsp::LowPowerMode::OscillatorSource::Internal) {
                lowPowerControl->SwitchOscillatorSource(bsp::LowPowerMode::OscillatorSource::External);
            }

            // then switch external RAM clock source
            if (driverSEMC) {
                driverSEMC->SwitchToPLL2ClockSource();
            }
        }

        // and increase frequency
        if (freq < bsp::CpuFrequencyHz::Level_6) {
            SetCpuFrequency(bsp::CpuFrequencyHz::Level_6);
        }
    }

    void PowerManager::DecreaseCpuFrequency() const
    {
        const auto freq = lowPowerControl->GetCurrentFrequencyLevel();
        auto level      = bsp::CpuFrequencyHz::Level_1;

        switch (freq) {
        case bsp::CpuFrequencyHz::Level_6:
            level = bsp::CpuFrequencyHz::Level_5;
            break;
        case bsp::CpuFrequencyHz::Level_5:
            level = bsp::CpuFrequencyHz::Level_4;
            break;
        case bsp::CpuFrequencyHz::Level_4:
            level = bsp::CpuFrequencyHz::Level_3;
            break;
        case bsp::CpuFrequencyHz::Level_3:
            level = bsp::CpuFrequencyHz::Level_2;
            break;
        case bsp::CpuFrequencyHz::Level_2:
            level = bsp::CpuFrequencyHz::Level_1;
            break;
        case bsp::CpuFrequencyHz::Level_1:
            break;
        }

        // decrease frequency first
        if (level != freq) {
            SetCpuFrequency(level);
        }

        if (level == bsp::CpuFrequencyHz::Level_1) {
            const auto oscSource = lowPowerControl->GetCurrentOscillatorSource();

            // then switch osc source
            if (oscSource == bsp::LowPowerMode::OscillatorSource::External) {
                lowPowerControl->SwitchOscillatorSource(bsp::LowPowerMode::OscillatorSource::Internal);
            }

            // and switch external RAM clock source
            if (driverSEMC) {
                driverSEMC->SwitchToPeripheralClockSource();
            }
        }
    }

    void PowerManager::RegisterNewSentinel(std::shared_ptr<CpuSentinel> newSentinel) const
    {
        cpuGovernor->RegisterNewSentinel(newSentinel);
    }

    void PowerManager::SetCpuFrequencyRequest(std::string sentinelName, bsp::CpuFrequencyHz request) const
    {
        cpuGovernor->SetCpuFrequencyRequest(sentinelName, request);
    }

    void PowerManager::SetCpuFrequency(bsp::CpuFrequencyHz freq) const
    {
        lowPowerControl->SetCpuFrequency(freq);
        cpuGovernor->InformSentinelsAboutCpuFrequencyChange(freq);
    }

    void PowerManager::ResetFrequencyShiftCounter()
    {
        aboveThresholdCounter = 0;
        belowThresholdCounter = 0;
    }

    [[nodiscard]] auto PowerManager::getExternalRamDevice() const noexcept -> std::shared_ptr<devices::Device>
    {
        return driverSEMC;
    }

} // namespace sys