tesla-charge-controller/src/tesla_charge_rate.rs

use std::sync::{Arc, RwLock};

use if_chain::if_chain;
use lazy_static::lazy_static;
use prometheus::{register_gauge, register_int_gauge, Gauge, IntGauge};
use serde::{Deserialize, Serialize};

use crate::{
    api_interface::InterfaceRequest,
    charge_controllers::pl::PlState,
    config::access_config,
    types::{CarState, ChargeState},
};

lazy_static! {
    pub static ref CONTROL_ENABLE_GAUGE: IntGauge =
        register_int_gauge!("tcrc_control_enable", "Enable Tesla charge rate control",).unwrap();
    pub static ref PROPORTIONAL_GAUGE: Gauge = register_gauge!(
        "tcrc_proportional",
        "Proportional component of requested change to charge rate",
    )
    .unwrap();
    pub static ref DERIVATIVE_GAUGE: Gauge = register_gauge!(
        "tcrc_derivative",
        "Derivative component of requested change to charge rate",
    )
    .unwrap();
    pub static ref LOAD_GAUGE: Gauge = register_gauge!(
        "tcrc_load",
        "Fudge factor from internal load of requested change to charge rate",
    )
    .unwrap();
    pub static ref CHANGE_REQUEST_GAUGE: Gauge =
        register_gauge!("tcrc_change_request", "Requested change to charge rate",).unwrap();
}

pub struct TeslaChargeRateController {
    pub car_state: Arc<RwLock<CarState>>,
    pub pl_state: Option<Arc<RwLock<PlState>>>,
    pub tcrc_state: Arc<RwLock<TcrcState>>,
    pid: PidLoop,
    voltage_low: u64,
}

#[allow(dead_code)]
pub enum TcrcRequest {
    DisableAutomaticControl,
    EnableAutomaticControl,
}

#[derive(Clone, Copy, Serialize, Deserialize, Debug)]
pub struct TcrcState {
    pub control_enable: bool,
}

impl TcrcState {
    pub fn set_control_enable(&mut self, to: bool) {
        self.control_enable = to;
        CONTROL_ENABLE_GAUGE.set(if to { 1 } else { 0 });
    }
}

impl Default for TcrcState {
    fn default() -> Self {
        Self {
            control_enable: true,
        }
    }
}

impl TeslaChargeRateController {
    pub fn new(car_state: Arc<RwLock<CarState>>, pl_state: Option<Arc<RwLock<PlState>>>) -> Self {
        Self {
            car_state,
            pl_state,
            tcrc_state: Default::default(),
            pid: Default::default(),
            voltage_low: 0,
        }
    }

    pub fn control_charge_rate(&mut self) -> Option<InterfaceRequest> {
        let delta_time = access_config().pid_controls.loop_time_seconds;
        if_chain! {
            if let Some(pl_state) = self.pl_state.as_ref().and_then(|v| v.read().ok());
            if let Some(charge_state) = self.car_state.read().ok().and_then(|v| v.charge_state);
            then {
                if pl_state.battery_voltage < access_config().shutoff_voltage {
                    self.voltage_low += 1;
                    if (self.voltage_low * delta_time)
                        >= access_config().shutoff_voltage_time_seconds
                    {
                        return Some(InterfaceRequest::StopCharge);
                    }
                } else {
                    self.voltage_low = 0;
                    if self.tcrc_state.read().is_ok_and(|v| v.control_enable) {
                        return self.pid.step(&pl_state, &charge_state, delta_time as f64).map(InterfaceRequest::SetChargeRate);
                    }
                }
            }
        }
        None
    }

    pub fn process_request(&mut self, message: TcrcRequest) {
        match message {
            TcrcRequest::DisableAutomaticControl => {
                self.tcrc_state
                    .write()
                    .expect("failed to write to tcrc state")
                    .set_control_enable(false);
            }
            TcrcRequest::EnableAutomaticControl => {
                self.tcrc_state
                    .write()
                    .expect("failed to write to tcrc state")
                    .set_control_enable(true);
            }
        }
    }
}

struct PidLoop {
    previous_error: f64,
}

impl Default for PidLoop {
    fn default() -> Self {
        Self { previous_error: 0. }
    }
}

impl PidLoop {
    fn step(
        &mut self,
        pl_state: &PlState,
        charge_state: &ChargeState,
        delta_time: f64,
    ) -> Option<i64> {
        let error = pl_state.battery_voltage - pl_state.target_voltage;
        let derivative = (error - self.previous_error) / delta_time;
        let config = access_config();

        let proportional_component = config.pid_controls.proportional_gain * error;
        let derivative_component = config.pid_controls.derivative_gain * derivative;

        let extra_offsets =
            (pl_state.internal_load_current / config.pid_controls.load_divisor).clamp(0., 2.);

        let offset = proportional_component + derivative_component + extra_offsets;

        PROPORTIONAL_GAUGE.set(proportional_component);
        DERIVATIVE_GAUGE.set(derivative_component);
        LOAD_GAUGE.set(extra_offsets);
        CHANGE_REQUEST_GAUGE.set(offset);

        let new_target = offset + (charge_state.charge_amps as f64);

        self.previous_error = error;

        let new_target_int = new_target.round() as i64;
        valid_rate(new_target_int, charge_state.charge_amps)
    }
}

fn valid_rate(rate: i64, previous: i64) -> Option<i64> {
    let config = access_config();
    let new = rate.clamp(config.min_rate, config.max_rate);
    if new == previous {
        None
    } else {
        Some(new)
    }
}