use rust_decimal::Decimal;
use std::collections::VecDeque;

#[derive(Debug, Clone)]
pub struct PidController {
    kp: Decimal,
    ki: Decimal,
    kd: Decimal,
    setpoint: Decimal,
    integral: Decimal,
    last_error: Option<Decimal>,
    prev_error: Option<Decimal>,
    last_timestamp: Option<i64>,
    error_history: VecDeque<Decimal>,
    max_history: usize,
    integral_limit: Option<Decimal>,
    output_limit: Option<(Decimal, Decimal)>,
    default_dt: Decimal,
}

impl PidController {
    pub fn new(
        kp: Decimal,
        ki: Decimal,
        kd: Decimal,
        setpoint: Decimal,
        max_history: usize,
    ) -> Self {
        Self {
            kp,
            ki,
            kd,
            setpoint,
            integral: Decimal::ZERO,
            last_error: None,
            prev_error: None,
            last_timestamp: None,
            error_history: VecDeque::with_capacity(max_history),
            max_history,
            integral_limit: None,
            output_limit: None,
            default_dt: Decimal::ONE, // Default: 1 time unit
        }
    }

    pub fn with_integral_limit(mut self, limit: Decimal) -> Self {
        self.integral_limit = Some(limit);
        self
    }

    pub fn with_output_limit(mut self, min: Decimal, max: Decimal) -> Self {
        self.output_limit = Some((min, max));
        self
    }

    pub fn update(&mut self, current_value: Decimal) -> Decimal {
        self.update_with_timestamp(current_value, None)
    }

    pub fn update_with_timestamp(
        &mut self,
        current_value: Decimal,
        timestamp: Option<i64>,
    ) -> Decimal {
        let error = self.setpoint - current_value;

        // Calculate dt (time interval)
        let dt = if let (Some(current_ts), Some(last_ts)) = (timestamp, self.last_timestamp) {
            let dt_seconds = (current_ts - last_ts).max(1); // Minimum 1 second
            Decimal::from(dt_seconds)
        } else {
            self.default_dt
        };

        self.error_history.push_back(error);
        if self.error_history.len() > self.max_history {
            self.error_history.pop_front();
        }

        let proportional = self.kp * error;

        // Integral term with dt scaling
        self.integral += error * dt;
        if let Some(limit) = self.integral_limit {
            self.integral = self.integral.clamp(-limit, limit);
        }
        let integral = self.ki * self.integral;

        // Derivative term with dt scaling
        let derivative = if let Some(last_err) = self.last_error {
            self.kd * (error - last_err) / dt
        } else {
            Decimal::ZERO
        };

        self.prev_error = self.last_error;
        self.last_error = Some(error);
        self.last_timestamp = timestamp;

        let mut output = proportional + integral + derivative;

        if let Some((min, max)) = self.output_limit {
            output = output.clamp(min, max);
        }

        output
    }

    pub fn set_setpoint(&mut self, setpoint: Decimal) {
        self.setpoint = setpoint;
    }

    pub fn reset(&mut self) {
        self.integral = Decimal::ZERO;
        self.last_error = None;
        self.prev_error = None;
        self.last_timestamp = None;
        self.error_history.clear();
    }

    pub fn get_error_metrics(&self) -> (Decimal, Decimal, Decimal) {
        if self.error_history.is_empty() {
            return (Decimal::ZERO, Decimal::ZERO, Decimal::ZERO);
        }

        let sum: Decimal = self.error_history.iter().sum();
        let mean = sum / Decimal::from(self.error_history.len());

        let variance = self
            .error_history
            .iter()
            .map(|&e| {
                let diff = e - mean;
                diff * diff
            })
            .sum::<Decimal>()
            / Decimal::from(self.error_history.len());

        // Approximate square root for variance (simplified)
        let std_dev = if variance.is_zero() {
            Decimal::ZERO
        } else {
            // Simple Newton's method approximation for square root
            let mut x = variance;
            for _ in 0..10 {
                let x_next = (x + variance / x) / Decimal::from(2);
                if (x_next - x).abs() < Decimal::new(1, 6) {
                    // precision of 0.000001
                    break;
                }
                x = x_next;
            }
            x
        };

        (mean, variance, std_dev)
    }

    pub fn get_components(&self) -> (Decimal, Decimal, Decimal) {
        let error = self.last_error.unwrap_or(Decimal::ZERO);
        let proportional = self.kp * error;
        let integral = self.ki * self.integral;
        let derivative = match (self.last_error, self.prev_error) {
            (Some(current_err), Some(prev_err)) => self.kd * (current_err - prev_err),
            _ => Decimal::ZERO,
        };

        (proportional, integral, derivative)
    }
}

#[derive(Debug, Clone)]
pub struct AdaptivePidController {
    base_controller: PidController,
    adaptive_gains: bool,
    kp_range: (Decimal, Decimal),
    ki_range: (Decimal, Decimal),
    kd_range: (Decimal, Decimal),
    performance_window: usize,
    performance_history: VecDeque<Decimal>,
}

impl AdaptivePidController {
    pub fn new(
        kp: Decimal,
        ki: Decimal,
        kd: Decimal,
        setpoint: Decimal,
        max_history: usize,
        performance_window: usize,
    ) -> Self {
        Self {
            base_controller: PidController::new(kp, ki, kd, setpoint, max_history),
            adaptive_gains: true,
            kp_range: (kp * Decimal::new(5, 1), kp * Decimal::new(15, 1)), // 0.5x to 1.5x
            ki_range: (ki * Decimal::new(5, 1), ki * Decimal::new(15, 1)),
            kd_range: (kd * Decimal::new(5, 1), kd * Decimal::new(15, 1)),
            performance_window,
            performance_history: VecDeque::with_capacity(performance_window),
        }
    }

    pub fn update(&mut self, current_value: Decimal) -> Decimal {
        self.update_with_timestamp(current_value, None)
    }

    pub fn update_with_timestamp(
        &mut self,
        current_value: Decimal,
        timestamp: Option<i64>,
    ) -> Decimal {
        let output = self
            .base_controller
            .update_with_timestamp(current_value, timestamp);

        let error = (self.base_controller.setpoint - current_value).abs();
        self.performance_history.push_back(error);
        if self.performance_history.len() > self.performance_window {
            self.performance_history.pop_front();
        }

        if self.adaptive_gains && self.performance_history.len() >= self.performance_window {
            self.adapt_gains();
        }

        output
    }

    fn adapt_gains(&mut self) {
        let error_trend = self.calculate_error_trend();

        if error_trend > Decimal::new(1, 1) {
            // Error increasing
            self.base_controller.kp =
                (self.base_controller.kp * Decimal::new(11, 1)).min(self.kp_range.1);
            self.base_controller.ki =
                (self.base_controller.ki * Decimal::new(95, 2)).max(self.ki_range.0);
        } else if error_trend < -Decimal::new(1, 1) {
            // Error decreasing
            self.base_controller.kp =
                (self.base_controller.kp * Decimal::new(98, 2)).max(self.kp_range.0);
            self.base_controller.ki =
                (self.base_controller.ki * Decimal::new(102, 2)).min(self.ki_range.1);
        }
    }

    fn calculate_error_trend(&self) -> Decimal {
        if self.performance_history.len() < 2 {
            return Decimal::ZERO;
        }

        let recent_avg = self
            .performance_history
            .iter()
            .skip(self.performance_history.len() / 2)
            .sum::<Decimal>()
            / Decimal::from(self.performance_history.len() / 2);

        let early_avg = self
            .performance_history
            .iter()
            .take(self.performance_history.len() / 2)
            .sum::<Decimal>()
            / Decimal::from(self.performance_history.len() / 2);

        if early_avg.is_zero() {
            return Decimal::ZERO;
        }

        (recent_avg - early_avg) / early_avg * Decimal::from(100)
    }

    pub fn set_setpoint(&mut self, setpoint: Decimal) {
        self.base_controller.set_setpoint(setpoint);
    }

    pub fn reset(&mut self) {
        self.base_controller.reset();
        self.performance_history.clear();
    }

    pub fn get_gains(&self) -> (Decimal, Decimal, Decimal) {
        (
            self.base_controller.kp,
            self.base_controller.ki,
            self.base_controller.kd,
        )
    }

    pub fn get_error_metrics(&self) -> (Decimal, Decimal, Decimal) {
        self.base_controller.get_error_metrics()
    }

    pub fn get_components(&self) -> (Decimal, Decimal, Decimal) {
        self.base_controller.get_components()
    }
}

#[derive(Debug, Clone)]
pub struct PidThresholds {
    pub kp: Decimal,
    pub ki: Decimal,
    pub kd: Decimal,
    pub setpoint_mode: SetpointMode,
    pub adaptive: bool,
    pub max_error_threshold: Decimal,
    pub signal_threshold: Decimal,
    pub reset_threshold: Decimal,
}

#[derive(Debug, Clone)]
pub enum SetpointMode {
    FixedPrice(Decimal),
    MovingAverage(usize),
    ATRBased(Decimal),    // ATR multiplier
    RSIBaseline(Decimal), // RSI target level
}

impl Default for PidThresholds {
    fn default() -> Self {
        Self {
            kp: Decimal::new(1, 1), // 0.1
            ki: Decimal::new(5, 2), // 0.05
            kd: Decimal::new(2, 2), // 0.02
            setpoint_mode: SetpointMode::MovingAverage(20),
            adaptive: true,
            max_error_threshold: Decimal::new(5, 0), // 5%
            signal_threshold: Decimal::new(1, 0),    // 1.0
            reset_threshold: Decimal::new(10, 0),    // 10%
        }
    }
}
