#include "battery.h"
#include "magic_constants.h"
#include <wiringPi.h>
#include <wiringPiI2C.h>

#include <maps/libs/log8/include/log8.h>

#include <string>
#include <iostream>
#include <chrono>
#include <thread>
#include <sstream>

namespace maps::mrc::drive {

using namespace std::chrono;

PowerManagerException::PowerManagerException(const char *message)
    : message_(message)
{
}

const char *PowerManagerException::what() const noexcept
{
    return message_.c_str();
}

Battery::Battery(int channel,
                 int address,
                 BatteryType battery,
                 bool doEnableBattery)
    : isBatteryOn_(false),
      batteryId_(battery),
      batteryLiveStatus_(BatteryLiveStatus::Alive),
      isCharging_(false)
{
    wiringPiSetup();
    std::string wiringPiInterfaceStr = "/dev/i2c-" + std::to_string(channel);
    wiringPiDesc_ = wiringPiI2CSetupInterface(
        wiringPiInterfaceStr.c_str(),
        address
    );

    if (checkHardware()) {
        setup(doEnableBattery);
        enableMonitoring();
    } else {
        throw PowerManagerException("PM: No charger detected");
    }
}

bool Battery::checkHardware() const
{
    return wiringPiI2CReadReg16(wiringPiDesc_, i2c_addr::HARDWARE_CHECK) == 0x8840;
}

double Battery::batteryVoltage() const
{
    return wiringPiI2CReadReg8(wiringPiDesc_, i2c_addr::BAT_VOLTAGE) / 10.;
}

double Battery::inputVoltage() const
{
    return wiringPiI2CReadReg8(wiringPiDesc_, i2c_addr::INPUT_VOLTAGE) / 10.;
}

bool Battery::isChargerConnected() const
{
    return (wiringPiI2CReadReg8(wiringPiDesc_, i2c_addr::CHARGER_STATUS) & 0x80) == 0x80;
}

double Battery::batteryDischargeCurrent() const
{
    // [ p.66 bq25713.pdf ]
    return wiringPiI2CReadReg8(wiringPiDesc_, i2c_addr::BAT_DISCHARGE_CURRENT) * 0.256;
}

double Battery::batteryChargeCurrent() const
{
    // [ p.66 bq25713.pdf ]
    return wiringPiI2CReadReg8(wiringPiDesc_, i2c_addr::BAT_CHARGE_CURRENT) * 0.064;
}

void Battery::startCharging()
{
    // 0.5A or 0.4A
    int current = (batteryId_ == BatteryType::LIFEPO4) ? 0x0140 : 0x0180;

    // 7.2v or 8.4v
    int voltage = batteryVoltageToSet();

    wiringPiI2CWriteReg16(wiringPiDesc_, i2c_addr::CHARGE_CURRENT, current);
    wiringPiI2CWriteReg16(wiringPiDesc_, i2c_addr::CHARGE_VOLTAGE, voltage);

    // enable charging
    wiringPiI2CWriteReg16(wiringPiDesc_, i2c_addr::BAT_BASE, 0x070E);

    isCharging_ = true;
}

void Battery::stopCharging()
{
    wiringPiI2CWriteReg16(wiringPiDesc_, i2c_addr::BAT_BASE, 0x070F);
    isCharging_ = false;
}

int Battery::batteryVoltageToSet() const
{
    return (batteryId_ == BatteryType::LIFEPO4) ? 0x1c20 : 0x20d0;
}

void Battery::setChargingVoltage()
{
    wiringPiI2CWriteReg16(wiringPiDesc_, i2c_addr::CHARGE_VOLTAGE, batteryVoltageToSet());
}

void Battery::enableMonitoring()
{
    // [ p.48 bq25713.pdf ]
    wiringPiI2CWriteReg8(wiringPiDesc_, i2c_addr::ADC_MSB, 0xC0);
    wiringPiI2CWriteReg8(wiringPiDesc_, i2c_addr::ADC_LSB, 0xFF);
}

void Battery::enableBattery()
{
    if (batteryLiveStatus_ == BatteryLiveStatus::Dead) {
        isBatteryOn_ = false;
    } else {
        pinMode(4, OUTPUT);
        digitalWrite(4, HIGH);
        isBatteryOn_ = true;
    }
}

void Battery::disableBattery()
{
    pinMode(4, OUTPUT);
    digitalWrite(4, LOW);
    isBatteryOn_ = false;
}

bool Battery::isBatteryOn() const
{
    return isBatteryOn_;
}

void Battery::setup(bool doEnableBattery)
{
    pinMode(4, OUTPUT);
    pinMode(2, OUTPUT);
    stopCharging();
    std::this_thread::sleep_for(seconds(2));

    if (batteryVoltage() < 2) {
        WARN() << "PM: No Battery detected";
        digitalWrite(4, LOW);
    } else {
        digitalWrite(4, HIGH);
    }

    // min 5v
    wiringPiI2CWriteReg8(wiringPiDesc_, i2c_addr::MIN_VOLTAGE, 0x14);
    digitalWrite(2, HIGH);
    if (doEnableBattery)
        enableBattery();
    else
        disableBattery();
}

void Battery::updateBatteryLiveStatus()
{
    batteryLiveStatus_ = (batteryVoltage() > 2.) ? BatteryLiveStatus::Alive
                                                 : BatteryLiveStatus::Dead;
}

BatteryType Battery::detectBatteryType() const
{
    return (batteryVoltage() >= 7.)
           ? BatteryType::LIION
           : BatteryType::LIFEPO4;
}

bool Battery::needCharging() const
{
    if (batteryLiveStatus_ == BatteryLiveStatus::Dead)
        return false;

    double v = batteryVoltage();

    if (batteryId_ == BatteryType::LIFEPO4 && v < 6.4)
        return true;
    return batteryId_ == BatteryType::LIION && v < 7.4;
}

bool Battery::isCharging() const
{
    return isCharging_;
}

BatteryType Battery::batteryType() const
{
    return batteryId_;
}

bool Battery::isBatteryAlive() const
{
    return batteryLiveStatus_ == BatteryLiveStatus::Alive;
}

} // namespace maps::mrc::drive
