// Copyright 2022 Haute école d'ingénierie et d'architecture de Fribourg
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/****************************************************************************
 * @file bike_system.cpp
 * @author Serge Ayer <serge.ayer@hefr.ch>
 * @author Rémi Heredero <remi@heredero.ch>
 * @author Yann Sierro <yannsierro.pro@gmail.com>
 *
 * @brief Bike System implementation (static scheduling)
 *
 * @date 2023-11-15
 * @version 1.1.0
 ***************************************************************************/

#include "bike_system.hpp"

#include <chrono>

#include "mbed_trace.h"
#if MBED_CONF_MBED_TRACE_ENABLE
#define TRACE_GROUP "BikeSystem"
#endif  // MBED_CONF_MBED_TRACE_ENABLE

namespace multi_tasking {

BikeSystem::BikeSystem()
    : _gearDevice(),
      _pedalDevice(),
      _resetDevice(callback(this, &BikeSystem::onReset)),
      _speedometer(_timer),
      _cpuLogger(_timer) {}

void BikeSystem::start() {
    // new thread dedicated for ISRs with its event queue

    tr_info("Starting Super-Loop with event handling");

    init();

    _isrEventThread.start(callback(this, &BikeSystem::dispatch_isr_events));
    _mainEventThread.start(callback(this, &BikeSystem::dispatch_events));
}

void BikeSystem::onReset() {
    Event<void()> resetEvent(&_isrEventQueue, callback(this, &BikeSystem::resetTask));
    resetEvent.post();
}

#if defined(MBED_TEST_MODE)
const advembsof::TaskLogger& BikeSystem::getTaskLogger() { return _taskLogger; }
#endif  // defined(MBED_TEST_MODE)

void BikeSystem::init() {
    // start the timer
    _timer.start();

    // initialize the lcd display
    disco::ReturnCode rc = _displayDevice.init();
    if (rc != disco::ReturnCode::Ok) {
        tr_error("Failed to initialized the lcd display: %d", static_cast<int>(rc));
    }

    // initialize the sensor device
    bool present = _sensorDevice.init();
    if (!present) {
        tr_error("Sensor not present or initialization failed");
    }

    // enable/disable task logging
    _taskLogger.enable(true);
}

void BikeSystem::gearTask() {
    // gear task
    auto taskStartTime = _timer.elapsed_time();

    // no need to protect access to data members (single threaded)
    _currentGear     = _gearDevice.getCurrentGear();
    _currentGearSize = _gearDevice.getCurrentGearSize();

    _taskLogger.logPeriodAndExecutionTime(
        _timer, advembsof::TaskLogger::kGearTaskIndex, taskStartTime);
}

void BikeSystem::speedDistanceTask() {
    auto taskStartTime = _timer.elapsed_time();

    const auto pedalRotationTime = _pedalDevice.getCurrentRotationTime();
    _speedometer.setCurrentRotationTime(pedalRotationTime);
    _speedometer.setGearSize(_currentGearSize);

    _currentSpeed     = _speedometer.getCurrentSpeed();
    _traveledDistance = _speedometer.getDistance();

    _taskLogger.logPeriodAndExecutionTime(
        _timer, advembsof::TaskLogger::kSpeedTaskIndex, taskStartTime);
}

void BikeSystem::temperatureTask() {
    auto taskStartTime = _timer.elapsed_time();

    // tr_warn("Tick1 %" PRIu64, _timer.elapsed_time().count());

    // no need to protect access to data members (single threaded)
    _currentTemperature = _sensorDevice.readTemperature();

    // tr_warn("Tick2 %" PRIu64, _timer.elapsed_time().count());

    ThisThread::sleep_for(std::chrono::duration_cast<std::chrono::milliseconds>(
        kTemperatureTaskComputationTime - (_timer.elapsed_time() - taskStartTime)));

    _taskLogger.logPeriodAndExecutionTime(
        _timer, advembsof::TaskLogger::kTemperatureTaskIndex, taskStartTime);
}

void BikeSystem::resetTask() {
    auto taskStartTime = _timer.elapsed_time();

    tr_info("Reset task: response time is %" PRIu64 " usecs", responseTime.count());
    _speedometer.reset();

    _taskLogger.logPeriodAndExecutionTime(
        _timer, advembsof::TaskLogger::kResetTaskIndex, taskStartTime);
}

void BikeSystem::displayTask1() {
    auto taskStartTime = _timer.elapsed_time();

    _displayDevice.displayGear(_currentGear);
    _displayDevice.displaySpeed(_currentSpeed);
    _displayDevice.displayDistance(_traveledDistance);

    ThisThread::sleep_for(std::chrono::duration_cast<std::chrono::milliseconds>(
        kDisplayTask1ComputationTime - (_timer.elapsed_time() - taskStartTime)));

    _taskLogger.logPeriodAndExecutionTime(
        _timer, advembsof::TaskLogger::kDisplayTask1Index, taskStartTime);
}

void BikeSystem::displayTask2() {
    auto taskStartTime = _timer.elapsed_time();

    _displayDevice.displayTemperature(_currentTemperature);

    ThisThread::sleep_for(std::chrono::duration_cast<std::chrono::milliseconds>(
        kDisplayTask2ComputationTime - (_timer.elapsed_time() - taskStartTime)));

    _taskLogger.logPeriodAndExecutionTime(
        _timer, advembsof::TaskLogger::kDisplayTask2Index, taskStartTime);
}

void BikeSystem::cpuTask() { _cpuLogger.printStats(); }

void BikeSystem::dispatch_isr_events() { _isrEventQueue.dispatch_forever(); }

void BikeSystem::dispatch_events() { _eventQueue.dispatch_forever(); }

}  // namespace multi_tasking