using InnovEnergy.Lib.Protocols.Modbus.Conversions;
using InnovEnergy.Lib.Utils;

namespace InnovEnergy.Lib.Devices.Battery48TL;

public static class BatteryDataParser
{
    public static Decimal ParseDecimal(this ModbusRegisters data, Int32 register, Decimal scaleFactor = 1.0m, Double offset = 0.0)
    {
        var value = data[register].ConvertTo<Int32>();  // widen to 32bit signed

        if (value >= 0x8000)
            value -= 0x10000;    // Fiamm stores their integers signed AND with sign-offset @#%^&!

        return (Decimal)(value + offset) * scaleFactor;
    }

    internal static Decimal ReadCurrent(this ModbusRegisters data)
    {
        return ParseDecimal(data, register: 1001, scaleFactor: 0.01m, offset: -10000);
    }

    internal static Decimal ReadVoltage(this ModbusRegisters data)
    {
        return ParseDecimal(data, register:  1000, scaleFactor: 0.01m);
    }

    internal static Boolean ParseBool(this ModbusRegisters data, Int32 baseRegister, Int16 bit)
    {
        var x = bit / 16;
        var y = bit % 16;
        
        var value = (UInt32)data[baseRegister + x]; 

        return (value & (1 << y)) > 0;
    }

    internal static LedState ParseLedState(this ModbusRegisters data, Int32 register, LedColor led)
    {
        var lo = ParseBool(data, register, (led.ConvertTo<Int16>() * 2).ConvertTo<Int16>());
        var hi = ParseBool(data, register, (led.ConvertTo<Int16>() * 2 + 1).ConvertTo<Int16>());

        if (hi)
        {
            if (lo)
            {
                return LedState.BlinkingFast;
            }
            else
            {
                return LedState.BlinkingSlow;
            }
        }
        else
        {
            if (lo)
            {
                return LedState.On;
            }
            else
            {
                return LedState.Off;
            }
        }

    }

    internal static String ParseRegisters(this ModbusRegisters data, Int32 register, Int16 count)
    {
        var container = "";

        var start = register;
        var end = register + count;

        for (var i = start; i < end; i++)
        {
            var binary = Convert.ToString(data[register], 2);
            container += binary.PadLeft(16, '0');
        }
        return container;
    }

    internal static Boolean ParseEocReached(this ModbusRegisters data)
    {
        return ParseLedState(data, 1005, LedColor.Green) == LedState.On &&
               ParseLedState(data, 1005, LedColor.Amber) == LedState.Off &&
               ParseLedState(data, 1005, LedColor.Blue) == LedState.Off;
    }

    internal static Boolean ParseBatteryCold(this ModbusRegisters data)
    {
        return ParseLedState(data, 1005, LedColor.Green) >= LedState.BlinkingSlow &&
               ParseLedState(data, 1005, LedColor.Blue) >= LedState.BlinkingSlow;
    }

    private static Decimal CalcPowerLimitImposedByVoltageLimit(Decimal v,Decimal i,Decimal vLimit,Decimal rInt)
    {
        var dv = vLimit - v;
        var di = dv / rInt;
        var pLimit = vLimit * (i + di);

        return pLimit;
    }

    private static Decimal CalcPowerLimitImposedByCurrentLimit(Decimal v, Decimal i, Decimal iLimit, Decimal rInt)
    {
        var di = iLimit - i;
        var dv = di * rInt;
        var pLimit = iLimit * (v + dv);

        return pLimit;
    }
    
    
    private static Decimal CalcPowerLimitImposedByTempLimit(Decimal t, Decimal maxAllowedTemp, Decimal power , Decimal setpoint)
    {
       // const Int32 holdZone = 300;
       // const Int32 maxAllowedTemp = 315;

       var kp = 0.05m;
       var error = setpoint - power;
       var controlOutput = (kp * error) *(1 - Math.Abs((t-307.5m)/7.5m));
       
       return controlOutput;  

        // var a = holdZone - maxAllowedTemp;
        // var b = -a * maxAllowedTemp;
    }

    internal static Decimal CalcMaxChargePower(this ModbusRegisters data)
    {
        var v = ReadVoltage(data);
        var i = ReadCurrent(data);

        var pLimits = new[]
        {
            CalcPowerLimitImposedByVoltageLimit(v, i, Constants.VMax, Constants.RIntMin),
            CalcPowerLimitImposedByVoltageLimit(v, i, Constants.VMax, Constants.RIntMax),
            CalcPowerLimitImposedByCurrentLimit(v, i, Constants.IMax, Constants.RIntMin),
            CalcPowerLimitImposedByCurrentLimit(v, i, Constants.IMax, Constants.RIntMax)
        };

        var pLimit = pLimits.Min();

        return Math.Max(pLimit, 0);
    }

    internal static Decimal CalcMaxDischargePower(this ModbusRegisters data)
    {
        var v = ReadVoltage(data);
        var i = ReadCurrent(data);
        var t = data.ParseDecimal(register: 1004, scaleFactor: 0.1m, offset: -400);

        var pLimits = new[]
        {
            CalcPowerLimitImposedByVoltageLimit(v, i, Constants.VMin, Constants.RIntMin),
            CalcPowerLimitImposedByVoltageLimit(v, i, Constants.VMin, Constants.RIntMax),
            CalcPowerLimitImposedByCurrentLimit(v, i, -Constants.IMax, Constants.RIntMin),
            CalcPowerLimitImposedByCurrentLimit(v, i, -Constants.IMax, Constants.RIntMax),
           // CalcPowerLimitImposedByTempLimit(t,315,300)
        };

        var pLimit = pLimits.Max();

        return Math.Min(pLimit, 0);
    }
}