#include "power.h"
#include "utils.h"

#if defined(HAS_AXP20X) || defined(USE_XPOWERSLIB)
bool pmu_irq = false;
#endif
bool Power::setup()
{
#ifdef HAS_AXP20X
    axp192Init();
#endif    

#ifdef USE_XPOWERSLIB
#ifdef PMU_USE_WIRE1
    TwoWire &wire = Wire1;
#else
    TwoWire &wire = Wire;
#endif
    m_power = new XPowersPMU(wire);
    m_pmuFound = m_power->init();
    if(m_pmuFound)
    {
        DEBUG_MSG("PMU Found\n");
        m_power->setChargingLedMode(XPOWERS_CHG_LED_CTRL_CHG);

        /**
         * gnss module power channel
         * The default ALDO4 is off, you need to turn on the GNSS power first, otherwise it will be invalid during
         * initialization
         */
        m_power->setPowerChannelVoltage(XPOWERS_ALDO4, 3300);
        m_power->enablePowerOutput(XPOWERS_ALDO4);

        // lora radio power channel
        m_power->setPowerChannelVoltage(XPOWERS_ALDO3, 3300);
        m_power->enablePowerOutput(XPOWERS_ALDO3);

        // m.2 interface
        m_power->setPowerChannelVoltage(XPOWERS_DCDC3, 3300);
        m_power->enablePowerOutput(XPOWERS_DCDC3);

        /**
         * ALDO2 cannot be turned off.
         * It is a necessary condition for sensor communication.
         * It must be turned on to properly access the sensor and screen
         * It is also responsible for the power supply of PCF8563
         */
        m_power->setPowerChannelVoltage(XPOWERS_ALDO2, 3300);
        m_power->enablePowerOutput(XPOWERS_ALDO2);

        // 6-axis , magnetometer ,bme280 , oled screen power channel
        m_power->setPowerChannelVoltage(XPOWERS_ALDO1, 3300);
        m_power->enablePowerOutput(XPOWERS_ALDO1);

        // sdcard power channel
        m_power->setPowerChannelVoltage(XPOWERS_BLDO1, 3300);
        m_power->enablePowerOutput(XPOWERS_BLDO1);

        // not use channel
        m_power->disablePowerOutput(XPOWERS_DCDC2); // not elicited
        m_power->disablePowerOutput(XPOWERS_DCDC5); // not elicited
        m_power->disablePowerOutput(XPOWERS_DLDO1); // Invalid power channel, it does not exist
        m_power->disablePowerOutput(XPOWERS_DLDO2); // Invalid power channel, it does not exist
        m_power->disablePowerOutput(XPOWERS_VBACKUP);

        m_power->setChargeTargetVoltage(XPOWERS_AXP2101_CHG_VOL_4V2);
        m_power->setChargerConstantCurr(XPOWERS_AXP2101_CHG_CUR_1000MA);


        m_power->disableIRQ(XPOWERS_AXP2101_ALL_IRQ);
        m_power->clearIrqStatus();

        // TBeam1.1 /T-Beam S3-Core has no external TS detection,
        // it needs to be disabled, otherwise it will cause abnormal charging
        m_power->disableTSPinMeasure();

        // PMU->enableSystemVoltageMeasure();
        m_power->enableVbusVoltageMeasure();
        m_power->enableBattVoltageMeasure();
        m_power->setPowerKeyPressOnTime(XPOWERS_POWERON_1S);
        m_power->setPowerKeyPressOffTime(XPOWERS_POWEROFF_6S);
    }
    else
    {
        DEBUG_MSG("PMU NOT Found\n");
    }
#ifdef PMU_IRQ
    uint64_t pmuIrqMask = 0;

    if (m_power->getChipModel() == XPOWERS_AXP192) {
        pmuIrqMask = XPOWERS_AXP192_VBUS_INSERT_IRQ | XPOWERS_AXP192_BAT_INSERT_IRQ | XPOWERS_AXP192_PKEY_SHORT_IRQ | XPOWERS_AXP192_PKEY_LONG_IRQ;
    } else if (m_power->getChipModel() == XPOWERS_AXP2101) {
        pmuIrqMask = XPOWERS_AXP2101_VBUS_INSERT_IRQ | XPOWERS_AXP2101_BAT_INSERT_IRQ | XPOWERS_AXP2101_PKEY_SHORT_IRQ | XPOWERS_AXP2101_PKEY_LONG_IRQ;
    }

    pinMode(PMU_IRQ, INPUT);
    attachInterrupt(
        PMU_IRQ, [] { pmu_irq = true; }, FALLING);

    // we do not look for AXPXXX_CHARGING_FINISHED_IRQ & AXPXXX_CHARGING_IRQ because it occurs repeatedly while there is
    // no battery also it could cause inadvertent waking from light sleep just because the battery filled
    // we don't look for AXPXXX_BATT_REMOVED_IRQ because it occurs repeatedly while no battery installed
    // we don't look at AXPXXX_VBUS_REMOVED_IRQ because we don't have anything hooked to vbus
    m_power->enableIRQ(pmuIrqMask);

    m_power->clearIrqStatus();
#endif
#endif
    concurrency::PeriodicTask::setup(); // We don't start our periodic task unless we actually found the device
    setPeriod(1);

    return true;
}
#ifdef BATTERY_PIN
static float read_battery() {
#ifdef ADC_CTRL
    digitalWrite(ADC_CTRL, HIGH);
    delay(2);
#endif
  uint16_t v = analogRead(BATTERY_PIN);
  //float battery_voltage =  (float)v/4095*2*3.3*1.1;
  float battery_voltage =  (float)v*BATTERY_M;
#ifdef ADC_CTRL
    digitalWrite(ADC_CTRL, LOW);
#endif
  return battery_voltage;
}
#else
static float read_battery() {
  return 0;
}
#endif

/// Reads power status to powerStatus singleton.
//
// TODO(girts): move this and other axp stuff to power.h/power.cpp.
void Power::readPowerStatus()
{
#ifdef HAS_AXP20X    
    bool hasBattery = axp.isBatteryConnect();
    int batteryVoltageMv = 0;
    uint8_t batteryChargePercent = 0;
    if (hasBattery)
    {
        batteryVoltageMv = axp.getBattVoltage();
        // If the AXP192 returns a valid battery percentage, use it
        if (axp.getBattPercentage() >= 0)
        {
            batteryChargePercent = axp.getBattPercentage();
        }
        else
        {
            // If the AXP192 returns a percentage less than 0, the feature is either not supported or there is an error
            // In that case, we compute an estimate of the charge percent based on maximum and minimum voltages defined in power.h
            batteryChargePercent = clamp((int)(((batteryVoltageMv - BAT_MILLIVOLTS_EMPTY) * 1e2) / (BAT_MILLIVOLTS_FULL - BAT_MILLIVOLTS_EMPTY)), 0, 100);
        }
    }

    // Notify any status instances that are observing us
    const PowerStatus powerStatus = PowerStatus(hasBattery, axp.isVBUSPlug(), axp.isChargeing(), batteryVoltageMv, batteryChargePercent);
    newStatus.notifyObservers(&powerStatus);

    // If we have a battery at all and it is less than 10% full, force deep sleep
    if (powerStatus.getHasBattery() && !powerStatus.getHasUSB() &&
        axp.getBattVoltage() < MIN_BAT_MILLIVOLTS)
    {
        //powerFSM.trigger(EVENT_LOW_BATTERY);
    }
#elif defined(USE_XPOWERSLIB)
    bool hasBattery = m_power->isBatteryConnect();
    int batteryVoltageMv = 0;
    uint8_t batteryChargePercent = 0;
    if (hasBattery)
    {
        batteryVoltageMv = m_power->getBattVoltage();
        // If the AXP192 returns a valid battery percentage, use it
        if (m_power->getBatteryPercent() >= 0)
        {
            batteryChargePercent = m_power->getBatteryPercent();
        }
        else
        {
            // If the AXP192 returns a percentage less than 0, the feature is either not supported or there is an error
            // In that case, we compute an estimate of the charge percent based on maximum and minimum voltages defined in power.h
            batteryChargePercent = clamp((int)(((batteryVoltageMv - BAT_MILLIVOLTS_EMPTY) * 1e2) / (BAT_MILLIVOLTS_FULL - BAT_MILLIVOLTS_EMPTY)), 0, 100);
        }
    }

    // Notify any status instances that are observing us
    const PowerStatus powerStatus = PowerStatus(hasBattery, m_power->isVbusIn(), m_power->isCharging(), batteryVoltageMv, batteryChargePercent);
    newStatus.notifyObservers(&powerStatus);

    // If we have a battery at all and it is less than 10% full, force deep sleep
    if (powerStatus.getHasBattery() && !powerStatus.getHasUSB() &&
        m_power->getBattVoltage() < MIN_BAT_MILLIVOLTS)
    {
        //powerFSM.trigger(EVENT_LOW_BATTERY);
    }
#else
    int batteryVoltageMv = read_battery()*1000;
    uint8_t batteryChargePercent = clamp((int)(((batteryVoltageMv - BAT_MILLIVOLTS_EMPTY) * 1e2) / (BAT_MILLIVOLTS_FULL - BAT_MILLIVOLTS_EMPTY)), 0, 100);
    const PowerStatus powerStatus = PowerStatus(true, false, batteryVoltageMv>4200, batteryVoltageMv, batteryChargePercent);
    newStatus.notifyObservers(&powerStatus);    
#endif    
}

void Power::doTask()
{
    readPowerStatus();

    // Only read once every 20 seconds once the power status for the app has been initialized
    if (statusHandler && statusHandler->isInitialized())
        setPeriod(1000 * 20);
}

void Power::gpsOff()
{
#ifdef USE_XPOWERSLIB
    m_power->disablePowerOutput(XPOWERS_ALDO4);
#endif    

#ifdef HAS_AXP20X    
    axp.setPowerOutPut(AXP192_LDO3, AXP202_OFF);
#endif    
}

void Power::shutdown()
{
#ifdef USE_XPOWERSLIB
    m_power->shutdown();
#endif    
#ifdef HAS_AXP20X
    axp.shutdown();
#endif    
}

void Power::gpsOn()
{
#ifdef USE_XPOWERSLIB
    m_power->enablePowerOutput(XPOWERS_ALDO4);
#endif     
#ifdef HAS_AXP20X    
    axp.setPowerOutPut(AXP192_LDO3, AXP202_ON);    
#endif    
}

#ifdef AXP192_SLAVE_ADDRESS
/**
 * Init the power manager chip
 *
 * axp192 power
    DCDC1 0.7-3.5V @ 1200mA max -> OLED // If you turn this off you'll lose comms to the axp192 because the OLED and the axp192
 share the same i2c bus, instead use ssd1306 sleep mode DCDC2 -> unused DCDC3 0.7-3.5V @ 700mA max -> ESP32 (keep this on!) LDO1
 30mA -> charges GPS backup battery // charges the tiny J13 battery by the GPS to power the GPS ram (for a couple of days), can
 not be turned off LDO2 200mA -> LORA LDO3 200mA -> GPS
 */

#ifdef HAS_AXP20X
void Power::axp192Init()
{
    if (!axp.begin(Wire, AXP192_SLAVE_ADDRESS))
    {
        DEBUG_MSG("AXP192 Begin PASS\n");

        // axp.setChgLEDMode(LED_BLINK_4HZ);
        axp.setChgLEDModeCharging();        
        DEBUG_MSG("DCDC1: %s\n", axp.isDCDC1Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("DCDC2: %s\n", axp.isDCDC2Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("LDO2: %s\n", axp.isLDO2Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("LDO3: %s\n", axp.isLDO3Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("DCDC3: %s\n", axp.isDCDC3Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("Exten: %s\n", axp.isExtenEnable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("----------------------------------------\n");
        axp.setDCDC1Voltage(3300); // for the OLED power        
        axp.setPowerOutPut(AXP192_LDO2, AXP202_ON); // LORA radio
        axp.setPowerOutPut(AXP192_LDO3, AXP202_ON);  // GPS main power
        axp.setPowerOutPut(AXP192_DCDC2, AXP202_ON);
        axp.setPowerOutPut(AXP192_EXTEN, AXP202_ON);
        axp.setPowerOutPut(AXP192_DCDC1, AXP202_ON);
        axp.setDCDC1Voltage(3300); // for the OLED power        

        DEBUG_MSG("DCDC1: %s\n", axp.isDCDC1Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("DCDC2: %s\n", axp.isDCDC2Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("LDO2: %s\n", axp.isLDO2Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("LDO3: %s\n", axp.isLDO3Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("DCDC3: %s\n", axp.isDCDC3Enable() ? "ENABLE" : "DISABLE");
        DEBUG_MSG("Exten: %s\n", axp.isExtenEnable() ? "ENABLE" : "DISABLE");

        axp.setChargingTargetVoltage(AXP202_TARGET_VOL_4_2V);
        axp.setChargeControlCur(AXP1XX_CHARGE_CUR_1320MA); // actual limit (in HW) on the tbeam is 450mA
        axp.setStartupTime(2);
        axp.setShutdownTime(0);
        axp.EnableCoulombcounter();
#if 0

      // Not connected
      //val = 0xfc;
      //axp._writeByte(AXP202_VHTF_CHGSET, 1, &val); // Set temperature protection

      //not used
      //val = 0x46;
      //axp._writeByte(AXP202_OFF_CTL, 1, &val); // enable bat detection
#endif
        axp.debugCharging();

#ifdef PMU_IRQ
        pinMode(PMU_IRQ, INPUT);
        attachInterrupt(
            PMU_IRQ, [] { pmu_irq = true; }, FALLING);

        axp.adc1Enable(AXP202_BATT_CUR_ADC1, 1);
        axp.enableIRQ(AXP202_BATT_REMOVED_IRQ | AXP202_BATT_CONNECT_IRQ | AXP202_CHARGING_FINISHED_IRQ | AXP202_CHARGING_IRQ |
                          AXP202_VBUS_REMOVED_IRQ | AXP202_VBUS_CONNECT_IRQ | AXP202_PEK_SHORTPRESS_IRQ | AXP202_PEK_LONGPRESS_IRQ,
                      1);

        axp.clearIRQ();
#endif
        readPowerStatus();
    }
    else
    {
        DEBUG_MSG("AXP192 Begin FAIL\n");
        delay(1000);
        abort();
    }
}
#endif
#endif

int  Power::loop()
{
    int result = 0;
#ifdef HAS_AXP20X
#ifdef PMU_IRQ
    if (pmu_irq)
    {
        pmu_irq = false;
        axp.readIRQ();

        DEBUG_MSG("pmu irq!\n");

        if (axp.isChargingIRQ())
        {
            DEBUG_MSG("Battery start charging\n");
        }
        if (axp.isChargingDoneIRQ())
        {
            DEBUG_MSG("Battery fully charged\n");
        }
        if (axp.isVbusRemoveIRQ())
        {
            DEBUG_MSG("USB unplugged\n");
        }
        if (axp.isVbusPlugInIRQ())
        {
            DEBUG_MSG("USB plugged In\n");
        }
        if (axp.isBattPlugInIRQ())
        {
            DEBUG_MSG("Battery inserted\n");
        }
        if (axp.isBattRemoveIRQ())
        {
            DEBUG_MSG("Battery removed\n");
        }
        if (axp.isPEKShortPressIRQ())
        {
            DEBUG_MSG("PEK short button press\n");
            result |= 1;
        }
        if (axp.isPEKLongtPressIRQ())
        {
            DEBUG_MSG("PEK long button press\n");
            result |= 2;
        }

        readPowerStatus();
        axp.clearIRQ();
    }
#endif
#else
#ifdef USE_XPOWERSLIB
#ifdef PMU_IRQ
    if (pmu_irq)
    {
        pmu_irq = false;
        m_power->getIrqStatus();

        DEBUG_MSG("pmu irq!\n");

        if (m_power->isBatChargeStartIrq())
        {
            DEBUG_MSG("Battery start charging\n");
        }
        if (m_power->isBatChargeDoneIrq())
        {
            DEBUG_MSG("Battery fully charged\n");
        }
        if (m_power->isVbusRemoveIrq())
        {
            DEBUG_MSG("USB unplugged\n");
        }
        if (m_power->isBatInsertIrq())
        {
            DEBUG_MSG("USB plugged In\n");
        }
        if (m_power->isBatInsertIrq())
        {
            DEBUG_MSG("Battery inserted\n");
        }
        if (m_power->isBatRemoveIrq())
        {
            DEBUG_MSG("Battery removed\n");
        }
        if (m_power->isPekeyShortPressIrq())
        {
            DEBUG_MSG("PEK short button press\n");
            result |= 1;
        }
        if (m_power->isPekeyLongPressIrq())
        {
            DEBUG_MSG("PEK long button press\n");
            result |= 2;
        }

        readPowerStatus();
        m_power->clearIrqStatus();
    }
#endif
#endif
#endif
    return result;
}