Innovenergy_trunk/csharp/App/SaliMax/src/Ess/Controller.cs

using InnovEnergy.App.SaliMax.SystemConfig;
using InnovEnergy.Lib.Devices.Battery48TL;
using InnovEnergy.Lib.Devices.Battery48TL.DataTypes;
using InnovEnergy.Lib.Devices.Trumpf.TruConvertAc.DataTypes;
using InnovEnergy.Lib.Units;
using InnovEnergy.Lib.Utils;

namespace InnovEnergy.App.SaliMax.Ess;

public static class Controller
{
    private static readonly Double BatteryHeatingPower = 200.0; // TODO: move to config
    
    public static EssMode SelectControlMode(this StatusRecord s)
    {
        //return EssMode.OptimizeSelfConsumption;
        
        return s.StateMachine.State != 23  ? EssMode.Off
             : s.MustHeatBatteries()       ? EssMode.HeatBatteries
             : s.MustDoCalibrationCharge() ? EssMode.CalibrationCharge
             : s.MustReachMinSoc()         ? EssMode.ReachMinSoc
             : s.GridMeter is null         ? EssMode.NoGridMeter
             :                               EssMode.OptimizeSelfConsumption;
    }


    public static EssControl ControlEss(this StatusRecord s)
    {
        var mode = s.SelectControlMode().WriteLine();

        if (mode is EssMode.Off or EssMode.NoGridMeter)
            return EssControl.Default;

        var essDelta = s.ComputePowerDelta(mode);

        var unlimitedControl = new EssControl
        {
            Mode            = mode,
            LimitedBy       = EssLimit.NoLimit,
            PowerCorrection = essDelta,
            PowerSetpoint   = 0
        };
        
        var limitedControl = unlimitedControl
                            .LimitChargePower(s)
                            .LimitDischargePower(s)
                            .LimitInverterPower(s);

        var currentPowerSetPoint = s.CurrentPowerSetPoint();

        return limitedControl with { PowerSetpoint = currentPowerSetPoint + limitedControl.PowerCorrection };
    }

    private static EssControl LimitInverterPower(this EssControl control, StatusRecord s)
    {
        var powerDelta = control.PowerCorrection.Value;
        
        var acDcs = s.AcDc.Devices;

        var nInverters = acDcs.Count;
        
        if (nInverters < 2)
            return control;   // current loop cannot happen 

        var nominalPower = acDcs.Average(d => d.Status.Nominal.Power);
        var maxStep = nominalPower / 25; //TODO magic number to config
        
        var clampedPowerDelta = powerDelta.Clamp(-maxStep, maxStep);
        
        var dcLimited = acDcs.Any(d => d.Status.PowerLimitedBy == PowerLimit.DcLink);

        if (!dcLimited)
            return control with { PowerCorrection = clampedPowerDelta };

        var maxPower = acDcs.Max(d => d.Status.Ac.Power.Active.Value);
        var minPower = acDcs.Min(d => d.Status.Ac.Power.Active.Value);
        
        var powerDifference = maxPower - minPower;

        if (powerDifference < maxStep)
            return control with { PowerCorrection = clampedPowerDelta };

        var correction = powerDifference / 4; //TODO magic number to config
        
        
        // find out if we reach the lower or upper Dc limit by comparing the current Dc voltage to the reference voltage
        return s.AcDc.Dc.Voltage > s.Config.GridTie.AcDc.ReferenceDcLinkVoltage
             ? control with { PowerCorrection = clampedPowerDelta.ClampMax(-correction), LimitedBy = EssLimit.ChargeLimitedByMaxDcBusVoltage }
             : control with { PowerCorrection = clampedPowerDelta.ClampMin(correction), LimitedBy = EssLimit.DischargeLimitedByMinDcBusVoltage };
    }

   
    private static EssControl LimitChargePower(this EssControl control, StatusRecord s)
    {
                
        //var maxInverterChargePower = s.ControlInverterPower(s.Config.MaxInverterPower);
        var maxBatteryChargePower  = s.MaxBatteryChargePower();
        
        return control
              //.LimitChargePower(, EssLimit.ChargeLimitedByInverterPower)
              .LimitChargePower(maxBatteryChargePower, EssLimit.ChargeLimitedByBatteryPower);
        
    }
    
    private static EssControl LimitDischargePower(this EssControl control, StatusRecord s)
    {
        var maxBatteryDischargeDelta = s.Battery?.Devices.Where(b => b.ConnectedToDcBus).Sum(b => b.MaxDischargePower) ?? 0;
        var keepMinSocLimitDelta     = s.ControlBatteryPower(s.HoldMinSocPower());

        return control
              .LimitDischargePower(maxBatteryDischargeDelta , EssLimit.DischargeLimitedByBatteryPower)
              .LimitDischargePower(keepMinSocLimitDelta     , EssLimit.DischargeLimitedByMinSoc);
    }

    

    

    private static Double ComputePowerDelta(this StatusRecord s, EssMode mode)
    {
        var chargePower = s.AcDc.Devices.Sum(d => d.Status.Nominal.Power.Value);

        return mode switch
        {
            EssMode.HeatBatteries            => s.ControlInverterPower(chargePower),
            EssMode.ReachMinSoc              => s.ControlInverterPower(chargePower),
            EssMode.CalibrationCharge        => s.ControlInverterPower(chargePower),
            EssMode.OptimizeSelfConsumption  => s.ControlGridPower(s.Config.GridSetPoint),
            _                                => throw new ArgumentException(null, nameof(mode))
        };
    }

    private static Boolean MustHeatBatteries(this StatusRecord s)
    {
        var batteries = s.GetBatteries();

        if (batteries.Count <= 0)
            return true; // batteries might be there but BMS is without power 

        return batteries
              .Select(b => b.Temperatures.State)
              .Contains(TemperatureState.Cold);
    }

    private static Double MaxBatteryChargePower(this StatusRecord s)
    {
        // This introduce a limit when we don't have communication with batteries
        // Otherwise the limit will be 0 and the batteries will be not heated
                                
        var batteries = s.GetBatteries();
        
        var maxChargePower = batteries.Count == 0 
                           ? s.Config.Devices.BatteryNodes.Length * BatteryHeatingPower
                           : batteries.Sum(b => b.MaxChargePower);
        
        maxChargePower.W().ToDisplayString().WriteLine(" Max Charge Power");
        
        return maxChargePower;
    }

    private static Double CurrentPowerSetPoint(this StatusRecord s)
    {
        return s
              .AcDc
              .Devices
              .Select(d =>
              {
                  var acPowerControl = d.Control.Ac.Power;
                  
                  return acPowerControl.L1.Active 
                       + acPowerControl.L2.Active 
                       + acPowerControl.L3.Active;
              })
              .Sum(p => p);
    }

    private static Boolean MustReachMinSoc(this StatusRecord s)
    {
        var batteries = s.GetBatteries();
        
        return batteries.Count > 0
            && batteries.Any(b => b.Soc < s.Config.MinSoc);
    }

    private static IReadOnlyList<Battery48TlRecord> GetBatteries(this StatusRecord s)
    {
        return s.Battery?.Devices ?? Array.Empty<Battery48TlRecord>();
    }

    private static Boolean MustDoCalibrationCharge(this StatusRecord statusRecord)
    {       
        var calibrationChargeForced           = statusRecord.Config.ForceCalibrationCharge;
        var batteryCalibrationChargeRequested = statusRecord.Battery?.CalibrationChargeRequested?? false ;

        var mustDoCalibrationCharge           = batteryCalibrationChargeRequested || calibrationChargeForced == CalibrationChargeType.Yes || calibrationChargeForced == CalibrationChargeType.UntilEoc ;

        if (statusRecord.Battery is not null)
        {
            if (calibrationChargeForced == CalibrationChargeType.UntilEoc && statusRecord.Battery.Eoc )
            {
                statusRecord.Config.ForceCalibrationCharge = CalibrationChargeType.No;
            }
        }
        return mustDoCalibrationCharge;
    }


    private static Double ControlGridPower(this StatusRecord status, Double targetPower)
    {
        return ControlPower
        (
            measurement : status.GridMeter!.Ac.Power.Active,
            target      : targetPower,
            pConstant   : status.Config.PConstant
        );
    }

    public static Double ControlInverterPower(this StatusRecord status, Double targetInverterPower)
    {
        return ControlPower
        (
            measurement : status.AcDc.Ac.Power.Active,
            target      : targetInverterPower,
            pConstant   : status.Config.PConstant
        );
    }

    public static Double ControlBatteryPower(this StatusRecord status, Double targetBatteryPower)
    {
        return ControlPower
        (
            measurement: status.GetBatteries().Sum(b => b.Dc.Power),
            target: targetBatteryPower,
            pConstant: status.Config.PConstant
        );
    }

    private static Double HoldMinSocPower(this StatusRecord s)
    {
        // TODO: explain LowSOC curve

        var batteries = s.GetBatteries();

        if (batteries.Count == 0)
            return Double.NegativeInfinity;

        var a = -2 * s.Config.BatterySelfDischargePower * batteries.Count / s.Config.HoldSocZone;
        var b = -a * (s.Config.MinSoc + s.Config.HoldSocZone);

        return batteries.Min(d => d.Soc.Value) * a + b;
    }

    private static Double ControlPower(Double measurement, Double target, Double pConstant)
    {
        var error = target - measurement;
        return error * pConstant;
    }

    // ReSharper disable once UnusedMember.Local, TODO
    private static Double ControlPowerWithIntegral(Double measurement, Double target, Double p, Double i)
    {
        var errorSum = 0; // this is must be sum of error
        var error = target - measurement;
        var kp = p * error;
        var ki = i * errorSum;
        return ki + kp;
    }

}