#include <cstddef>
#include <cstdint>

extern "C"
{
#include "FreeRTOS.h"
#include "event_groups.h"
#include "task.h"
#include "timers.h"
#include "hazard3_irq.h"
#include "lab_freertos.h"
#include "lab_io.h"
}

#include "freertos/event_group.hpp"
#include "freertos/isr_drivers.hpp"
#include "freertos/kernel.hpp"
#include "freertos/queue.hpp"
#include "freertos/software_timer.hpp"
#include "freertos/task_storage.hpp"
#include "integration_model.hpp"

namespace
{

constexpr std::uintptr_t io_base = 0xC0000000UL;
constexpr std::uintptr_t io_set_irq = io_base + 0x020UL;
constexpr std::uintptr_t io_clear_irq = io_base + 0x030UL;
constexpr std::uint32_t irq_number = 5U;
constexpr std::uint32_t irq_mask = 1UL << irq_number;
constexpr std::uint32_t gpio_mask = 1U;
constexpr std::size_t task_stack_words = 256U;
constexpr std::size_t trace_capacity = 32U;

enum class ServiceBit : EventBits_t
{
    producer = 1U,
    processor = 2U,
    reporter = 4U
};

using Services = freertos::EventGroup< ServiceBit >;
using ServiceMask = freertos::EventMask< ServiceBit >;
using InputQueue = freertos::StaticQueue< integration::Sample, 2U >;
using ReportQueue = freertos::Queue< integration::Report >;

struct TimerContext
{
    TaskHandle_t producer;
    void on_timer( TimerHandle_t timer ) noexcept;
};
using PeriodicTimer = freertos::PeriodicTimer< TimerContext >;

InputQueue * g_input_queue;
ReportQueue * g_report_queue;
Services * g_services;
PeriodicTimer * g_periodic_timer;
TaskHandle_t g_producer_handle;

volatile std::uint32_t & io_register( std::uintptr_t address ) noexcept
{
    return *reinterpret_cast< volatile std::uint32_t * >( address );
}

ServiceMask all_services() noexcept
{
    return ServiceMask{ ServiceBit::producer } |
           ServiceMask{ ServiceBit::processor } |
           ServiceMask{ ServiceBit::reporter };
}

} // namespace

extern "C"
{
freertos::StaticQueueStorage< integration::Sample, 2U > g_input_storage;
freertos::StaticEventGroupStorage g_service_storage;
StaticTimer_t g_timer_storage;
freertos::StaticTaskStorage< task_stack_words > g_producer_storage;
freertos::StaticTaskStorage< task_stack_words > g_processor_storage;
freertos::StaticTaskStorage< task_stack_words > g_reporter_storage;
freertos::GpioRegisters g_gpio_registers;
integration::TraceEvent g_trace[ trace_capacity ];

volatile std::uint32_t g_trace_count;
volatile std::uint32_t g_trace_overflow;
volatile std::uint32_t g_trace_digest;
volatile std::uint32_t g_barrier_mask;
volatile std::uint32_t g_isr_wake_requested;
volatile std::uint32_t g_isr_yield_calls;
volatile std::uint32_t g_isr_kick_tick;
volatile std::uint32_t g_producer_kick_tick;
volatile std::uint32_t g_kick_seen;
volatile std::uint32_t g_kick_seen_at_reporter_resume;
volatile std::uint32_t g_timer_start_accepted;
volatile std::uint32_t g_timer_stop_accepted;
volatile std::uint32_t g_timer_callbacks;
volatile std::uint32_t g_timer_ticks[ 3 ];
volatile std::uint32_t g_timer_active_after_stop;
volatile std::uint32_t g_samples_produced;
volatile std::uint32_t g_samples_processed;
volatile std::uint32_t g_reports_received;
volatile std::uint32_t g_pressure_dropped;
volatile std::uint32_t g_pressure_dropped_sequence;
volatile std::uint32_t g_pressure_recovered;
volatile std::uint32_t g_input_queue_peak;
volatile std::uint32_t g_report_queue_peak;
volatile std::uint32_t g_report_sum;
volatile std::uint32_t g_first_sample_tick;
volatile std::uint32_t g_last_report_tick;
volatile std::uint32_t g_end_to_end_latency;
volatile std::uint32_t g_producer_hwm;
volatile std::uint32_t g_processor_hwm;
volatile std::uint32_t g_reporter_hwm;
volatile std::size_t g_heap_baseline;
volatile std::size_t g_heap_after_static;
volatile std::size_t g_heap_after_dynamic;
volatile std::size_t g_dynamic_queue_heap_cost;
volatile std::size_t g_free_before_failed_allocation;
volatile std::size_t g_free_after_failed_allocation;
volatile std::size_t g_heap_minimum;
volatile std::uint32_t g_allocation_failure_recovered;
volatile std::uint32_t g_integration_pass;

__attribute__( ( noinline, used ) )
void integration_debug_checkpoint( std::uint32_t point )
{
    ( void ) point;
    __asm__ volatile( "" ::: "memory" );
}
}

namespace
{

void publish_trace( integration::TraceStage stage,
                    std::uint32_t value,
                    TickType_t tick ) noexcept
{
    UBaseType_t const interrupt_state = taskENTER_CRITICAL_FROM_ISR();
    std::uint32_t const index = g_trace_count;
    if( index < trace_capacity )
    {
        g_trace[ index ] = { index + 1U, tick, stage, value };
        g_trace_count = index + 1U;
    }
    else
    {
        g_trace_overflow = 1U;
    }
    taskEXIT_CRITICAL_FROM_ISR( interrupt_state );
}

void update_peak( volatile std::uint32_t & peak, UBaseType_t current ) noexcept
{
    if( current > peak ) { peak = current; }
}

void arrive_and_wait( ServiceBit bit ) noexcept
{
    ( void ) g_services->set( ServiceMask{ bit } );
    auto const result =
        g_services->wait( all_services(), freertos::WaitMode::all,
                          freertos::ClearMode::keep, portMAX_DELAY );
    if( !result.satisfied( all_services(), freertos::WaitMode::all ) )
    {
        lab_fail( 0x4B160001U );
    }
    g_barrier_mask = g_services->bits().raw();
    publish_trace( integration::TraceStage::barrier,
                   static_cast< std::uint32_t >( bit ),
                   xTaskGetTickCount() );
}

void TimerContext::on_timer( TimerHandle_t timer ) noexcept
{
    if( timer == nullptr || producer == nullptr )
    {
        lab_fail( 0x4B160101U );
    }
    std::uint32_t const index = g_timer_callbacks;
    if( index < 3U ) { g_timer_ticks[ index ] = xTaskGetTickCount(); }
    g_timer_callbacks = index + 1U;
    publish_trace( integration::TraceStage::timer_callback,
                   index + 1U, xTaskGetTickCount() );
    xTaskNotifyGive( producer );
}

class Producer final
    : public freertos::StaticTask< Producer, task_stack_words >
{
public:
    explicit Producer( freertos::StaticTaskStorage< task_stack_words > & storage ) noexcept
        : StaticTask{ "producer", 3U, storage } {}

    void run() noexcept
    {
        arrive_and_wait( ServiceBit::producer );
        for( ;; )
        {
            ( void ) ulTaskNotifyTake( pdFALSE, portMAX_DELAY );
            if( g_kick_seen == 0U )
            {
                g_kick_seen = 1U;
                g_producer_kick_tick = xTaskGetTickCount();
                publish_trace( integration::TraceStage::producer_kick,
                               1U, xTaskGetTickCount() );
                continue;
            }

            if( g_samples_produced == 0U )
            {
                integration::Sample const sample{
                    1U, 10U, integration::MessageKind::sample };
                if( !g_input_queue->send( sample, 0U ) )
                {
                    lab_fail( 0x4B160201U );
                }
                g_samples_produced = 1U;
                g_first_sample_tick = xTaskGetTickCount();
                update_peak( g_input_queue_peak, g_input_queue->size() );
                publish_trace( integration::TraceStage::input_queued,
                               sample.sequence, xTaskGetTickCount() );
                continue;
            }

            integration::Sample const second{
                2U, 20U, integration::MessageKind::sample };
            integration::Sample const third{
                3U, 30U, integration::MessageKind::sample };
            integration::Sample const overflow{
                4U, 40U, integration::MessageKind::sample };
            if( !g_input_queue->send( second, 0U ) ||
                !g_input_queue->send( third, 0U ) )
            {
                lab_fail( 0x4B160202U );
            }
            g_samples_produced = 3U;
            update_peak( g_input_queue_peak, g_input_queue->size() );
            publish_trace( integration::TraceStage::input_queued,
                           second.sequence, xTaskGetTickCount() );
            publish_trace( integration::TraceStage::input_queued,
                           third.sequence, xTaskGetTickCount() );
            if( g_input_queue->send( overflow, 0U ) )
            {
                lab_fail( 0x4B160203U );
            }
            g_pressure_dropped = 1U;
            g_pressure_dropped_sequence = overflow.sequence;
            publish_trace( integration::TraceStage::input_dropped,
                           overflow.sequence, xTaskGetTickCount() );

            g_timer_stop_accepted =
                g_periodic_timer->stop() == freertos::TimerCommand::accepted ? 1U : 0U;
            integration::Sample const end{
                0U, 0U, integration::MessageKind::end };
            g_pressure_recovered =
                g_input_queue->send( end, portMAX_DELAY ) ? 1U : 0U;
            publish_trace( integration::TraceStage::end_marker,
                           1U, xTaskGetTickCount() );
            g_producer_hwm = uxTaskGetStackHighWaterMark( nullptr );
            return;
        }
    }
};

class Processor final
    : public freertos::StaticTask< Processor, task_stack_words >
{
public:
    explicit Processor( freertos::StaticTaskStorage< task_stack_words > & storage ) noexcept
        : StaticTask{ "processor", 2U, storage } {}

    void run() noexcept
    {
        arrive_and_wait( ServiceBit::processor );
        for( ;; )
        {
            integration::Sample sample{};
            if( !g_input_queue->receive( sample, portMAX_DELAY ) )
            {
                lab_fail( 0x4B160301U );
            }
            integration::Report const report = integration::transform( sample );
            if( !g_report_queue->send( report, portMAX_DELAY ) )
            {
                lab_fail( 0x4B160302U );
            }
            update_peak( g_report_queue_peak, g_report_queue->size() );
            if( sample.kind == integration::MessageKind::end )
            {
                publish_trace( integration::TraceStage::end_marker,
                               2U, xTaskGetTickCount() );
                g_processor_hwm = uxTaskGetStackHighWaterMark( nullptr );
                return;
            }
            ++g_samples_processed;
            publish_trace( integration::TraceStage::report_created,
                           report.sequence, xTaskGetTickCount() );
        }
    }
};

class Reporter final
    : public freertos::StaticTask< Reporter, task_stack_words >
{
public:
    explicit Reporter( freertos::StaticTaskStorage< task_stack_words > & storage ) noexcept
        : StaticTask{ "reporter", 1U, storage } {}

    void run() noexcept
    {
        arrive_and_wait( ServiceBit::reporter );
        vTaskDelay( 1U );
        io_register( io_set_irq ) = irq_mask;
        __asm__ volatile( "nop" ::: "memory" );
        g_kick_seen_at_reporter_resume = g_kick_seen;
        publish_trace( integration::TraceStage::reporter_resume,
                       g_kick_seen_at_reporter_resume,
                       xTaskGetTickCount() );

        for( ;; )
        {
            integration::Report report{};
            if( !g_report_queue->receive( report, portMAX_DELAY ) )
            {
                lab_fail( 0x4B160401U );
            }
            if( report.kind == integration::MessageKind::end )
            {
                publish_trace( integration::TraceStage::end_marker,
                               3U, xTaskGetTickCount() );
                break;
            }
            {
                freertos::CriticalSection guard;
                g_report_sum += report.transformed;
                ++g_reports_received;
            }
            g_last_report_tick = xTaskGetTickCount();
            publish_trace( integration::TraceStage::report_received,
                           report.sequence, xTaskGetTickCount() );
        }

        g_end_to_end_latency = g_last_report_tick - g_first_sample_tick;
        vTaskDelay( 6U );
        g_timer_active_after_stop = g_periodic_timer->active() ? 1U : 0U;
        g_reporter_hwm = uxTaskGetStackHighWaterMark( nullptr );
        g_heap_minimum = xPortGetMinimumEverFreeHeapSize();
        freertos::GpioPin gpio{ g_gpio_registers };
        gpio.task_view().write_output( gpio_mask, true );

        g_integration_pass =
            g_barrier_mask == 7U &&
            g_isr_wake_requested == 1U &&
            g_isr_yield_calls == 1U &&
            g_isr_kick_tick == 1U &&
            g_producer_kick_tick == 1U &&
            g_kick_seen_at_reporter_resume == 1U &&
            g_timer_start_accepted == 1U &&
            g_timer_stop_accepted == 1U &&
            g_timer_callbacks == 2U &&
            g_timer_ticks[ 0 ] == 5U &&
            g_timer_ticks[ 1 ] == 10U &&
            g_timer_active_after_stop == 0U &&
            g_samples_produced == 3U &&
            g_samples_processed == 3U &&
            g_reports_received == 3U &&
            g_pressure_dropped == 1U &&
            g_pressure_dropped_sequence == 4U &&
            g_pressure_recovered == 1U &&
            g_input_queue_peak == 2U &&
            g_report_queue_peak == 2U &&
            g_report_sum == 120U &&
            g_end_to_end_latency == 6U &&
            g_heap_baseline == g_heap_after_static &&
            g_dynamic_queue_heap_cost > 0U &&
            g_allocation_failure_recovered == 1U &&
            g_free_before_failed_allocation == g_free_after_failed_allocation &&
            g_heap_minimum <= g_heap_after_dynamic &&
            g_producer_hwm > 0U &&
            g_processor_hwm > 0U &&
            g_reporter_hwm > 0U &&
            g_gpio_registers.direction == gpio_mask &&
            g_gpio_registers.data == gpio_mask &&
            g_trace_overflow == 0U &&
            g_trace_count >= 20U ? 1U : 0U;

        publish_trace( integration::TraceStage::final_report,
                       g_integration_pass, xTaskGetTickCount() );
        g_trace_digest =
            integration::trace_digest( g_trace, g_trace_count );
        integration_debug_checkpoint( 8U );
        if( g_integration_pass == 0U )
        {
            lab_put_u32( g_barrier_mask );
            lab_put_u32( g_isr_wake_requested );
            lab_put_u32( g_isr_yield_calls );
            lab_put_u32( g_isr_kick_tick );
            lab_put_u32( g_producer_kick_tick );
            lab_put_u32( g_timer_callbacks );
            lab_put_u32( g_timer_ticks[ 0 ] );
            lab_put_u32( g_timer_ticks[ 1 ] );
            lab_put_u32( g_samples_produced );
            lab_put_u32( g_samples_processed );
            lab_put_u32( g_reports_received );
            lab_put_u32( g_input_queue_peak );
            lab_put_u32( g_report_queue_peak );
            lab_put_u32( g_report_sum );
            lab_put_u32( g_end_to_end_latency );
            lab_put_u32( static_cast< std::uint32_t >( g_heap_baseline - g_heap_after_static ) );
            lab_put_u32( static_cast< std::uint32_t >( g_dynamic_queue_heap_cost ) );
            lab_put_u32( g_allocation_failure_recovered );
            lab_put_u32( g_producer_hwm );
            lab_put_u32( g_processor_hwm );
            lab_put_u32( g_reporter_hwm );
            lab_put_u32( g_trace_count );
            lab_fail( 0x4B160402U );
        }
        lab_puts( "PASS task01: integrated barrier/timer/IRQ/queues/report\n" );
        lab_put_u32( static_cast< std::uint32_t >( g_dynamic_queue_heap_cost ) );
        lab_put_u32( g_report_sum );
        lab_put_u32( g_pressure_dropped_sequence );
        lab_put_u32( g_end_to_end_latency );
        lab_put_u32( g_trace_digest );
        lab_exit( 0U );
    }
};

} // namespace

extern "C"
{
__attribute__( ( noinline, used ) )
void freertos_risc_v_application_interrupt_handler()
{
    freertos::IsrContext context;
    io_register( io_clear_irq ) = irq_mask;
    freertos::TaskNotificationRef notification{ g_producer_handle };
    ( void ) notification.give_from_isr( context );
    g_isr_kick_tick = xTaskGetTickCountFromISR();
    g_isr_wake_requested = context.wake_requested() ? 1U : 0U;
    publish_trace( integration::TraceStage::isr_kick,
                   g_isr_wake_requested, xTaskGetTickCountFromISR() );
    ( void ) context.yield_if_needed();
    g_isr_yield_calls = context.yield_calls();
}
}

int main()
{
    lab_heap_initialize();
    g_heap_baseline = xPortGetFreeHeapSize();
    publish_trace( integration::TraceStage::inventory,
                   static_cast< std::uint32_t >( g_heap_baseline ), 0U );

    InputQueue input_queue{ g_input_storage };
    Services services{ g_service_storage };
    TimerContext timer_context{};
    PeriodicTimer timer{ g_timer_storage, "sampler", 5U, timer_context };
    if( !input_queue.valid() || !services.valid() || !timer.valid() )
    {
        lab_fail( 0x4B160501U );
    }
    g_heap_after_static = xPortGetFreeHeapSize();

    ReportQueue report_queue{ 2U };
    if( !report_queue.valid() )
    {
        lab_fail( 0x4B160502U );
    }
    g_heap_after_dynamic = xPortGetFreeHeapSize();
    g_dynamic_queue_heap_cost = g_heap_after_static - g_heap_after_dynamic;
    g_free_before_failed_allocation = xPortGetFreeHeapSize();
    void * const rejected = pvPortMalloc( configTOTAL_HEAP_SIZE * 2U );
    g_free_after_failed_allocation = xPortGetFreeHeapSize();
    g_allocation_failure_recovered =
        rejected == nullptr &&
        g_free_before_failed_allocation == g_free_after_failed_allocation ? 1U : 0U;
    if( rejected != nullptr ) { vPortFree( rejected ); }

    g_input_queue = &input_queue;
    g_report_queue = &report_queue;
    g_services = &services;
    g_periodic_timer = &timer;

    Producer producer{ g_producer_storage };
    Processor processor{ g_processor_storage };
    Reporter reporter{ g_reporter_storage };
    if( !producer.start() || !processor.start() || !reporter.start() )
    {
        lab_fail( 0x4B160503U );
    }
    g_producer_handle = producer.native_handle();
    timer_context.producer = g_producer_handle;

    freertos::GpioPin gpio{ g_gpio_registers };
    gpio.task_view().configure_output( gpio_mask );
    h3irq_enable( irq_number, true );
    h3irq_set_priority( irq_number, 8U );
    g_timer_start_accepted =
        timer.start() == freertos::TimerCommand::accepted ? 1U : 0U;
    freertos::Scheduler::start();
    lab_fail( 0x4B160504U );
}
