#include "sensor_simulator.hpp"
#include <iostream>
#include <chrono>
#include <random>
#include <cmath>

SensorSimulator::SensorSimulator(float base_pressure, float noise_level)
    : base_pressure_(base_pressure), 
      current_pressure_(base_pressure),
      noise_level_(noise_level),
      sensor_normal_(true),
      buffer_head_(0),
      buffer_tail_(0),
      buffer_count_(0) {
    
    // 初始化随机数生成器
    std::random_device rd;
    rng_.seed(rd());
    noise_dist_ = std::normal_distribution<float>(0.0f, noise_level);
    
    // 分配缓冲区
    buffer_ = std::make_unique<SensorRawData[]>(BUFFER_SIZE);
    
    std::cout << "传感器模拟器初始化完成，基础气压: " << base_pressure 
              << " hPa，噪声水平: " << noise_level << " hPa" << std::endl;
}

SensorSimulator::~SensorSimulator() {
    // 自动清理缓冲区
}

bool SensorSimulator::initialize() {
    // 重置缓冲区
    clearBuffer();
    
    // 设置初始传感器状态
    sensor_normal_ = true;
    current_pressure_ = base_pressure_;
    
    std::cout << "传感器模拟器已初始化。" << std::endl;
    return true;
}

SensorRawData SensorSimulator::readSensor() {
    // 生成模拟气压值
    float pressure = generatePressure();
    
    // 获取当前时间戳（毫秒）
    auto now = std::chrono::system_clock::now();
    auto timestamp_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
        now.time_since_epoch()).count();
    
    // 设置状态标志位
    uint8_t flags = 0;
    if (!sensor_normal_) {
        flags |= 0x01;  // 传感器故障标志
    }
    
    // 创建传感器数据
    SensorRawData data(pressure, static_cast<uint32_t>(timestamp_ms), flags);
    
    // 添加到缓冲区
    addToBuffer(data);
    
    // 更新当前气压值
    current_pressure_ = pressure;
    
    return data;
}

float SensorSimulator::getCurrentPressure() const {
    return current_pressure_;
}

std::vector<SensorRawData> SensorSimulator::getBufferData(size_t count) const {
    std::vector<SensorRawData> data;
    
    // 确保不超过现有数据点数
    size_t actual_count = std::min(count, buffer_count_);
    
    // 从最新数据开始获取（从head向前）
    for (size_t i = 0; i < actual_count; ++i) {
        // 计算索引：head_-1-i，考虑环形缓冲区
        size_t index = (buffer_head_ + BUFFER_SIZE - 1 - i) % BUFFER_SIZE;
        data.push_back(buffer_[index]);
    }
    
    return data;
}

void SensorSimulator::clearBuffer() {
    buffer_head_ = 0;
    buffer_tail_ = 0;
    buffer_count_ = 0;
    std::cout << "传感器缓冲区已清空。" << std::endl;
}

void SensorSimulator::setSensorStatus(bool normal) {
    bool previous_status = sensor_normal_;
    sensor_normal_ = normal;
    
    if (previous_status != sensor_normal_) {
        if (sensor_normal_) {
            std::cout << "传感器状态：恢复正常" << std::endl;
        } else {
            std::cout << "传感器状态：发生故障" << std::endl;
        }
    }
}

void SensorSimulator::setBasePressure(float pressure) {
    // 验证气压值是否合理
    if (pressure >= 300.0f && pressure <= 1100.0f) {
        base_pressure_ = pressure;
        std::cout << "基础气压已更新为: " << pressure << " hPa" << std::endl;
    } else {
        std::cerr << "无效的基础气压值: " << pressure << " hPa" << std::endl;
    }
}

void SensorSimulator::setNoiseLevel(float noise_level) {
    if (noise_level >= 0.0f && noise_level <= 10.0f) {
        noise_level_ = noise_level;
        noise_dist_ = std::normal_distribution<float>(0.0f, noise_level);
        std::cout << "噪声水平已更新为: " << noise_level << " hPa" << std::endl;
    } else {
        std::cerr << "无效的噪声水平: " << noise_level << " hPa" << std::endl;
    }
}

void SensorSimulator::simulateHeightChange(float delta_height) {
    // 根据高度变化计算气压变化
    float pressure_change = pressureChangeForHeight(delta_height);
    base_pressure_ += pressure_change;
    
    // 确保气压值在合理范围内
    if (base_pressure_ < 300.0f) base_pressure_ = 300.0f;
    if (base_pressure_ > 1100.0f) base_pressure_ = 1100.0f;
    
    std::cout << "模拟高度变化: " << delta_height << " 米，"
              << "气压变化: " << pressure_change << " hPa，"
              << "新基础气压: " << base_pressure_ << " hPa" << std::endl;
}

float SensorSimulator::generatePressure() {
    float pressure = base_pressure_;
    
    // 添加噪声（如果传感器正常）
    if (sensor_normal_) {
        pressure += noise_dist_(rng_);
    } else {
        // 传感器故障时，返回无效值或固定错误值
        pressure = 0.0f;  // 表示传感器故障
    }
    
    // 确保气压值在合理范围内
    if (pressure < 300.0f) pressure = 300.0f;
    if (pressure > 1100.0f) pressure = 1100.0f;
    
    return pressure;
}

void SensorSimulator::addToBuffer(const SensorRawData& data) {
    if (BUFFER_SIZE == 0) {
        return;
    }
    
    // 将数据添加到环形缓冲区
    buffer_[buffer_head_] = data;
    buffer_head_ = nextBufferIndex(buffer_head_);
    
    // 更新计数
    if (buffer_count_ < BUFFER_SIZE) {
        buffer_count_++;
    } else {
        // 缓冲区已满，移动尾指针
        buffer_tail_ = nextBufferIndex(buffer_tail_);
    }
}

size_t SensorSimulator::nextBufferIndex(size_t current) const {
    return (current + 1) % BUFFER_SIZE;
}

float SensorSimulator::pressureChangeForHeight(float height) const {
    // 简化公式：每升高100米，气压下降约12 hPa
    // 实际公式更复杂，这里使用近似值
    constexpr float PRESSURE_LAPSE_RATE = -12.0f / 100.0f;  // hPa/米
    return height * PRESSURE_LAPSE_RATE;
}