// 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 <cstdint>

#include "cmsis_os.h"
#include "common/constants.hpp"
#include "mbed_trace.h"

#if MBED_CONF_MBED_TRACE_ENABLE
#define TRACE_GROUP "BikeSystem"
#endif  // MBED_CONF_MBED_TRACE_ENABLE

namespace multi_tasking {

static constexpr std::chrono::milliseconds kGearTaskComputationTime          = 100ms;
static constexpr std::chrono::milliseconds kSpeedDistanceTaskComputationTime = 200ms;
static constexpr std::chrono::milliseconds kDisplayTask1Period               = 1600ms;
static constexpr std::chrono::milliseconds kDisplayTask1Delay                = 300ms;
static constexpr std::chrono::milliseconds kDisplayTask1ComputationTime      = 200ms;
static constexpr std::chrono::milliseconds kTemperatureTaskPeriod            = 1600ms;
static constexpr std::chrono::milliseconds kTemperatureTaskDelay             = 1100ms;
static constexpr std::chrono::milliseconds kTemperatureTaskComputationTime   = 100ms;
static constexpr std::chrono::milliseconds kCPUTaskPeriod                    = 1600ms;
static constexpr std::chrono::milliseconds kCPUTaskDelay                     = 0ms;

BikeSystem::BikeSystem()
    : _timer(),
      _isrEventQueue(),
      _eventQueue(),
      _mailPedalDevice(),
      _mailGearDevice(),
      _mutexGearSize(),
      _mutexGear(),
      _mutexSpeed(),
      _mutexDistance(),
      _mutexSpeedometer(),
      _isrEventThread(osPriorityAboveNormal, OS_STACK_SIZE, nullptr, "ISR_Event"),
      _speedDistanceThread(
          osPriorityNormal, OS_STACK_SIZE, nullptr, "Speed_distance_Task"),
      _gearTaskThread(osPriorityAboveNormal, OS_STACK_SIZE, nullptr, "Gear_Task"),
      _gearDevice(&_mailGearDevice, _timer),
      _pedalDevice(&_mailPedalDevice, _timer),
      _resetDevice(callback(this, &BikeSystem::onReset)),
      _displayDevice(),
      _speedometer(_timer),
      _sensorDevice(),
      _taskLogger(),
      _cpuLogger(_timer) {}

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

bike_computer::Speedometer& BikeSystem::getSpeedometer() {
    // ENTER CRITICAL SECTION
    _mutexSpeedometer.lock();
    bike_computer::Speedometer& speedometer = _speedometer;
    _mutexSpeedometer.unlock();
    // END CRITICAL SECTION

    return speedometer;
}

GearDevice& BikeSystem::getGearDevice() { return _gearDevice; }

void BikeSystem::setCallbackGearChage(Callback<void()> cbGearChange) {
    _cbGearChange = cbGearChange;
}

#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("Ffalseailed 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
    bool runTaskLogger = false;

#if defined(MBED_TEST_MODE)
    runTaskLogger = true;
#endif

    _taskLogger.enable(runTaskLogger);
}

void BikeSystem::start() {
    init();

    Event<void()> temperatureEvent(&_eventQueue,
                                   callback(this, &BikeSystem::temperatureTask));
    temperatureEvent.delay(kTemperatureTaskDelay);
    temperatureEvent.period(kTemperatureTaskPeriod);
    temperatureEvent.post();

    Event<void()> displayEvent(&_eventQueue, callback(this, &BikeSystem::displayTask));
    displayEvent.delay(kDisplayTask1Delay);
    displayEvent.period(kDisplayTask1Period);
    displayEvent.post();

    osStatus status =
        _isrEventThread.start(callback(this, &BikeSystem::dispatch_isr_events));
    if (status != osOK) {
        tr_error("Thread %s started with status %ld",
                 _isrEventThread.get_name(),
                 static_cast<int32_t>(status));
    }

    status =
        _speedDistanceThread.start(callback(this, &BikeSystem::loop_speed_distance_task));
    if (status != osOK) {
        tr_error("Thread %s started with status %ld",
                 _isrEventThread.get_name(),
                 static_cast<int32_t>(status));
    }

    status = _gearTaskThread.start(callback(this, &BikeSystem::loop_gear_task));
    if (status != osOK) {
        tr_error("Thread %s started with status %ld",
                 _gearTaskThread.get_name(),
                 static_cast<int32_t>(status));
    }

#if !defined(MBED_TEST_MODE)
    Event<void()> cpuEvent(&_eventQueue, callback(this, &BikeSystem::cpuTask));
    cpuEvent.delay(kCPUTaskDelay);
    cpuEvent.period(kCPUTaskPeriod);
    cpuEvent.post();
#endif

    // dispatch the main queue in the main thread
    dispatch_events();
}

void BikeSystem::stop() {
    osStatus status = _isrEventThread.terminate();
    status += _speedDistanceThread.terminate();
    status += _gearTaskThread.terminate();
    if (status != 0) {
        tr_error("Stop thread error");
    }
    tr_info("Bike system has stopped !");
}

/* Callback from isr */

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

// ISR thread functions

void BikeSystem::resetTask() {
#if !defined(MBED_TEST_MODE)
    auto taskStartTime = _timer.elapsed_time();

    std::chrono::microseconds responseTime = _timer.elapsed_time() - _resetTime;
    tr_info("Reset task: response time is %" PRIu64 " usecs", responseTime.count());
#endif
    // ENTER CRITICAL SECTION
    _mutexSpeedometer.lock();
    _speedometer.reset();
    _mutexSpeedometer.unlock();
    // END CRITICAL SECTION

#if !defined(MBED_TEST_MODE)
    _taskLogger.logPeriodAndExecutionTime(
        _timer, advembsof::TaskLogger::kResetTaskIndex, taskStartTime);
#endif
}

// Speed distance thread functions

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

    pedalMail_t* currentStep = _mailPedalDevice.try_get();

    if (currentStep != nullptr) {
        const auto pedalRotationTime =
            PedalDevice::getCurrentRotationTime(currentStep->step);

        // ENTER CRITICAL SECTION
        _mutexSpeedometer.lock();
        _speedometer.setCurrentRotationTime(pedalRotationTime);
        _mutexSpeedometer.unlock();
        // END CRITICAL SECTION

        std::chrono::microseconds responseTime =
            _timer.elapsed_time() - currentStep->callTime;
        tr_info("Speed distance task: response time is %" PRIu64 " usecs",
                responseTime.count());

        osStatus status = _mailPedalDevice.free(currentStep);
        if (status != osOK) {
            tr_error("free current step in the speed distance tasks doesn't work !");
        }
    }

    // ENTER CRITICAL SECTION
    _mutexSpeedometer.lock();
    _speedometer.setGearSize(getCurrentGearSize());
    _mutexSpeed.lock();
    _currentSpeed = _speedometer.getCurrentSpeed();
    _mutexSpeed.unlock();
    _mutexDistance.lock();
    _traveledDistance = _speedometer.getDistance();
    _mutexDistance.unlock();
    _mutexSpeedometer.unlock();
    // END CRITICAL SECTION

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

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

/* Gear thread functions */
void BikeSystem::gearTask() {
    auto taskStartTime = _timer.elapsed_time();

    gearMail_t* currentGear = _mailGearDevice.try_get();

    if (currentGear != nullptr) {
#if !defined(MBED_TEST_MODE)
        std::chrono::microseconds responseTime =
            _timer.elapsed_time() - currentGear->callTime;
        tr_info("Gear task: response time is %" PRIu64 " usecs", responseTime.count());
#endif

#if defined(MBED_TEST_MODE)
        _cbGearChange();
#endif

        // ENTER CRITICAL SECTION
        _mutexGear.lock();
        _currentGear = currentGear->gear;
        _mutexGear.unlock();
        _mutexGearSize.lock();
        _currentGearSize = bike_computer::kMaxGearSize - currentGear->gear;
        _mutexGearSize.unlock();
        // END CRITICAL SECTION

        osStatus status = _mailGearDevice.free(currentGear);
        if (status != osOK) {
            tr_error("free current gear in the gear tasks doesn't work !");
        }
    }

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

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

/* Main thread functions */

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

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

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

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

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

    // ENTER CRITICAL SECTION
    _displayDevice.displayGear(getCurrentGear());
    _displayDevice.displaySpeed(getCurrentSpeed());
    _displayDevice.displayDistance(getCurrentDistance());
    // END CRITICAL SECTION

    _displayDevice.displayTemperature(_currentTemperature);

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

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

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

void BikeSystem::dispatch_isr_events() {
    tr_info("Start dispatching isr events");
    _isrEventQueue.dispatch_forever();
    tr_info("Stop dispatching isr events");
}

void BikeSystem::dispatch_events() {
    tr_info("Start dispatching main events");
    _eventQueue.dispatch_forever();
    tr_info("Stop dispatching main events");
}

void BikeSystem::loop_speed_distance_task() {
    tr_info("Start loop speed-distance calculation");
    while (true) {
        speedDistanceTask();
    }
}

void BikeSystem::loop_gear_task() {
    tr_info("Start loop gear calculation");
    while (true) {
        gearTask();
    }
}

uint8_t BikeSystem::getCurrentGear() {
    uint8_t currentGear;

    // ENTER CRITICAL SECTION
    _mutexGear.lock();
    currentGear = _currentGear;
    _mutexGear.unlock();
    // END CRITICAL SECTION

    return currentGear;
}

uint8_t BikeSystem::getCurrentGearSize() {
    uint8_t currentGearSize;

    // ENTER CRITICAL SECTION
    _mutexGearSize.lock();
    currentGearSize = _currentGearSize;
    _mutexGearSize.unlock();
    // END CRITICAL SECTION

    return currentGearSize;
}

float BikeSystem::getCurrentSpeed() {
    float currentSpeed;

    // ENTER CRITICAL SECTION
    _mutexSpeed.lock();
    currentSpeed = _currentSpeed;
    _mutexSpeed.unlock();
    // END CRITICAL SECTION

    return currentSpeed;
}

float BikeSystem::getCurrentDistance() {
    float currentDistance;

    // ENTER CRITICAL SECTION
    _mutexDistance.lock();
    currentDistance = _traveledDistance;
    _mutexDistance.unlock();
    // END CRITICAL SECTION

    return currentDistance;
}

}  // namespace multi_tasking