//LiFePO4 4S battery soc and level calculation library

//todo:
  //- battery level will forced after 60-seconds if not passing battPercentageLevelThreshold (optional)

static uint8_t battPercentageLevelThreshold = 5; //threshold for switch discrete level 0-4
static uint8_t battLastLevel = 0;
static uint8_t battLastPercentageForLevel = 0;

//variables
static float battVoltageFullChargeMinimal = 14.4; //used for longevity charge
static float battVoltageFullChargeMaximal = 14.6; //3.60V - 3.65V * 4S = 14.4V - 14.6V (can be used for "topped-off" charge mode, )
static float battVoltageFullRest = 13.6;   //~3.40V - 3.50V * 4S = ~13.6V - 13.7V (can be used for "optimal" charge mode, preferred for longevity)
static float battVoltageUpperZone = 13.3; //Also can be used for "Storage/Long-term Maintenance" charge mode, or after long usage without load  ~13.2V - 13.3V (50-60% state of charge)
static float battVoltageLowerZone = 12.9;
//float battVoltageHalf = 13.1;      //~3.25V - 3.28V * 4S = ~13.0V - 13.1V, known as nomimal
static float battVoltageEmpty = 10.0;      //~2.50V - 2.80V * 4S = ~10.0V - 11.2V
static float battVoltageCritical = 12.0;   //Default ~10% => 3.0V * 4S = 12V
static float battVoltageForceShutdown = 11.2;   //Default ~5% => 2.8V * 4S = 11.2V
static float battVoltageResumeChargingDefault = 13.4; //Default ~90% = ~3.35V *4S = 13.4V

//error condition
static float battOverVoltage = 15.3; //3.75V - 3.80V * 4S = 15V - 15.3V
static float battVoltageDetected = 8;
static float battVoltageNotDetected = 7;

static void resetBatteryLevel() {
  battLastLevel = 0;
  battLastPercentageForLevel = 0;
}

static void setBatteryPercentage(uint8_t soc) {
  if (soc <= 0) {
    battLastLevel = 0;
    battLastPercentageForLevel = 0;
    return;
  } else if (soc >= 100) {
    battLastLevel = 4;
    battLastPercentageForLevel = 100;
    return;
  }

  if (soc > battLastPercentageForLevel) {
    //charging
    if (soc - battLastPercentageForLevel > battPercentageLevelThreshold) {
      battLastPercentageForLevel = soc;
      if (battLastPercentageForLevel > 75) {
        battLastLevel = 4;
      } else if (battLastPercentageForLevel > 50) {
        battLastLevel = 3;
      } else if (battLastPercentageForLevel > 25) {
        battLastLevel = 2;
      } else if (battLastPercentageForLevel > 10) {
        battLastLevel = 1;
      } else {
        battLastLevel = 0;
      }
    }
  } else {
    //depleted or equal
    if (battLastPercentageForLevel - soc > battPercentageLevelThreshold) {
      battLastPercentageForLevel = soc;
      if (battLastPercentageForLevel < 10) {
        battLastLevel = 0;
      } else if (battLastPercentageForLevel < 25) {
        battLastLevel = 1;
      } else if (battLastPercentageForLevel < 50) {
        battLastLevel = 2;
      } else if (battLastPercentageForLevel < 75) {
        battLastLevel = 3;
      } else {
        battLastLevel = 4;
      }
    }
  }
}

static uint8_t getBatteryPercentage(float voltage) {
  float soc = 0;
  if (voltage > battVoltageUpperZone) {
    soc = 90.0 + (10.0 * ((voltage - battVoltageUpperZone) / (battVoltageFullRest - battVoltageUpperZone)));
    if (soc > 100) soc = 100;
  } else if (voltage > battVoltageLowerZone && voltage <= battVoltageUpperZone) {
    soc = 20.0 + (70.0 * ((voltage - battVoltageLowerZone) / (battVoltageUpperZone - battVoltageLowerZone)));
  } else if (voltage > battVoltageEmpty) {
    soc = (20.0 * ((voltage - battVoltageEmpty) / (battVoltageLowerZone - battVoltageEmpty)));
  }
  setBatteryPercentage(soc);
  return (uint8_t) soc;
}

static uint8_t getRawBatteryLevelFromPercentage(uint8_t soc) {
  if (soc <= 0) {
    return 0;
  } else if (soc >= 100) {
    return 4;
  }

  if (soc > 75) {
    return 4;
  } else if (soc > 50) {
    return 3;
  } else if (soc > 25) {
    return 2;
  } else if (soc > 5) {
    return 1;
  } else {
    return 0;
  }
  return 0;
}

static uint8_t getBatteryLevel() {
  return battLastLevel;
}

static uint8_t getBatteryLevel(float voltage) {
  getBatteryPercentage(voltage);
  return battLastLevel;
}