/** \file
 *
 *  Contains the Onboard class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include <stdlib.h>

#include "Onboard.h"
#include "OnboardIF.h"

#include "bitModule/CBITIF.h"
#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/SimSlate.h"
#include "data/UniversalDataWriter.h"
#include "data/Slate.h"
#include "io/LPC3Reg.h"
#include "units/Units.h"

#define AVG_VOLT_ARRAY_SIZE  20

Onboard::Onboard( const Module* module )
    : AsyncComponent( OnboardIF::NAME, module, Timespan( 5.0 ) ),
      debug_( false ),
      coeffA0_( 0.0 ),
      coeffB1_( 0.0 ),
      coeffB2_( 0.0 ),
      coeffC12_( 0.0 ),
      humidityDataRequested_( false ),
      i2cHumidity_( OnboardHumidityIF::I2C, OnboardHumidityIF::I2C_ADDR, !simulateHardware(), logger_ ),
      i2cPressure_( OnboardPressureIF::I2C, OnboardPressureIF::I2C_ADDR, !simulateHardware(), logger_ ),
      fgAvgTimeoutCycles_( 12 ) // number of sample cycles to use for current/power averaging
{
    StrValue sysNode;

    // Environmental monitoring
    pressureWriter_ = newDataWriter( OnboardIF::PRESSURE_READING );
    temperatureWriter_ = newDataWriter( OnboardIF::TEMPERATURE_READING );
    humidityWriter_ = newDataWriter( OnboardIF::HUMIDITY_READING );

    batteryVoltageWriter_ = newUniversalWriter( UniversalURI::PLATFORM_BATTERY_VOLTAGE );
    batteryVoltageWriter_->setAccuracy( Units::VOLT, OnboardBatteryIF::ONBOARD_BATTERY_VOLTAGE_ACCURACY );

    vehiclePowerWriter_ = newUniversalWriter( UniversalURI::PLATFORM_VEHICLE_POWER );
    vehiclePowerWriter_->setAccuracy( Units::WATT, OnboardBatteryIF::ONBOARD_POWER_ACCURACY );

    averageCurrentWriter_ = newUniversalWriter( UniversalURI::PLATFORM_AVERAGE_CURRENT );
    averageCurrentWriter_->setAccuracy( Units::AMPERE, OnboardBatteryIF::ONBOARD_CURRENT_ACCURACY );

    averagePowerWriter_ = newUniversalWriter( UniversalURI::PLATFORM_AVERAGE_POWER );
    averagePowerWriter_->setAccuracy( Units::WATT, OnboardBatteryIF::ONBOARD_POWER_ACCURACY );

    mainBatteryVoltageWriter_ = newDataWriter( OnboardIF::MAIN_BATTERY_VOLTAGE );
    backupBatteryVoltageWriter_ = newDataWriter( OnboardIF::BACKUP_BATTERY_VOLTAGE );
    batteryCurrentWriter_ = newDataWriter( OnboardIF::BATTERY_CURRENT );

    pressureA0CfgReader_ = newConfigReader( OnboardPressureIF::COEFA0 );
    pressureB1CfgReader_ = newConfigReader( OnboardPressureIF::COEFB1 );
    pressureB2CfgReader_ = newConfigReader( OnboardPressureIF::COEFB2 );
    pressureC12CfgReader_ = newConfigReader( OnboardPressureIF::COEFC12 );

    humidityAddrCfgReader_ = newConfigReader( OnboardHumidityIF::I2C_ADDR ); // I2C Address of sensor

    // open main battery SysFS nodes
    Slate::ReadOnce( OnboardBatteryIF::MAIN_BATTERY_SYSNODE, sysNode, logger_ );
    mainVoltageSysNode_ = openSysNode( sysNode.toString(), "voltage_now" );
    mainCurrentSysNode_ = openSysNode( sysNode.toString(), "current_now" );
    mainChargeSysNode_ = openSysNode( sysNode.toString(), "charge_now" );
    mainChargeResetSysNode_ = openSysNode( sysNode.toString(), "charge_full" );

    // open backup battery SysFS nodes
    Slate::ReadOnce( OnboardBatteryIF::BACKUP_BATTERY_SYSNODE, sysNode, logger_ );
    backupVoltageSysNode_ = openSysNode( sysNode.toString(), "voltage_now" );
    backupCurrentSysNode_ = openSysNode( sysNode.toString(), "current_now" );
    backupChargeSysNode_ = openSysNode( sysNode.toString(), "charge_now" );
    backupChargeResetSysNode_ = openSysNode( sysNode.toString(), "charge_full" );

    // calculate every how many cycles we should store the battery voltage for
    // determining average power consumption
    avgVoltageCycles_ = fgAvgTimeoutCycles_ > AVG_VOLT_ARRAY_SIZE ? fgAvgTimeoutCycles_ / AVG_VOLT_ARRAY_SIZE + 1 : 1;
}

Onboard::~Onboard()
{}


void Onboard::initialize()
{
    readConfig();
    sampleTime_ = Timestamp::Now();
}


FILE* Onboard::openSysNode( Str sysNode, Str filename )
{
    FILE *f;
    Str sysPath_ = Str( "/sys/class/power_supply/" + sysNode + "/" + filename );
    f = fopen( sysPath_.cStr(), "rb" );
    if( f == NULL )
    {
        logger_.syslog( "Can't open SysFS node file: " + sysPath_, Syslog::ERROR );
        return NULL;
    }
    return f;
}


void Onboard::run()
{


    if( !simulateHardware() )
    {
        getHumidity();
        getPressure();
        getBatteryStatus();
        sampleTime_ = Timestamp::Now();
    }
}


void Onboard::uninitialize()
{
}


bool Onboard::readConfig()
{
    bool ok( true );
    ok &= pressureA0CfgReader_->read( Units::NONE, coeffA0_ );
    ok &= pressureB1CfgReader_->read( Units::NONE, coeffB1_ );
    ok &= pressureB2CfgReader_->read( Units::NONE, coeffB2_ );
    ok &= pressureC12CfgReader_->read( Units::NONE, coeffC12_ );
    return ok;
}


/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI Onboard::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        return OnboardIF::POWER;
    case CONFIG_SIMULATE_HARDWARE:
        return OnboardIF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}


void Onboard::getHumidity( void )
{
// Read ADC and convert to humidity and temperature
    char buf[10];
    // Grab the address
    int addr = 0;
    humidityAddrCfgReader_->read( Units::COUNT, addr );
    buf[0] = addr;

    if( !humidityDataRequested_ )
    {
        i2cHumidity_.writeBytes( buf ); // request humidity
        humidityDataRequested_ = true;
    }
    else
    {
        if( i2cHumidity_.readBytes( buf, 4 ) ) // read back the 4 bytes
        {
            // Humidity is contained in the last 14 bits of the 2 bytes
            buf[0] = buf[0] & 0x3F; // Lose the first to bits
            float humidity = buf[0] << 8 | buf[1];
            humidity = humidity / ( pow( 2, 14 ) - 2 ) * 100;
            //logger_.syslog( Str( "Humidity is:" ) + humidity + "%" , Syslog::IMPORTANT ); // *** DEBUG

            // Temperature is contained in the first 14 bits of the 2 bytes
            float temperature = buf[2] << 6 | buf[3];
            temperature = temperature / ( pow( 2, 14 ) - 2 ) * 165 - 40;
            //logger_.syslog( Str( "Temperature is:" ) + temperature + "degC" , Syslog::IMPORTANT ); // *** DEBUG

            // Write the results
            humidityWriter_->write( Units::PERCENT, humidity );
            temperatureWriter_->write( Units::CELSIUS, temperature );
        }
        else
        {
            logger_.syslog( "Cannot read onboard humidity.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }

        humidityDataRequested_ = false;
    }
}


void Onboard::getPressure( void )
{

    char buf[10];
    char outVal[4] = { 0 };
    bool readOk = true;

// Request the pressure conversion
    buf[0] = 0x12; // Convert pressure and temp command
    buf[1] = 0x00;
    i2cPressure_.writeBytes( buf, 2 );

// Get the results
    // Despite what the spec claims, to get the entire 4 bytes of temp/pressure
    // one must initiate 4 reads using the commands for pressure MSB/LSB and temp
    // MSB/LSB. They are stored one by one below.

    // Read temp LSB
    buf[0] = 0x03;
    buf[1] = 0xC1; // Device address plus read bit
    i2cPressure_.writeBytes( buf, 2 );
    if( i2cPressure_.readBytes( buf, 1 ) ) // read back the 4 bytes
    {
        outVal[3] = buf[0];
    }
    else
    {
        logger_.syslog( "Can't read temperature LSB.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        readOk = false;
    }

    // Read temp MSB
    buf[0] = 0x02;
    buf[1] = 0xC1; // Device address plus read bit
    i2cPressure_.writeBytes( buf, 2 );
    if( i2cPressure_.readBytes( buf, 1 ) ) // read back the 4 bytes
    {
        outVal[2] = buf[0];
    }
    else
    {
        logger_.syslog( "Can't read temperature MSB.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        readOk = false;
    }

    // Read pressure LSB
    buf[0] = 0x01;
    buf[1] = 0xC1; // Device address plus read bit
    i2cPressure_.writeBytes( buf, 2 );
    if( i2cPressure_.readBytes( buf, 1 ) ) // read back the 4 bytes
    {
        outVal[1] = buf[0];
    }
    else
    {
        logger_.syslog( "Can't read pressure LSB.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        readOk = false;
    }

    // Read pressure MSB
    buf[0] = 0x00;
    buf[1] = 0xC1; // Device address plus read bit
    i2cPressure_.writeBytes( buf, 2 );
    if( i2cPressure_.readBytes( buf, 1 ) ) // read back the 4 bytes
    {
        outVal[0] = buf[0];
    }
    else
    {
        logger_.syslog( "Can't read pressure MSB.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        readOk = false;
    }

    if( readOk )
    {
        unsigned short pressure = outVal[0] << 8 | outVal[1]; // Combine the two pressure bytes
        unsigned short temperature = outVal[2] << 8 | outVal[3]; // Combine the two temperature bytes

        pressure = pressure >> 6; // Pressure only uses top 10 bits
        temperature = temperature >> 6; // temperature only uses top 10 bits

        if( debug_ ) logger_.syslog( Str( "Pressure ADC counts:" ) + pressure + ", Tmp ADC Counts:" + temperature, Syslog::IMPORTANT );  // *** DEBUG

        // And now for some math from section 3.5 of the doc
        float c12x2 = coeffC12_ * ( float )temperature;
        float a1 = coeffB1_ + c12x2;
        float a1x1 = a1 * ( float )pressure;
        float y1 = coeffA0_ + a1x1;
        float a2x2 = coeffB2_ * ( float )temperature;
        float pComp = y1 + a2x2;

        float finalPressure = pComp * ( ( 115 - 50 ) / 1023.0 ) + 50.0;
        pressureWriter_->write( Units::KILOPASCAL, finalPressure );
        if( debug_ ) logger_.syslog( Str( "Pressure:" ) + finalPressure + " kPa " + ( finalPressure * 0.009869 ) + " ATM", Syslog::IMPORTANT ); // *** DEBUG
    }
    else
    {

    }
}


void Onboard::getBatteryStatus( void )
{
    char buf[16] = {0};
    float mainVoltage = 0;
    float backupVoltage = 0;
    float batteryCurrent = 0;
    static long lastCharge = -1;
    long charge = 0;
    float avgCurrent;
    float avgPower = 0;
    static float avgVoltage[AVG_VOLT_ARRAY_SIZE];
    static int avgVoltageIdx = 0;
    static int avgCycles = 0;
    static float avgElapsed = 0;
    int i;

    avgElapsed += sampleTime_.elapsed().asFloat();

    if( mainVoltageSysNode_ )
    {
        rewind( mainVoltageSysNode_ );
        ( void )fread( buf, 1, 16, mainVoltageSysNode_ );
        mainVoltage = float( strtol( buf, NULL, 10 ) / 1000 ) / 1000;
        mainBatteryVoltageWriter_->write( Units::VOLT, mainVoltage );
        if( debug_ ) logger_.syslog( Str( "Main Battery Voltage:" ) + mainVoltage + " V", Syslog::IMPORTANT );
    }

    if( backupVoltageSysNode_ )
    {
        rewind( backupVoltageSysNode_ );
        ( void )fread( buf, 1, 16, backupVoltageSysNode_ );
        backupVoltage = float( strtol( buf, NULL, 10 ) / 1000 ) / 1000;
        backupBatteryVoltageWriter_->write( Units::VOLT, backupVoltage );
        if( debug_ ) logger_.syslog( Str( "Backup Battery Voltage:" ) + backupVoltage + " V", Syslog::IMPORTANT );
    }

    // The current measurements are done on the low side but battery grounds are
    // joined. So it makes no sense to report them separately, just report total
    // battery current.
    if( mainCurrentSysNode_ )
    {
        rewind( mainCurrentSysNode_ );
        ( void )fread( buf, 1, 16, mainCurrentSysNode_ );
        batteryCurrent += float( strtol( buf, NULL, 10 ) ) / 1000;
    }

    if( backupCurrentSysNode_ )
    {
        rewind( backupCurrentSysNode_ );
        ( void )fread( buf, 1, 16, backupCurrentSysNode_ );
        batteryCurrent += float( strtol( buf, NULL, 10 ) ) / 1000;
    }

    if( batteryCurrent )
    {
        batteryCurrentWriter_->write( Units::MILLIAMPERE, batteryCurrent );
        if( debug_ ) logger_.syslog( Str( "Battery Current:" ) + batteryCurrent + " mA", Syslog::IMPORTANT );
    }

    // For the universal we report main battery voltage if we are running on
    // main batteries and backup battery voltage if we are on backup.
    // If we are running on shore power (and/or we are charging), report main
    // battery.
    if( LPC3Reg::QueryBattPower() || LPC3Reg::QueryShorepower() )
    {
        if( mainVoltage )
        {
            batteryVoltageWriter_->write( Units::VOLT, mainVoltage );
            if( batteryCurrent < 0 ) vehiclePowerWriter_->write( Units::WATT, batteryCurrent / -1000 * mainVoltage );
            if( !( avgCycles % avgVoltageCycles_ ) ) avgVoltage[ avgVoltageIdx++ % AVG_VOLT_ARRAY_SIZE ] = mainVoltage;
            if( debug_ ) logger_.syslog( "using main battery for universal", Syslog::IMPORTANT );
        }
    }
    else
    {
        if( backupVoltage )
        {
            batteryVoltageWriter_->write( Units::VOLT, backupVoltage );
            if( batteryCurrent < 0 ) vehiclePowerWriter_->write( Units::WATT, batteryCurrent / -1000 * backupVoltage );
            if( !( avgCycles % avgVoltageCycles_ ) ) avgVoltage[avgVoltageIdx++ % AVG_VOLT_ARRAY_SIZE] = backupVoltage;
            if( debug_ ) logger_.syslog( "using backup battery for universal", Syslog::IMPORTANT );
        }
    }

    /*
     * Determine average current and power.
     * This is done with the fuel gauge's current accumulation feature. We
     * measure the charge in uAh and wait for fgAvgTimeoutCycles_ * fgTimeout_
     * seconds to read it again and determine the difference. This is a
     * fairly exact measurement of the average current over that period.
     * Note that the fuel gauge will not accumulate charge currents below
     * a few mA so if the vehicle is on shore power the actual current will
     * show a few mA going into the battery while the average current will
     * stay at 0.
     */
    ++avgCycles %= fgAvgTimeoutCycles_;
    if( !avgCycles )
    {
        if( mainChargeSysNode_ )
        {
            rewind( mainChargeSysNode_ );
            ( void )fread( buf, 1, 16, mainChargeSysNode_ );
            charge += strtol( buf, NULL, 10 );
        }

        if( backupChargeSysNode_ )
        {
            rewind( backupChargeSysNode_ );
            ( void )fread( buf, 1, 16, backupChargeSysNode_ );
            charge += strtol( buf, NULL, 10 );
        }

        // the first time around we have no reference so wait for the next round
        if( lastCharge > 0 )
        {
            avgCurrent = ( float )( lastCharge - charge ) * 3600 / avgElapsed;
            avgCurrent /= 1000;
            if( avgCurrent < 0 ) avgCurrent = 0;

            averageCurrentWriter_->write( Units::MILLIAMPERE, avgCurrent );
            if( debug_ ) logger_.syslog( Str( "Average Battery Current:" ) + avgCurrent + " mA", Syslog::IMPORTANT );

            avgVoltageIdx--;
            for( i = 0; i < avgVoltageIdx; i++ )
            {
                avgPower += avgVoltage[i];
            }
            avgPower = avgPower / avgVoltageIdx * avgCurrent / 1000;

            averagePowerWriter_->write( Units::WATT, avgPower );
            if( debug_ ) logger_.syslog( Str( "Average Battery Power:" ) + avgPower + " W", Syslog::IMPORTANT );

        }
        avgVoltageIdx = 0;

        // if fuel gauge gets below 10Ah, reset the charge level to 100Ah
        if( charge < 10000000 && mainChargeResetSysNode_ && backupChargeResetSysNode_ )
        {
            // read charge_full node to reset charge counter
            rewind( mainChargeResetSysNode_ );
            ( void )fread( buf, 1, 16, mainChargeResetSysNode_ );
            lastCharge = float( strtol( buf, NULL, 10 ) );

            rewind( backupChargeResetSysNode_ );
            ( void )fread( buf, 1, 16, backupChargeResetSysNode_ );
            lastCharge += float( strtol( buf, NULL, 10 ) );
        }
        else
        {
            lastCharge = charge;
        }

        avgElapsed = 0;
    }
}