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

using InnovEnergy.Lib.Devices.Battery48TL.DataTypes;
using InnovEnergy.Lib.Devices.Trumpf.TruConvertAc;
using InnovEnergy.Lib.Devices.Trumpf.TruConvertAc.DataTypes;
using InnovEnergy.Lib.Time.Unix;
using InnovEnergy.Lib.Utils;

namespace InnovEnergy.App.SaliMax.Ess;

public static class Controller
{
    private static readonly UnixTimeSpan MaxTimeWithoutEoc = UnixTimeSpan.FromDays(7); // TODO: move to config
    
    public static EssMode SelectControlMode(this StatusRecord s)
    {
        //return EssMode.OptimizeSelfConsumption;
        
        return s.StateMachine.State != 16  ? 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();

        mode.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).WriteLine("Max");
        var minPower = acDcs.Min(d => d.Status.Ac.Power.Active.Value).WriteLine("Min");
        
        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.ReferenceDcBusVoltage
             ? 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 maxInverterDischargeDelta = s.ControlInverterPower(-s.Config.MaxInverterPower);
        var maxBatteryDischargeDelta  = s.Battery.Devices.Sum(b => b.MaxDischargePower);
        var keepMinSocLimitDelta      = s.ControlBatteryPower(s.HoldMinSocPower());

        return control
          //    .LimitDischargePower(maxInverterDischargeDelta, EssLimit.DischargeLimitedByInverterPower)
              .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 HasPreChargeAlarm(this StatusRecord statusRecord)
    {
        return statusRecord.DcDc.Alarms.Contains(Lib.Devices.Trumpf.TruConvertDc.Status.AlarmMessage.DcDcPrecharge);
    }

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

        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)
    {
        return s.Battery.Devices.Sum(b => b.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.Battery.Devices;
        
        return batteries.Count > 0
            && batteries.Any(b => b.Soc < s.Config.MinSoc);
    }


    private static Boolean MustDoCalibrationCharge(this StatusRecord statusRecord)
    {
        var config = statusRecord.Config;

        if (statusRecord.Battery.Eoc)
        {
            "Batteries have reached EOC".LogInfo();
            config.LastEoc = UnixTime.Now;
            return false;
        }

        return UnixTime.Now - statusRecord.Config.LastEoc > MaxTimeWithoutEoc;
    }


    public 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.Battery.Devices.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.Battery.Devices;

        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;
    }

    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;
    }

    private static IEnumerable<InverterState> InverterStates(this AcDcDevicesRecord acDcStatus)
    {
        return acDcStatus
              .Devices
              .Select(d => d.Status.InverterState.Current);
    }

}