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

#include "Batt_Ocean_Server.h"
#include "Batt_Ocean_ServerIF.h"

#include <cstdlib>
#include <unistd.h>  // include for the sleep method

#include "bitModule/IBITIF.h"
#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

Batt_Ocean_Server::Batt_Ocean_Server( const Module* module )
    : SyncSensorComponent( Batt_Ocean_ServerIF::NAME, module ),
      battTimeout_( 45.0 ),
      initialized_( false ),
      usingSimWarned_( false ),
      uartA_( Batt_Ocean_ServerAIF::UART, Batt_Ocean_ServerAIF::BAUD, 0.010, logger_, 4095 ),
      uartB_( Batt_Ocean_ServerBIF::UART, Batt_Ocean_ServerBIF::BAUD, 0.010, logger_, 4095 ),
      capacityFailed_( false ),
      voltageFailed_( false )
{

    // Slate outputs
    // TODO: Change from deprecated Slate::NewOutputWriter to newDataWriter. Leaving as-is for now because of the adaptive way it is initializing them.
    for( int i = 0; i < ( NUM_BATTSA + NUM_BATTSB ); i++ )
    {
        battPacks_[i].battStatusWriter_    = Slate::NewOutputWriter( Batt_Ocean_ServerIF::BATT_STATUS_BASENAME + i, this, Units::ENUM() );
        battPacks_[i].battVoltageWriter_   = Slate::NewOutputWriter( Batt_Ocean_ServerIF::BATT_VOLTAGE_BASENAME + i, this, Units::VOLT() );
        battPacks_[i].battCurrentWriter_   = Slate::NewOutputWriter( Batt_Ocean_ServerIF::BATT_CURRENT_BASENAME + i, this, Units::AMPERE() );
        battPacks_[i].battTempWriter_      = Slate::NewOutputWriter( Batt_Ocean_ServerIF::BATT_TEMP_BASENAME + i, this, Units::CELSIUS() );
        battPacks_[i].battCapacityWriter_  = Slate::NewOutputWriter( Batt_Ocean_ServerIF::BATT_CAPACITY_BASENAME + i, this, Units::AMPERE_HOUR() );
    }

    // Universal outputs
    batteryChargeWriter_   = newUniversalWriter( UniversalURI::PLATFORM_BATTERY_CHARGE, Units::AMPERE_HOUR, Batt_Ocean_ServerIF::PLATFORM_BATTERY_CHARGE_ACCURACY );
    batteryVoltageWriter_  = newUniversalWriter( UniversalURI::PLATFORM_BATTERY_VOLTAGE, Units::VOLT, Batt_Ocean_ServerIF::PLATFORM_BATTERY_VOLTAGE_ACCURACY );
    // NOTE: if those accuracies are real, and they vary, use writeWithAccuracy or setAccuracy later in the code instead
    batteryDischargingWriter_ = newUniversalWriter( UniversalURI::PLATFORM_BATTERY_DISCHARGING, Units::BOOL, 0.25 );
    batteryFullyChargedWriter_ = newUniversalWriter( UniversalURI::PLATFORM_BATTERY_FULLY_CHARGED, Units::BOOL, 0.25 );

    // Configuration Readers
    batteryCapacityThresholdCfgReader_ = newConfigReader( IBITIF::BATTERY_CAPACITY_THRESHOLD ); // Amount of charge left at which we return to the surface
    batteryVoltageThresholdCfgReader_ = newConfigReader( IBITIF::BATTERY_VOLTAGE_THRESHOLD );   // Amount of voltage left at which we return to the surface

    this->setFailureMissionCritical( false );
}


Batt_Ocean_Server::~Batt_Ocean_Server()
{
}


void Batt_Ocean_Server::initialize()
{
#ifndef __LPC3180_TARGET
    return;
#endif // #ifdef __LPC3180_TARGET

    initialized_ = false;
    deviceResponseA_[0] = '\0';
    deviceResponseB_[0] = '\0';
    this->setAllowableFailures( 3 );

    ok_ = true;

    // Create and open the uart
    uartA_.open();
    uartB_.open();
    if( uartA_.hasError() )
    {
        logger_.syslog( "Error opening port: ", uartA_.errorString(), Syslog::ERROR );
    }
    else if( uartB_.hasError() )
    {
        logger_.syslog( "Error opening port: ", uartB_.errorString(), Syslog::ERROR );
    }
    else
    {
        uartA_.enableUART();
        uartB_.enableUART();

        uartA_.flush();
        uartB_.flush();
        // Send a " " "B" to start receiving battery status
        uartA_ << " ";
        uartB_ << " ";
        // Throw away the menu stuff
        uartA_.flushCRLF().setStartTimeout( 1.0 ).readUntil( deviceResponseA_, sizeof( deviceResponseA_ ), '>' );
        uartB_.flushCRLF().setStartTimeout( 1.0 ).readUntil( deviceResponseB_, sizeof( deviceResponseB_ ), '>' );


        // Wait for menu to activate
        Timespan::Seconds( 0.5 ).sleepFor();
        uartA_ << "B";
        uartB_ << "B";
        // Grab the data (and the "B" that's echoed)
        uartA_.flushCRLF().readUntil( deviceResponseA_, sizeof( deviceResponseA_ ), 'B' );
        uartA_.flushCRLF().readUntil( deviceResponseA_, sizeof( deviceResponseA_ ), '=' );
        uartA_.resetStartTimeout();
        // From the second controller card as well....
        uartB_.flushCRLF().readUntil( deviceResponseB_, sizeof( deviceResponseB_ ), 'B' );
        uartB_.flushCRLF().readUntil( deviceResponseB_, sizeof( deviceResponseB_ ), '=' );
        uartB_.resetStartTimeout();


        if( uartA_.hasError() )
        {
            deviceResponseA_[0] = '\0';
            logger_.syslog( "Batt_Ocean_Server A initialization uart error: ", uartA_.errorString(), Syslog::ERROR );
        }
        else if( uartB_.hasError() )
        {
            deviceResponseB_[0] = '\0';
            logger_.syslog( "Batt_Ocean_Server B initialization uart error: ", uartB_.errorString(), Syslog::ERROR );
        }
        else // No error
        {
            //If we have good data, then set initialized to true
            initialized_ = true;

            // Clear any failures and get rid of the rest of the line of "=" characters
            startTimeA_ = Timestamp::Now();
            startTimeB_ = Timestamp::Now();
            uartA_.flushCRLF().readLine( deviceResponseA_, sizeof( deviceResponseA_ ) );
            uartB_.flushCRLF().readLine( deviceResponseB_, sizeof( deviceResponseB_ ) );
        }
    }

    // There's been a failure
    if( !initialized_ )
    {
        logger_.syslog( Str( "Ocean Server Batteries failed to initialize. Re-initializing" ), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return;
    }
    else
    {
        logger_.syslog( Str( "Ocean Server Batteries initialized" ), Syslog::INFO );
    }

    // Get rid of anything else
    deviceResponseA_[0] = '\0';
    deviceResponseB_[0] = '\0';
    uartA_.flush();
    uartB_.flush();
}


// Load parameters
bool Batt_Ocean_Server::readConfig( void )
{
    // Check if all the parameters are read correctly
    bool ok = true;
    ok &= batteryCapacityThresholdCfgReader_->read( Units::AMPERE_HOUR, batteryCapacityThreshold_ );
    ok &= batteryVoltageThresholdCfgReader_->read( Units::VOLT, batteryVoltageThreshold_ );
    return ok;
}


void Batt_Ocean_Server::run()
{
    bool write_RdyA = false;
    bool write_RdyB = false;

    if( !readConfig() )
    {
        ok_ = false;
        // Critical error
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine. Returning.\n" ), Syslog::CRITICAL );
        return;
    }

    if( initialized_ )
    {
        // Start with the first controller card
        if( uartA_.canReadUntil( '=' ) )
        {
            // Reset our timeout if we've gotten data recently
            if( readDataA() )
            {
                startTimeA_ = Timestamp::Now();
                write_RdyA = true;
            }
        }
        // If there's not data, keep waiting until timeout expires
        else
        {
            // Re-init if we've expired
            if( startTimeA_.elapsed() > battTimeout_ )
            {
                initialized_ = false;
                logger_.syslog( Str( "Battery controller A communication timeout. Re-initializing" ), Syslog::ERROR );
            }
        }

        // Then, the second controller card
        int tmpLen;
        if( ( tmpLen = uartB_.canReadUntil( '=' ) ) )
        {
            // Reset our timeout if we've gotten data recently
            if( readDataB() )
            {
                startTimeB_ = Timestamp::Now();
                write_RdyB = true;
            }
        }
        // If there's not data, keep waiting until timeout expires
        else
        {
            // Re-init if we've expired
            if( startTimeB_.elapsed() > battTimeout_ )
            {
                initialized_ = false;
                logger_.syslog( Str( "Battery controller B communication timeout. Re-initializing" ), Syslog::ERROR );
            }
        }

        // Write the values
        if( ( write_RdyA ) && ( write_RdyB ) )
        {
            this->resetFailCount();
            float capacity = totalCapacityA_ + totalCapacityB_;
            batteryChargeWriter_->write( Units::AMPERE_HOUR, capacity );

            float voltage = ( ( ( totalVoltageA_ / NUM_BATTSA ) + ( totalVoltageB_ / NUM_BATTSB ) ) / 2 );
            batteryVoltageWriter_->write( Units::VOLT, voltage );
            // Finally, check to see that we're not running low
            checkBatt( capacity, voltage );
        }

    }
    // We're not initialized?????
    else
    {
        this->setFailure( FailureMode::COMMUNICATIONS );
        // Try a couple more times
        uninitialize();
        initialize();
    }
}

void Batt_Ocean_Server::uninitialize()
{
#ifdef __LPC3180_TARGET
    uartA_.disableUART();
    uartB_.disableUART();

    uartA_.close();
    uartB_.close();
#endif // __LPC3180_TARGET
    initialized_ = false;
}


bool Batt_Ocean_Server::readDataA()
{
    // Stores the response from the modem
    bool retVal = true;
    char* strTokPtr;

    // Grab the data
    uartA_.flushCRLF().readUntil( deviceResponseA_, sizeof( deviceResponseA_ ), '=' );

    if( uartA_.hasError() )
    {
        deviceResponseA_[0] = '\0';
        logger_.syslog( "Batt_Ocean_Server uart error: ", uartA_.errorString(), Syslog::ERROR );
        retVal = false;
    }
    else // No error
    {
        //If we have good data, then parse it
        strtok_r( deviceResponseA_, "|", &strTokPtr );
        char* response = strtok_r( NULL, "|", &strTokPtr );

        int line_num = 0;
        // Grab lines 1,3,4,
        while( response != NULL )
        {
            switch( line_num )
            {
            case 1:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%x %x %x %x %x %x %x %x", &( battPacks_[0].status_ ), &( battPacks_[1].status_ ), &( battPacks_[2].status_ ), &( battPacks_[3].status_ ), &( battPacks_[4].status_ ), &( battPacks_[5].status_ ), &( battPacks_[6].status_ ), &( battPacks_[7].status_ ) );
                break;
            case 3:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%f %f %f %f %f %f %f %f", &( battPacks_[0].voltage_ ), &( battPacks_[1].voltage_ ), &( battPacks_[2].voltage_ ), &( battPacks_[3].voltage_ ), &( battPacks_[4].voltage_ ), &( battPacks_[5].voltage_ ), &( battPacks_[6].voltage_ ), &( battPacks_[7].voltage_ ) );
                break;
            case 4:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%f %f %f %f %f %f %f %f", &( battPacks_[0].current_ ), &( battPacks_[1].current_ ), &( battPacks_[2].current_ ), &( battPacks_[3].current_ ), &( battPacks_[4].current_ ), &( battPacks_[5].current_ ), &( battPacks_[6].current_ ), &( battPacks_[7].current_ ) );
                break;
            case 5:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%f %f %f %f %f %f %f %f", &( battPacks_[0].temp_ ), &( battPacks_[1].temp_ ), &( battPacks_[2].temp_ ), &( battPacks_[3].temp_ ), &( battPacks_[4].temp_ ), &( battPacks_[5].temp_ ), &( battPacks_[6].temp_ ), &( battPacks_[7].temp_ ) );
                break;
            case 7:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%f %f %f %f %f %f %f %f", &( battPacks_[0].capacity_ ), &( battPacks_[1].capacity_ ), &( battPacks_[2].capacity_ ), &( battPacks_[3].capacity_ ), &( battPacks_[4].capacity_ ), &( battPacks_[5].capacity_ ), &( battPacks_[6].capacity_ ), &( battPacks_[7].capacity_ ) );
                break;
            default:
                break;
            }
            // Increment to the next token
            response = strtok_r( NULL, "|", &strTokPtr );
            line_num++;
        }

        // Check for enough valid lines
        if( line_num < 7 )
        {
            retVal = false;
        }
        else
        {
            // Write data to the slate
            // First the individual packs
            totalCapacityA_ = 0;
            totalVoltageA_ = 0;
            for( int i = 0; i < NUM_BATTSA; i++ )
            {
                if( battPacks_[i].voltage_ <= 10 )
                {
                    retVal = false;
                    break;
                }

                battPacks_[i].battStatusWriter_->write( Units::ENUM, battPacks_[i].status_ );
                battPacks_[i].battVoltageWriter_->write( Units::VOLT, battPacks_[i].voltage_ );
                battPacks_[i].battCurrentWriter_->write( Units::AMPERE, battPacks_[i].current_ );
                battPacks_[i].battTempWriter_->write( Units::CELSIUS, battPacks_[i].temp_ );
                battPacks_[i].battCapacityWriter_->write( Units::AMPERE_HOUR, battPacks_[i].capacity_ );

                totalCapacityA_ += ( battPacks_[i].capacity_ * 4 );
                totalVoltageA_ += ( battPacks_[i].voltage_ );

                // Check for an invalid status (Busy Bus)
                if( battPacks_[i].status_ > 0xFFF0 )
                {
                    logger_.syslog( Str( "Bus busy reported for battery bank#" + ( Str )i + Str( " STATUS: " ) + ( Str )battPacks_[i].status_ ), Syslog::INFO );
                }
                else
                {
                    // Check for other alarms (note: can be spikey)
                    if( battPacks_[i].status_ & OVER_TEMP_ALARM )
                    {
                        logger_.syslog( Str( "Over Temperature Alarm! Battery Bank #" + ( Str )i + Str( " STATUS: " ) + ( Str )battPacks_[i].status_ ), Syslog::INFO );
                    }
                    if( battPacks_[i].status_ & TERMINATE_DISCHARGE_ALARM )
                    {
                        logger_.syslog( Str( "Terminate Discharge Alarm! Battery Bank #" + ( Str )i + Str( " STATUS: " ) + ( Str )battPacks_[i].status_ ), Syslog::INFO );
                    }
                    if( ( battPacks_[i].status_ & INITIALIZED ) == 0 )
                    {
                        logger_.syslog( Str( "Not Initialized - Battery Bank #" + ( Str )i + Str( " STATUS: " ) + ( Str )battPacks_[i].status_ ), Syslog::INFO );
                    }
                }
            }

            // Check charged/discharge status
            bool dischargingBool = false;
            for( int i = 0; i < NUM_BATTSA; i++ )
            {
                if( battPacks_[i].status_ & DISCHARGING )
                {
                    dischargingBool = true;
                    break;
                }
            }
            batteryDischargingWriter_->write( Units::BOOL, dischargingBool );

            bool chargedBool = true;
            for( int i = 0; i < NUM_BATTSA; i++ )
            {
                if( ( battPacks_[i].status_ & FULLY_CHARGED ) == 0 ) // At least one battery is not fully charged
                {
                    chargedBool = false;
                    break;
                }
            }
            batteryFullyChargedWriter_->write( Units::BOOL, chargedBool );

        }
    }
    // Get rid of the last line of "=" signs and anything else
    uartA_.flushCRLF().readLine( deviceResponseA_, sizeof( deviceResponseA_ ) );
    uartA_.flush();
    return retVal;
}


bool Batt_Ocean_Server::readDataB()
{
    // Stores the response from the modem
    bool retVal = true;
    char* strTokPtr;

    // Grab the data
    uartB_.flushCRLF().readUntil( deviceResponseB_, sizeof( deviceResponseB_ ), '=' );

    if( uartB_.hasError() )
    {
        deviceResponseB_[0] = '\0';
        logger_.syslog( "Batt_Ocean_Server uart error: ", uartB_.errorString(), Syslog::ERROR );
        retVal = false;
    }
    else // No error
    {
        //If we have good data, then parse it
        strtok_r( deviceResponseB_, "|", &strTokPtr );
        char* response = strtok_r( NULL, "|", &strTokPtr );

        int line_num = 0;
        // Grab lines 1,3,4,
        while( response != NULL )
        {
            switch( line_num )
            {
            case 1:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%x %x %x %x", &( battPacks_[8].status_ ), &( battPacks_[9].status_ ), &( battPacks_[10].status_ ), &( battPacks_[11].status_ ) );
                break;
            case 3:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%f %f %f %f", &( battPacks_[8].voltage_ ), &( battPacks_[9].voltage_ ), &( battPacks_[10].voltage_ ), &( battPacks_[11].voltage_ ) );
                break;
            case 4:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%f %f %f %f", &( battPacks_[8].current_ ), &( battPacks_[9].current_ ), &( battPacks_[10].current_ ), &( battPacks_[11].current_ ) );
                break;
            case 5:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%f %f %f %f", &( battPacks_[8].temp_ ), &( battPacks_[9].temp_ ), &( battPacks_[10].temp_ ), &( battPacks_[11].temp_ ) );
                break;
            case 7:
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( response, "%f %f %f %f", &( battPacks_[8].capacity_ ), &( battPacks_[9].capacity_ ), &( battPacks_[10].capacity_ ), &( battPacks_[11].capacity_ ) );
                break;
            default:
                break;
            }
            // Increment to the next token
            response = strtok_r( NULL, "|", &strTokPtr );
            line_num++;
        }

        // Check for enough valid lines
        if( line_num < 7 )
        {
            retVal = false;
        }
        else
        {
            // Write data to the slate
            // First the individual packs
            totalCapacityB_ = 0;
            totalVoltageB_ = 0;
            for( int i = NUM_BATTSA; i < ( NUM_BATTSA + NUM_BATTSB ); i++ )
            {
                if( battPacks_[i].voltage_ <= 10 )
                {
                    retVal = false;
                    break;
                }

                battPacks_[i].battStatusWriter_->write( Units::ENUM, battPacks_[i].status_ );
                battPacks_[i].battVoltageWriter_->write( Units::VOLT, battPacks_[i].voltage_ );
                battPacks_[i].battCurrentWriter_->write( Units::AMPERE, battPacks_[i].current_ );
                battPacks_[i].battTempWriter_->write( Units::CELSIUS, battPacks_[i].temp_ );
                battPacks_[i].battCapacityWriter_->write( Units::AMPERE_HOUR, battPacks_[i].capacity_ );

                totalCapacityB_ += ( battPacks_[i].capacity_ * 3 ); // Note: secondary controller is 4 packs of 3
                totalVoltageB_ += ( battPacks_[i].voltage_ );

                // Check for an invalid status (Busy Bus)
                if( battPacks_[i].status_ > 0xFFF0 )
                {
                    logger_.syslog( Str( "Bus busy reported for battery bank#" + ( Str )i + Str( " STATUS: " ) + ( Str )battPacks_[i].status_ ), Syslog::INFO );
                }
                else
                {
                    // Check for other alarms (note: can be spikey)
                    if( battPacks_[i].status_ & OVER_TEMP_ALARM )
                    {
                        logger_.syslog( Str( "Over Temperature Alarm! Battery Bank #" + ( Str )i + Str( " STATUS: " ) + ( Str )battPacks_[i].status_ ), Syslog::INFO );
                    }
                    if( battPacks_[i].status_ & TERMINATE_DISCHARGE_ALARM )
                    {
                        logger_.syslog( Str( "Terminate Discharge Alarm! Battery Bank #" + ( Str )i + Str( " STATUS: " ) + ( Str )battPacks_[i].status_ ), Syslog::INFO );
                    }
                    if( ( battPacks_[i].status_ & INITIALIZED ) == 0 )
                    {
                        logger_.syslog( Str( "Not Initialized - Battery Bank #" + ( Str )i + Str( " STATUS: " ) + ( Str )battPacks_[i].status_ ), Syslog::INFO );
                    }
                }
            }
        }
    }
    // Get rid of the last line of "=" signs and anything else
    uartB_.flushCRLF().readLine( deviceResponseB_, sizeof( deviceResponseB_ ) );
    uartB_.flush();
    return retVal;
}


void Batt_Ocean_Server::checkBatt( const float capacity, const float voltage )
{
    if( capacityFailed_ == false && capacity <= batteryCapacityThreshold_ )
    {
        logger_.syslog( Str( "Battery Capacity Below Threshold. " + ( Str )capacity + Str( " aH" ) ), Syslog::CRITICAL );
        capacityFailed_ = true;
    }

    if( voltageFailed_ == false && voltage <= batteryVoltageThreshold_ )
    {
        logger_.syslog( Str( "Battery Voltage Below Threshold. " + ( Str )voltage + Str( " V" ) ), Syslog::CRITICAL );
        voltageFailed_ = true;
    }
}


// Initialize battery pack values
Batt_Ocean_Server::BattPack::BattPack()
    :
    status_( 0 ),
    battStatusWriter_( NULL ),
    voltage_( 0.0 ),
    battVoltageWriter_( NULL ),
    current_( 0.0 ),
    battCurrentWriter_( NULL ),
    temp_( 0.0 ),
    battTempWriter_( NULL ),
    charge_( 0.0 ),
    capacity_( 0.0 ),
    battCapacityWriter_( NULL )
{}