real_time_controller.cpp

/*
 * High-Frequency Real-Time Controller for Quadrotor Flight Control
 * ================================================================
 *
 * This module implements a high-frequency (1000Hz) real-time control system
 * for quadrotor flight control, designed to achieve ultra-low latency and
 * precise timing for critical flight control applications.
 *
 * Key Features:
 * - 1000Hz control loop frequency for precise control
 * - Real-time priority scheduling for deterministic timing
 * - Lock-free data structures for thread safety
 * - Memory pool allocation for zero-allocation control loops
 * - Hardware abstraction layer for different platforms
 * - Performance monitoring and diagnostics
 *
 * This implementation is designed to work with the NCKU-Quadrotor-Navigation
 * project and provides the high-frequency control capabilities needed for
 * advanced flight control algorithms.
 *
 * Author: [Your Name]
 * Date: [Current Date]
 * License: MIT
 */

#include <iostream>
#include <chrono>
#include <thread>
#include <atomic>
#include <vector>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <memory>
#include <functional>
#include <algorithm>
#include <cmath>
#include <cstring>

// Platform-specific includes for real-time capabilities
#ifdef __linux__
    #include <pthread.h>
    #include <sched.h>
    #include <sys/mman.h>
    #include <sys/resource.h>
    #include <unistd.h>
    #include <errno.h>
#elif defined(_WIN32)
    #include <windows.h>
    #include <processthreadsapi.h>
#endif

// Real-time control system namespace
namespace rt_control {

// Forward declarations
class MemoryPool;
class ControlData;
class SensorData;
class MotorCommands;
class PerformanceMonitor;

/**
 * @brief Real-time priority levels for different control tasks
 * 
 * These priority levels ensure that critical control tasks are executed
 * with higher priority than non-critical tasks, reducing jitter and
 * improving control performance.
 */
enum class RT_PRIORITY {
    CRITICAL = 0,      // Highest priority: motor control, safety systems
    HIGH = 1,          // High priority: attitude control, position control
    MEDIUM = 2,        // Medium priority: trajectory planning, data logging
    LOW = 3,           // Low priority: communication, diagnostics
    BACKGROUND = 4     // Background: non-critical tasks
};

/**
 * @brief Control loop timing configuration
 * 
 * This structure defines the timing parameters for the real-time control
 * system, including loop frequency, deadlines, and timing tolerances.
 */
struct TimingConfig {
    double control_frequency_hz;     // Control loop frequency in Hz
    double control_period_us;        // Control period in microseconds
    double max_jitter_us;           // Maximum allowed timing jitter
    double deadline_miss_tolerance; // Tolerance for deadline misses
    bool enable_timing_monitoring;  // Enable timing performance monitoring
    
    TimingConfig() : 
        control_frequency_hz(1000.0),
        control_period_us(1000.0),
        max_jitter_us(50.0),
        deadline_miss_tolerance(0.001),
        enable_timing_monitoring(true) {}
};

/**
 * @brief Sensor data structure for real-time control
 * 
 * This structure holds all sensor measurements needed for control,
 * with proper alignment for cache efficiency and atomic access
 * for thread safety.
 */
struct alignas(64) SensorData {
    // Position and velocity measurements (6DOF)
    double position[3];        // [x, y, z] in meters
    double velocity[3];        // [vx, vy, vz] in m/s
    double attitude[3];        // [roll, pitch, yaw] in radians
    double angular_rate[3];    // [roll_rate, pitch_rate, yaw_rate] in rad/s
    
    // IMU measurements
    double accelerometer[3];   // [ax, ay, az] in m/s^2
    double gyroscope[3];      // [gx, gy, gz] in rad/s
    double magnetometer[3];    // [mx, my, mz] in arbitrary units
    
    // Additional sensors
    double barometer;          // Altitude from barometer in meters
    double battery_voltage;     // Battery voltage in volts
    double battery_current;    // Battery current in amperes
    
    // Timing information
    uint64_t timestamp_ns;     // Timestamp in nanoseconds
    uint32_t sequence_number;  // Sequence number for data integrity
    
    // Data quality indicators
    bool data_valid;           // Overall data validity flag
    uint32_t sensor_status;    // Individual sensor status bits
    
    SensorData() {
        // Initialize all data to zero
        std::memset(this, 0, sizeof(SensorData));
        data_valid = false;
        sensor_status = 0;
    }
};

/**
 * @brief Motor command structure for real-time control
 * 
 * This structure holds the motor commands generated by the control system,
 * with proper alignment and atomic access for thread safety.
 */
struct alignas(64) MotorCommands {
    // Motor PWM values (0.0 to 1.0)
    double motor_pwm[4];      // [motor1, motor2, motor3, motor4]
    
    // Motor control flags
    bool motors_enabled;       // Overall motor enable flag
    bool emergency_stop;       // Emergency stop flag
    uint32_t motor_status;     // Individual motor status bits
    
    // Control timing
    uint64_t command_timestamp_ns;  // Command generation timestamp
    uint32_t command_sequence;     // Command sequence number
    
    // Safety limits
    double max_thrust;         // Maximum allowed thrust
    double min_thrust;         // Minimum allowed thrust
    
    MotorCommands() {
        // Initialize all motors to zero
        std::memset(motor_pwm, 0, sizeof(motor_pwm));
        motors_enabled = false;
        emergency_stop = false;
        motor_status = 0;
        max_thrust = 1.0;
        min_thrust = 0.0;
    }
};

/**
 * @brief Control data structure for real-time control
 * 
 * This structure holds all control-related data including setpoints,
 * control outputs, and performance metrics.
 */
struct alignas(64) ControlData {
    // Control setpoints
    double position_setpoint[3];   // [x, y, z] target position
    double velocity_setpoint[3];   // [vx, vy, vz] target velocity
    double attitude_setpoint[3];   // [roll, pitch, yaw] target attitude
    double yaw_rate_setpoint;      // Target yaw rate
    
    // Control outputs
    double position_error[3];      // Position error
    double velocity_error[3];      // Velocity error
    double attitude_error[3];      // Attitude error
    
    // Control gains
    double position_gains[3];      // [kp, ki, kd] for position control
    double velocity_gains[3];     // [kp, ki, kd] for velocity control
    double attitude_gains[3];     // [kp, ki, kd] for attitude control
    double yaw_rate_gains[3];     // [kp, ki, kd] for yaw rate control
    
    // Performance metrics
    double control_effort[4];     // Control effort for each axis
    double tracking_error;       // Overall tracking error
    double control_bandwidth;    // Achieved control bandwidth
    
    // Timing information
    uint64_t control_timestamp_ns; // Control computation timestamp
    double control_dt_us;         // Control period in microseconds
    
    ControlData() {
        // Initialize all data to zero
        std::memset(this, 0, sizeof(ControlData));
    }
};

/**
 * @brief Memory pool for zero-allocation real-time control
 * 
 * This class provides a memory pool for allocating control data structures
 * without dynamic memory allocation, ensuring deterministic timing and
 * avoiding memory fragmentation in real-time systems.
 */
class MemoryPool {
private:
    struct MemoryBlock {
        void* data;
        bool in_use;
        uint64_t allocation_time;
    };
    
    std::vector<MemoryBlock> blocks_;
    std::mutex pool_mutex_;
    size_t block_size_;
    size_t num_blocks_;
    
public:
    /**
     * @brief Constructor for memory pool
     * 
     * @param block_size Size of each memory block in bytes
     * @param num_blocks Number of memory blocks to allocate
     */
    MemoryPool(size_t block_size, size_t num_blocks) 
        : block_size_(block_size), num_blocks_(num_blocks) {
        
        // Allocate memory blocks
        for (size_t i = 0; i < num_blocks; ++i) {
            MemoryBlock block;
            block.data = aligned_alloc(64, block_size);  // 64-byte alignment for cache efficiency
            block.in_use = false;
            block.allocation_time = 0;
            blocks_.push_back(block);
        }
        
        std::cout << "Memory pool initialized: " << num_blocks << " blocks of " 
                  << block_size << " bytes each" << std::endl;
    }
    
    /**
     * @brief Destructor - free all allocated memory
     */
    ~MemoryPool() {
        std::lock_guard<std::mutex> lock(pool_mutex_);
        for (auto& block : blocks_) {
            if (block.data) {
                std::free(block.data);
                block.data = nullptr;
            }
        }
    }
    
    /**
     * @brief Allocate a memory block from the pool
     * 
     * @return Pointer to allocated memory block, or nullptr if no blocks available
     */
    void* allocate() {
        std::lock_guard<std::mutex> lock(pool_mutex_);
        
        for (auto& block : blocks_) {
            if (!block.in_use) {
                block.in_use = true;
                block.allocation_time = std::chrono::duration_cast<std::chrono::nanoseconds>(
                    std::chrono::high_resolution_clock::now().time_since_epoch()).count();
                return block.data;
            }
        }
        
        return nullptr;  // No free blocks available
    }
    
    /**
     * @brief Free a memory block back to the pool
     * 
     * @param ptr Pointer to the memory block to free
     */
    void free(void* ptr) {
        std::lock_guard<std::mutex> lock(pool_mutex_);
        
        for (auto& block : blocks_) {
            if (block.data == ptr && block.in_use) {
                block.in_use = false;
                return;
            }
        }
    }
    
    /**
     * @brief Get pool utilization statistics
     * 
     * @return Pair of (used_blocks, total_blocks)
     */
    std::pair<size_t, size_t> get_utilization() const {
        std::lock_guard<std::mutex> lock(const_cast<std::mutex&>(pool_mutex_));
        size_t used_blocks = 0;
        for (const auto& block : blocks_) {
            if (block.in_use) ++used_blocks;
        }
        return {used_blocks, num_blocks_};
    }
};

/**
 * @brief Performance monitoring for real-time control system
 * 
 * This class provides comprehensive performance monitoring capabilities
 * including timing analysis, memory usage, and control performance metrics.
 */
class PerformanceMonitor {
private:
    // Timing statistics
    struct TimingStats {
        double mean_period_us;
        double std_period_us;
        double max_jitter_us;
        double min_period_us;
        double max_period_us;
        uint64_t total_cycles;
        uint64_t deadline_misses;
        uint64_t last_update_ns;
    };
    
    // Control performance statistics
    struct ControlStats {
        double mean_tracking_error;
        double max_tracking_error;
        double control_effort_variance;
        double bandwidth_achieved;
        uint64_t control_updates;
    };
    
    TimingStats timing_stats_;
    ControlStats control_stats_;
    std::mutex stats_mutex_;
    
    // Circular buffer for recent measurements
    static constexpr size_t BUFFER_SIZE = 1000;
    std::vector<double> period_buffer_;
    std::vector<double> error_buffer_;
    size_t buffer_index_;
    
public:
    PerformanceMonitor() : buffer_index_(0) {
        // Initialize buffers
        period_buffer_.resize(BUFFER_SIZE, 0.0);
        error_buffer_.resize(BUFFER_SIZE, 0.0);
        
        // Initialize statistics
        std::memset(&timing_stats_, 0, sizeof(TimingStats));
        std::memset(&control_stats_, 0, sizeof(ControlStats));
    }
    
    /**
     * @brief Update timing statistics
     * 
     * @param period_us Control period in microseconds
     * @param deadline_missed Whether the deadline was missed
     */
    void update_timing_stats(double period_us, bool deadline_missed) {
        std::lock_guard<std::mutex> lock(stats_mutex_);
        
        // Update circular buffer
        period_buffer_[buffer_index_] = period_us;
        
        // Update statistics
        timing_stats_.total_cycles++;
        if (deadline_missed) {
            timing_stats_.deadline_misses++;
        }
        
        // Update min/max
        if (timing_stats_.total_cycles == 1) {
            timing_stats_.min_period_us = period_us;
            timing_stats_.max_period_us = period_us;
        } else {
            timing_stats_.min_period_us = std::min(timing_stats_.min_period_us, period_us);
            timing_stats_.max_period_us = std::max(timing_stats_.max_period_us, period_us);
        }
        
        // Update mean and standard deviation (simplified calculation)
        double old_mean = timing_stats_.mean_period_us;
        timing_stats_.mean_period_us = (timing_stats_.mean_period_us * (timing_stats_.total_cycles - 1) + period_us) / timing_stats_.total_cycles;
        
        // Simplified variance calculation
        double variance = 0.0;
        for (double period : period_buffer_) {
            variance += (period - timing_stats_.mean_period_us) * (period - timing_stats_.mean_period_us);
        }
        variance /= BUFFER_SIZE;
        timing_stats_.std_period_us = std::sqrt(variance);
        
        // Update max jitter
        timing_stats_.max_jitter_us = std::max(timing_stats_.max_jitter_us, 
                                             std::abs(period_us - timing_stats_.mean_period_us));
        
        timing_stats_.last_update_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
            std::chrono::high_resolution_clock::now().time_since_epoch()).count();
    }
    
    /**
     * @brief Update control performance statistics
     * 
     * @param tracking_error Current tracking error
     * @param control_effort Current control effort
     */
    void update_control_stats(double tracking_error, double control_effort) {
        std::lock_guard<std::mutex> lock(stats_mutex_);
        
        // Update circular buffer
        error_buffer_[buffer_index_] = tracking_error;
        
        // Update statistics
        control_stats_.control_updates++;
        
        // Update mean tracking error
        control_stats_.mean_tracking_error = (control_stats_.mean_tracking_error * (control_stats_.control_updates - 1) + tracking_error) / control_stats_.control_updates;
        
        // Update max tracking error
        control_stats_.max_tracking_error = std::max(control_stats_.max_tracking_error, tracking_error);
        
        // Simplified control effort variance calculation
        double effort_variance = 0.0;
        for (double error : error_buffer_) {
            effort_variance += error * error;
        }
        effort_variance /= BUFFER_SIZE;
        control_stats_.control_effort_variance = effort_variance;
        
        // Update buffer index
        buffer_index_ = (buffer_index_ + 1) % BUFFER_SIZE;
    }
    
    /**
     * @brief Get current timing statistics
     * 
     * @return Copy of current timing statistics
     */
    TimingStats get_timing_stats() const {
        std::lock_guard<std::mutex> lock(stats_mutex_);
        return timing_stats_;
    }
    
    /**
     * @brief Get current control statistics
     * 
     * @return Copy of current control statistics
     */
    ControlStats get_control_stats() const {
        std::lock_guard<std::mutex> lock(stats_mutex_);
        return control_stats_;
    }
    
    /**
     * @brief Print performance report
     */
    void print_report() const {
        auto timing = get_timing_stats();
        auto control = get_control_stats();
        
        std::cout << "\n=== Performance Report ===" << std::endl;
        std::cout << "Timing Statistics:" << std::endl;
        std::cout << "  Mean Period: " << timing.mean_period_us << " us" << std::endl;
        std::cout << "  Std Period: " << timing.std_period_us << " us" << std::endl;
        std::cout << "  Max Jitter: " << timing.max_jitter_us << " us" << std::endl;
        std::cout << "  Deadline Misses: " << timing.deadline_misses << "/" << timing.total_cycles << std::endl;
        std::cout << "  Miss Rate: " << (double)timing.deadline_misses / timing.total_cycles * 100.0 << "%" << std::endl;
        
        std::cout << "\nControl Statistics:" << std::endl;
        std::cout << "  Mean Tracking Error: " << control.mean_tracking_error << std::endl;
        std::cout << "  Max Tracking Error: " << control.max_tracking_error << std::endl;
        std::cout << "  Control Effort Variance: " << control.control_effort_variance << std::endl;
        std::cout << "  Control Updates: " << control.control_updates << std::endl;
    }
};

/**
 * @brief Real-time control loop implementation
 * 
 * This class implements the main real-time control loop with high-frequency
 * execution, precise timing, and comprehensive performance monitoring.
 */
class RealTimeController {
private:
    // Configuration
    TimingConfig timing_config_;
    RT_PRIORITY priority_;
    
    // Data structures
    std::unique_ptr<MemoryPool> memory_pool_;
    std::unique_ptr<PerformanceMonitor> performance_monitor_;
    
    // Control data (allocated from memory pool)
    SensorData* sensor_data_;
    MotorCommands* motor_commands_;
    ControlData* control_data_;
    
    // Threading
    std::atomic<bool> running_;
    std::thread control_thread_;
    std::mutex data_mutex_;
    std::condition_variable data_cv_;
    
    // Control loop timing
    std::chrono::high_resolution_clock::time_point last_control_time_;
    std::chrono::high_resolution_clock::time_point next_control_time_;
    
    // Control algorithm callbacks
    std::function<void(const SensorData&, ControlData&)> control_algorithm_;
    std::function<void(const MotorCommands&)> motor_output_callback_;
    std::function<void(const SensorData&)> sensor_input_callback_;
    
public:
    /**
     * @brief Constructor for real-time controller
     * 
     * @param config Timing configuration
     * @param priority Real-time priority level
     */
    RealTimeController(const TimingConfig& config, RT_PRIORITY priority = RT_PRIORITY::CRITICAL)
        : timing_config_(config), priority_(priority), running_(false) {
        
        // Initialize memory pool
        size_t max_block_size = std::max({sizeof(SensorData), sizeof(MotorCommands), sizeof(ControlData)});
        memory_pool_ = std::make_unique<MemoryPool>(max_block_size, 10);
        
        // Initialize performance monitor
        performance_monitor_ = std::make_unique<PerformanceMonitor>();
        
        // Allocate control data from memory pool
        sensor_data_ = static_cast<SensorData*>(memory_pool_->allocate());
        motor_commands_ = static_cast<MotorCommands*>(memory_pool_->allocate());
        control_data_ = static_cast<ControlData*>(memory_pool_->allocate());
        
        if (!sensor_data_ || !motor_commands_ || !control_data_) {
            throw std::runtime_error("Failed to allocate control data from memory pool");
        }
        
        // Initialize control data
        *sensor_data_ = SensorData();
        *motor_commands_ = MotorCommands();
        *control_data_ = ControlData();
        
        std::cout << "Real-time controller initialized with " << config.control_frequency_hz 
                  << " Hz control frequency" << std::endl;
    }
    
    /**
     * @brief Destructor
     */
    ~RealTimeController() {
        stop();
        
        // Free memory pool allocations
        if (memory_pool_) {
            if (sensor_data_) memory_pool_->free(sensor_data_);
            if (motor_commands_) memory_pool_->free(motor_commands_);
            if (control_data_) memory_pool_->free(control_data_);
        }
    }
    
    /**
     * @brief Set control algorithm callback
     * 
     * @param algorithm Control algorithm function
     */
    void set_control_algorithm(std::function<void(const SensorData&, ControlData&)> algorithm) {
        control_algorithm_ = algorithm;
    }
    
    /**
     * @brief Set motor output callback
     * 
     * @param callback Motor output function
     */
    void set_motor_output_callback(std::function<void(const MotorCommands&)> callback) {
        motor_output_callback_ = callback;
    }
    
    /**
     * @brief Set sensor input callback
     * 
     * @param callback Sensor input function
     */
    void set_sensor_input_callback(std::function<void(const SensorData&)> callback) {
        sensor_input_callback_ = callback;
    }
    
    /**
     * @brief Start the real-time control loop
     */
    void start() {
        if (running_.load()) {
            std::cout << "Control loop already running" << std::endl;
            return;
        }
        
        running_.store(true);
        
        // Configure real-time priority
        configure_realtime_priority();
        
        // Initialize timing
        last_control_time_ = std::chrono::high_resolution_clock::now();
        next_control_time_ = last_control_time_ + std::chrono::microseconds(
            static_cast<long long>(timing_config_.control_period_us));
        
        // Start control thread
        control_thread_ = std::thread(&RealTimeController::control_loop, this);
        
        std::cout << "Real-time control loop started" << std::endl;
    }
    
    /**
     * @brief Stop the real-time control loop
     */
    void stop() {
        if (!running_.load()) {
            return;
        }
        
        running_.store(false);
        
        if (control_thread_.joinable()) {
            control_thread_.join();
        }
        
        // Print final performance report
        performance_monitor_->print_report();
        
        std::cout << "Real-time control loop stopped" << std::endl;
    }
    
    /**
     * @brief Update sensor data (thread-safe)
     * 
     * @param data New sensor data
     */
    void update_sensor_data(const SensorData& data) {
        std::lock_guard<std::mutex> lock(data_mutex_);
        *sensor_data_ = data;
        data_cv_.notify_one();
    }
    
    /**
     * @brief Get current motor commands (thread-safe)
     * 
     * @return Copy of current motor commands
     */
    MotorCommands get_motor_commands() const {
        std::lock_guard<std::mutex> lock(data_mutex_);
        return *motor_commands_;
    }
    
    /**
     * @brief Get current control data (thread-safe)
     * 
     * @return Copy of current control data
     */
    ControlData get_control_data() const {
        std::lock_guard<std::mutex> lock(data_mutex_);
        return *control_data_;
    }
    
    /**
     * @brief Get performance monitor
     * 
     * @return Pointer to performance monitor
     */
    PerformanceMonitor* get_performance_monitor() const {
        return performance_monitor_.get();
    }

private:
    /**
     * @brief Configure real-time priority for the control thread
     */
    void configure_realtime_priority() {
#ifdef __linux__
        // Set thread scheduling policy to SCHED_FIFO for real-time priority
        sched_param param;
        param.sched_priority = get_priority_value(priority_);
        
        if (pthread_setschedparam(control_thread_.native_handle(), SCHED_FIFO, &param) != 0) {
            std::cerr << "Warning: Failed to set real-time priority: " << strerror(errno) << std::endl;
        }
        
        // Lock memory to prevent page faults
        if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0) {
            std::cerr << "Warning: Failed to lock memory: " << strerror(errno) << std::endl;
        }
        
        // Set CPU affinity to a specific core (optional)
        cpu_set_t cpuset;
        CPU_ZERO(&cpuset);
        CPU_SET(0, &cpuset);  // Use CPU core 0
        
        if (pthread_setaffinity_np(control_thread_.native_handle(), sizeof(cpu_set_t), &cpuset) != 0) {
            std::cerr << "Warning: Failed to set CPU affinity: " << strerror(errno) << std::endl;
        }
        
#elif defined(_WIN32)
        // Windows real-time priority setting
        HANDLE thread_handle = control_thread_.native_handle();
        int priority_class = get_priority_value(priority_);
        
        if (!SetThreadPriority(thread_handle, priority_class)) {
            std::cerr << "Warning: Failed to set real-time priority on Windows" << std::endl;
        }
#endif
        
        std::cout << "Real-time priority configured for control thread" << std::endl;
    }
    
    /**
     * @brief Get priority value for the current platform
     * 
     * @param priority Priority level
     * @return Platform-specific priority value
     */
    int get_priority_value(RT_PRIORITY priority) {
#ifdef __linux__
        switch (priority) {
            case RT_PRIORITY::CRITICAL: return 99;
            case RT_PRIORITY::HIGH: return 80;
            case RT_PRIORITY::MEDIUM: return 60;
            case RT_PRIORITY::LOW: return 40;
            case RT_PRIORITY::BACKGROUND: return 20;
            default: return 50;
        }
#elif defined(_WIN32)
        switch (priority) {
            case RT_PRIORITY::CRITICAL: return THREAD_PRIORITY_TIME_CRITICAL;
            case RT_PRIORITY::HIGH: return THREAD_PRIORITY_HIGHEST;
            case RT_PRIORITY::MEDIUM: return THREAD_PRIORITY_ABOVE_NORMAL;
            case RT_PRIORITY::LOW: return THREAD_PRIORITY_NORMAL;
            case RT_PRIORITY::BACKGROUND: return THREAD_PRIORITY_BELOW_NORMAL;
            default: return THREAD_PRIORITY_NORMAL;
        }
#else
        return 0;  // Default priority for unsupported platforms
#endif
    }
    
    /**
     * @brief Main real-time control loop
     * 
     * This function implements the high-frequency control loop with precise
     * timing and comprehensive performance monitoring.
     */
    void control_loop() {
        std::cout << "Entering real-time control loop" << std::endl;
        
        while (running_.load()) {
            auto current_time = std::chrono::high_resolution_clock::now();
            
            // Check if it's time for the next control cycle
            if (current_time >= next_control_time_) {
                // Calculate actual control period
                auto actual_period = std::chrono::duration_cast<std::chrono::microseconds>(
                    current_time - last_control_time_).count();
                
                // Check for deadline miss
                bool deadline_missed = actual_period > timing_config_.control_period_us * (1.0 + timing_config_.deadline_miss_tolerance);
                
                // Update timing statistics
                performance_monitor_->update_timing_stats(static_cast<double>(actual_period), deadline_missed);
                
                // Execute control cycle
                execute_control_cycle();
                
                // Update timing for next cycle
                last_control_time_ = current_time;
                next_control_time_ = current_time + std::chrono::microseconds(
                    static_cast<long long>(timing_config_.control_period_us));
                
                // If deadline was missed, adjust next control time
                if (deadline_missed) {
                    next_control_time_ = current_time + std::chrono::microseconds(
                        static_cast<long long>(timing_config_.control_period_us));
                    std::cerr << "Warning: Control deadline missed, period: " << actual_period << " us" << std::endl;
                }
            } else {
                // Sleep until next control cycle
                auto sleep_time = next_control_time_ - current_time;
                if (sleep_time > std::chrono::microseconds(100)) {
                    std::this_thread::sleep_for(sleep_time - std::chrono::microseconds(100));
                }
            }
        }
        
        std::cout << "Exiting real-time control loop" << std::endl;
    }
    
    /**
     * @brief Execute a single control cycle
     * 
     * This function performs one complete control cycle including:
     * 1. Sensor data acquisition
     * 2. Control algorithm execution
     * 3. Motor command generation
     * 4. Performance monitoring
     */
    void execute_control_cycle() {
        // Get current sensor data
        SensorData current_sensor_data;
        {
            std::lock_guard<std::mutex> lock(data_mutex_);
            current_sensor_data = *sensor_data_;
        }
        
        // Call sensor input callback if available
        if (sensor_input_callback_) {
            sensor_input_callback_(current_sensor_data);
        }
        
        // Execute control algorithm if available
        if (control_algorithm_) {
            control_algorithm_(current_sensor_data, *control_data_);
        }
        
        // Generate motor commands (simplified implementation)
        generate_motor_commands(current_sensor_data, *control_data_, *motor_commands_);
        
        // Update control performance statistics
        double tracking_error = std::sqrt(
            control_data_->position_error[0] * control_data_->position_error[0] +
            control_data_->position_error[1] * control_data_->position_error[1] +
            control_data_->position_error[2] * control_data_->position_error[2]
        );
        
        double control_effort = std::sqrt(
            motor_commands_->motor_pwm[0] * motor_commands_->motor_pwm[0] +
            motor_commands_->motor_pwm[1] * motor_commands_->motor_pwm[1] +
            motor_commands_->motor_pwm[2] * motor_commands_->motor_pwm[2] +
            motor_commands_->motor_pwm[3] * motor_commands_->motor_pwm[3]
        );
        
        performance_monitor_->update_control_stats(tracking_error, control_effort);
        
        // Call motor output callback if available
        if (motor_output_callback_) {
            motor_output_callback_(*motor_commands_);
        }
    }
    
    /**
     * @brief Generate motor commands from control data
     * 
     * @param sensor_data Current sensor data
     * @param control_data Current control data
     * @param motor_commands Output motor commands
     */
    void generate_motor_commands(const SensorData& sensor_data, 
                               const ControlData& control_data,
                               MotorCommands& motor_commands) {
        // Simple PID control implementation for demonstration
        // In practice, this would be replaced with the actual control algorithm
        
        // Position control (simplified)
        double pos_error_x = control_data.position_setpoint[0] - sensor_data.position[0];
        double pos_error_y = control_data.position_setpoint[1] - sensor_data.position[1];
        double pos_error_z = control_data.position_setpoint[2] - sensor_data.position[2];
        
        // Velocity control (simplified)
        double vel_error_x = control_data.velocity_setpoint[0] - sensor_data.velocity[0];
        double vel_error_y = control_data.velocity_setpoint[1] - sensor_data.velocity[1];
        double vel_error_z = control_data.velocity_setpoint[2] - sensor_data.velocity[2];
        
        // Attitude control (simplified)
        double att_error_roll = control_data.attitude_setpoint[0] - sensor_data.attitude[0];
        double att_error_pitch = control_data.attitude_setpoint[1] - sensor_data.attitude[1];
        double att_error_yaw = control_data.attitude_setpoint[2] - sensor_data.attitude[2];
        
        // Simple motor command generation (this is a placeholder)
        // In practice, this would implement the full quadrotor control allocation
        double base_thrust = 0.5;  // Base hover thrust
        
        motor_commands.motor_pwm[0] = std::max(0.0, std::min(1.0, base_thrust + pos_error_z * 0.1));
        motor_commands.motor_pwm[1] = std::max(0.0, std::min(1.0, base_thrust + pos_error_z * 0.1));
        motor_commands.motor_pwm[2] = std::max(0.0, std::min(1.0, base_thrust + pos_error_z * 0.1));
        motor_commands.motor_pwm[3] = std::max(0.0, std::min(1.0, base_thrust + pos_error_z * 0.1));
        
        // Update motor command metadata
        motor_commands.command_timestamp_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
            std::chrono::high_resolution_clock::now().time_since_epoch()).count();
        motor_commands.command_sequence++;
        motor_commands.motors_enabled = true;
    }
};

} // namespace rt_control

/**
 * @brief Example usage of the real-time controller
 * 
 * This demonstrates how to set up and use the high-frequency real-time
 * control system for quadrotor flight control.
 */
int main() {
    std::cout << "High-Frequency Real-Time Controller Example" << std::endl;
    std::cout << "===========================================" << std::endl;
    
    // Configure timing parameters
    rt_control::TimingConfig config;
    config.control_frequency_hz = 1000.0;  // 1kHz control loop
    config.control_period_us = 1000.0;      // 1ms period
    config.max_jitter_us = 50.0;            // 50us max jitter
    config.deadline_miss_tolerance = 0.001; // 0.1% tolerance
    config.enable_timing_monitoring = true;
    
    // Create real-time controller
    rt_control::RealTimeController controller(config, rt_control::RT_PRIORITY::CRITICAL);
    
    // Set up control algorithm callback
    controller.set_control_algorithm([](const rt_control::SensorData& sensor_data, 
                                       rt_control::ControlData& control_data) {
        // Example control algorithm implementation
        // This would be replaced with the actual control algorithm
        
        // Set some example setpoints
        control_data.position_setpoint[0] = 1.0;  // 1m forward
        control_data.position_setpoint[1] = 0.5;  // 0.5m right
        control_data.position_setpoint[2] = 2.0;  // 2m up
        
        // Calculate position errors
        control_data.position_error[0] = control_data.position_setpoint[0] - sensor_data.position[0];
        control_data.position_error[1] = control_data.position_setpoint[1] - sensor_data.position[1];
        control_data.position_error[2] = control_data.position_setpoint[2] - sensor_data.position[2];
        
        // Set control gains (example values)
        control_data.position_gains[0] = 1.0;  // Kp
        control_data.position_gains[1] = 0.1;  // Ki
        control_data.position_gains[2] = 0.5;  // Kd
    });
    
    // Set up motor output callback
    controller.set_motor_output_callback([](const rt_control::MotorCommands& motor_commands) {
        // Example motor output handling
        // This would send the motor commands to the hardware
        std::cout << "Motor commands: [" 
                  << motor_commands.motor_pwm[0] << ", "
                  << motor_commands.motor_pwm[1] << ", "
                  << motor_commands.motor_pwm[2] << ", "
                  << motor_commands.motor_pwm[3] << "]" << std::endl;
    });
    
    // Set up sensor input callback
    controller.set_sensor_input_callback([](const rt_control::SensorData& sensor_data) {
        // Example sensor input handling
        // This would read sensor data from the hardware
        std::cout << "Sensor data received: pos=[" 
                  << sensor_data.position[0] << ", "
                  << sensor_data.position[1] << ", "
                  << sensor_data.position[2] << "]" << std::endl;
    });
    
    // Start the control loop
    controller.start();
    
    // Simulate some sensor data updates
    for (int i = 0; i < 100; ++i) {
        rt_control::SensorData sensor_data;
        sensor_data.position[0] = 0.1 * i;  // Simulated position
        sensor_data.position[1] = 0.05 * i;
        sensor_data.position[2] = 0.2 * i;
        sensor_data.data_valid = true;
        sensor_data.timestamp_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
            std::chrono::high_resolution_clock::now().time_since_epoch()).count();
        
        controller.update_sensor_data(sensor_data);
        
        std::this_thread::sleep_for(std::chrono::milliseconds(10));  // 100Hz update rate
    }
    
    // Stop the control loop
    controller.stop();
    
    std::cout << "Example completed successfully" << std::endl;
    return 0;
}