/*
 * Copyright (c) 2006-2018, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2019-03-05     whj4674672   first version
 */

#include <rtdevice.h>
#include <rtthread.h>
#include <board.h>

#include "planner/s_curve_planner/include/s_curve_position_planner.h"

/* defined the LED0 pin: PC13 */
#define LED0_PIN    GET_PIN(C, 13)

namespace
{

constexpr rt_int32_t kWarmupIterations = 10;
constexpr rt_int32_t kBenchmarkIterations = 10000;
constexpr double kBenchmarkDtSec = 0.01;
constexpr double kMaxVelocityRad = 3.14;
constexpr double kMaxAccelerationRad = 31.4;
constexpr double kMaxJerkRad = 31.4;
constexpr double kPositionGain =2.0;
constexpr double kTargetPosA = 1.35;
constexpr double kTargetPosB = 1.35;

volatile double g_planner_sink = 0.0;

void run_position_planner_benchmark(const char* label, rt_int32_t target_switch_period)
{
    cmvr::SCurvePositionPlanner1D planner(
        kMaxVelocityRad,
        kMaxAccelerationRad,
        kMaxJerkRad);

    planner.setPositionGain(kPositionGain);
    planner.initialize(0.0, 0.0, 0.0);
    planner.setTarget(kTargetPosA);

    for (rt_int32_t i = 0; i < kWarmupIterations; ++i)
    {
        if (target_switch_period > 0 && (i % target_switch_period) == 0)
        {
            const bool use_target_a = ((i / target_switch_period) & 1) == 0;
            planner.setTarget(use_target_a ? kTargetPosA : kTargetPosB);
        }
        g_planner_sink = planner.update(kBenchmarkDtSec);
    }

    planner.initialize(0.0, 0.0, 0.0);
    planner.setTarget(kTargetPosA);

    const rt_tick_t start_tick = rt_tick_get();

    for (rt_int32_t i = 0; i < kBenchmarkIterations; ++i)
    {
        if (target_switch_period > 0 && (i % target_switch_period) == 0)
        {
            const bool use_target_a = ((i / target_switch_period) & 1) == 0;
            planner.setTarget(use_target_a ? kTargetPosA : kTargetPosB);
        }
        g_planner_sink = planner.update(kBenchmarkDtSec);
    }

    const rt_tick_t end_tick = rt_tick_get();
    const rt_uint64_t elapsed_ticks = static_cast<rt_uint64_t>(end_tick - start_tick);
    const rt_uint64_t elapsed_us =
        (elapsed_ticks * 1000000ULL) / RT_TICK_PER_SECOND;
    const rt_uint64_t avg_ns =
        (elapsed_ticks * 1000000000ULL) /
        (RT_TICK_PER_SECOND * static_cast<rt_uint64_t>(kBenchmarkIterations));

    rt_kprintf(
        "[PlannerBench] %s iter=%d dt=%.3fms switch=%d elapsed=%llu us avg=%llu ns pos=%.6f vel=%.6f moving=%d\n",
        label,
        kBenchmarkIterations,
        kBenchmarkDtSec * 1000.0,
        target_switch_period,
        elapsed_us,
        avg_ns,
        planner.getPosition(),
        planner.getVelocity(),
        planner.isMoving() ? 1 : 0);
}

void run_planner_benchmarks()
{
    rt_kprintf("[PlannerBench] start RT_TICK_PER_SECOND=%d\n", RT_TICK_PER_SECOND);
    run_position_planner_benchmark("hold_target", 0);
    run_position_planner_benchmark("retarget_20", 20);
    run_position_planner_benchmark("retarget_5", 5);
    rt_kprintf("[PlannerBench] done sink=%.6f\n", g_planner_sink);
}

} // namespace

int main(void)
{
    int count = 1;

    rt_pin_mode(LED0_PIN, PIN_MODE_OUTPUT);
    run_planner_benchmarks();

    while (count++)
    {
        rt_pin_write(LED0_PIN, PIN_HIGH);
        rt_thread_mdelay(100);
        rt_pin_write(LED0_PIN, PIN_LOW);
        rt_thread_mdelay(100);
    }

    return RT_EOK;
}