/** \file
 *
 *  Contains the Battery Pack Controller Battery Bank class implementation.
 *
 *  Copyright (c) 2014 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include <cstdlib>
#include <stdlib.h>     /* atoi, strtol */

#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "BPC1_BattBank.h"


/// BattBank constants
const unsigned short BattBank::MAX_STICKS( 8 );      // There can only be up to 8 sticks connected.
const unsigned short BattBank::MIN_STICKS( 7 );      // Min 7 sticks connected.
const unsigned int BattBank::HEX_REQUEST( 3 );       // bytes.
const unsigned int BattBank::MESSAGE_OFFSET( 77 );   // bytes. Length of system and controller data
const unsigned int BattBank::SHORT_IBPS_MENU( 76 );  // bytes.
const unsigned int BattBank::LONG_IBPS_MENU( 191 );  // bytes.
const unsigned int BattBank::SMBUS_STICK_MSG_LEN( 228 );  // bytes. Includes the $B1n and checksum that IBPS adds

/// Initialize battery bank class
BattBank::BattBank( const ConfigURI& uartCfg, const ConfigURI& baudCfg, Logger& logger, size_t bufferSize,
                    const double timeoutSec, const int numBattSticks, const int numReserveSticks,
                    const int stickOffset, Str bankName )
    :
    uart_( uartCfg, baudCfg, timeoutSec, logger, bufferSize ),
    logger_( logger ),
    numBattSticks_( numBattSticks ),
    numReserveSticks_( numReserveSticks ),
    stickOffset_( stickOffset ),
    name_( bankName ),
    dataError_( false ),
    bankRunState_( INITIALIZE ),
    debug_( false )
{
    // Slate outputs
    int stickID;
    StrValue stickSerial;
    BattStick battStick;

    for( int i = 0; i < numBattSticks_; i++ )
    {
        stickID = stickOffset_ + i;

        battStick = BattStick();

        Slate::ReadOnce( "Config/Battery", "stick" + Str( stickID ), stickSerial, logger_ );
        if( debug_ ) logger_.syslog( name_ + ": got serial number for stick " + Str( stickID ) + ": " + stickSerial.asString(), Syslog::INFO );

        // Store the unique ID and the serial number for future reference
        battStick.stickID_ = stickID;
        battStick.serialNumberStr_ = stickSerial.asString();
        battStick.serialNumber_ = strtoul( stickSerial.asString().cStr(), NULL, 16 );

        // Identify reserve batteries
        for( int j = 0; j < BPC1IF::NUMRESERVE; j++ )
        {
            if( stickID == BPC1IF::RESERVEBATTSID[j] )
            {
                if( debug_ ) logger_.syslog( name_ + ": marked stick " + Str( stickID ) + " as reserve.", Syslog::INFO );
                battStick.isReserveStick_ = true;
            }
        }

        battSticks_.push_back( battStick );
    }
}


BattBank::~BattBank()
{}

/// Runs the internal battery bank state machine, returns true wehn data hs been parsed
bool BattBank::runBank( void )
{
    //if( debug_ ) logger_.syslog( name_ + " data available: " + Str( uart_.dataAvailable() ) + " bytes.", Syslog::INFO ); // DEBUG
    switch( bankRunState_ )
    {
    case INITIALIZE:
        uart_.flush();
        sendIBPSBreak();
        bankRunState_ = REQUEST_DATA;
        break;

    case REQUEST_DATA:
        // Verify IBPS menu initilasation and request hex data
        if( menuInitialized() )
        {
            uart_.flush();
            if( debug_ ) logger_.syslog( name_ + ": initialized.", Syslog::INFO );

            // Request hex data
            requestHexData();
            bankRunState_ = WAIT;
        }
        break;

    case WAIT:
        // Wait for short message to clear and then queue up a IBPS break.
        // The break won't stop the datastream until after the current long message is fully published,
        // but will prevent the next message from rolling in.
        if( uart_.dataAvailable() > ( MESSAGE_OFFSET + SMBUS_STICK_MSG_LEN ) )
        {
            if( debug_ ) logger_.syslog( name_ + ": sending IBPS break.", Syslog::INFO );
            sendIBPSBreak();
            bankRunState_ = VALIDATE;
        }
        break;

    case VALIDATE:
        // Wait until we have the full message
        if( menuInitialized() )
        {
            if( debug_ ) logger_.syslog( name_ + ": validating data format.", Syslog::INFO );
            // Confirm that we have a parsable format
            msgSize_ = 0;
            if( getValidBytes( msgSize_ ) )
            {
                if( debug_ ) logger_.syslog( name_ + ": got valid message (" + Str( ( int )msgSize_ ) + " bytes).", Syslog::INFO );
                bankRunState_ = PARSE;
            }
            else
            {
                if( debug_ )
                {
                    deviceResponse_[0] = '\0';
                    uart_.read( deviceResponse_, uart_.dataAvailable() );
                    logger_.syslog( name_ + " failed to parse battery data due to unrecognized msg size.\n" + "Got:\n" + Str( deviceResponse_ ), Syslog::ERROR );
                }
                else
                {
                    logger_.syslog( name_ + " failed to parse battery data due to unrecognized msg size.", Syslog::ERROR );
                }
                bankRunState_ = INITIALIZE;
            }
        }
        break;

    case PARSE:
        // We have valid stick data - parse it and write!
        if( parseWriteData() )
        {
            if( debug_ ) logger_.syslog( name_ + ": data parsed.", Syslog::INFO );
            bankRunState_ = INITIALIZE;
            return true;
        }
        else
        {
            // logger_.syslog( name_ + " failed to parse battery data.", Syslog::ERROR );
            bankRunState_ = INITIALIZE;
            dataError_ = true;
        }
        break;

    }
    return false;
}


bool BattBank::getValidBytes( unsigned int& numBytes )
{
    unsigned int dataAvailable = 0;
    deviceResponse_[0] = '\0';

    // Check buffer size
    if( uart_.dataAvailable() > MAX_DEVICE_RESPONSE_SIZE )
    {
        logger_.syslog( name_ + " buffer size (" + Str( uart_.dataAvailable() ) + " bytes) exceeded the max device response limit.", Syslog::ERROR );
        return false;
    }

    // Strip the buffer from all lines preceding the stick data we're intrested in
    while( ( uart_.dataAvailable() > 0 ) && ( uart_.canReadUntil( "$C1" ) > 0 ) )
    {
        uart_.flushCRLF().readLine( deviceResponse_, SMBUS_STICK_MSG_LEN );
        // Check response
        if( uart_.hasError() )
        {
            logger_.syslog( name_ + " failed to read from buffer due to uart error: " + uart_.errorString(), Syslog::ERROR );
            return false;
        }
    }


    // Now remove the trailing IBPS menu. Do we have valid stick data?
    // If so, the number of bytes should be some multiplier of the SMBUS stick msg length.
    uart_.flushCRLF();
    dataAvailable = ( unsigned )uart_.dataAvailable();
    if( dataAvailable > 0 )
    {
        if( ( ( dataAvailable - SHORT_IBPS_MENU ) % SMBUS_STICK_MSG_LEN ) == 0 )
        {
            numBytes = dataAvailable - SHORT_IBPS_MENU;
            if( debug_ ) logger_.syslog( name_ + ": got " + Str( ( int )( numBytes / SMBUS_STICK_MSG_LEN ) ) + " stick msg (" + Str( ( int )numBytes ) + " bytes) with SHORT_IBPS_MENU.", Syslog::INFO );
            return true;
        }
        else if( ( ( dataAvailable - LONG_IBPS_MENU ) % SMBUS_STICK_MSG_LEN ) == 0 )
        {
            numBytes = dataAvailable - LONG_IBPS_MENU;
            if( debug_ ) logger_.syslog( name_ + ": got " + Str( ( int )( numBytes / SMBUS_STICK_MSG_LEN ) ) + " stick msg (" + Str( ( int )numBytes ) + " bytes) with LONG_IBPS_MENU.", Syslog::INFO );
            return true;
        }
        else
        {
            // No match.
            if( debug_ ) logger_.syslog( name_ + ": no match for SMBUS stick msg of size " + Str( ( int )dataAvailable ) + " bytes.", Syslog::ERROR );
            numBytes = 0;
        }
    }

    return false;
}


bool BattBank::parseWriteData( )
{
    unsigned short sticks;
    char stickSubStr[ SMBUS_STICK_MSG_LEN + 1 ];
    deviceResponse_[0] = '\0';

    // Check the number of sticks we think we have...just to be safe
    sticks = ( unsigned short )( msgSize_ ) / SMBUS_STICK_MSG_LEN;
    if( sticks > MAX_STICKS )
    {
        // Too many!
        logger_.syslog( name_ + " got IPBS message with " + Str( sticks ) + " sticks (max is " + Str( MAX_STICKS ) + ").", Syslog::ERROR );
        return false;
    }

    if( sticks < MIN_STICKS )
    {
        // Not enough... Msg is missing sticks. Continue parsing to extract data for reporting sticks.
        logger_.syslog( name_ + " got IPBS message with " + Str( sticks ) + " sticks (min is " + Str( MIN_STICKS ) + ").", Syslog::ERROR );
    }

    // And read in the data
    uart_.read( deviceResponse_, msgSize_ );
    if( uart_.hasError() )
    {
        deviceResponse_[0] = '\0';
        logger_.syslog( name_ + "Error reading " + Str( ( int )msgSize_ ) + " bytes. " + uart_.errorString(), Syslog::ERROR );
        return false;
    }

    bool parseWriteSuccess = true;
    charging_ = false;
    for( int i = 0; i < ( int )sticks; i++ )
    {
        // The beginning of each individual stick will reside at (i*SMBUS_STICK_MSG_LEN)
        int stickMsgStart = i * SMBUS_STICK_MSG_LEN;
        strncpy( stickSubStr, &deviceResponse_[ stickMsgStart ], ( size_t )SMBUS_STICK_MSG_LEN );

        if( debug_ )
        {
            // Check the OceanServer msg stick number (e.g., $B11, $B12, etc.)
            int msgStickNumber = stickSubStr[ 3 ] - '0'; // convert the char digit to an int
            if( msgStickNumber != ( i + 1 ) )
            {
                // A mismatch indicats that the msg is missing sticks.
                logger_.syslog( name_ + " expected report for stick number: " + Str( i + 1 ) + "/" + Str( sticks ) + ", but read number: " + Str( msgStickNumber ) + "." + "\n uart read:\n" + Str( deviceResponse_ ), Syslog::ERROR );
            }
        }

        // Parse stick msg
        int current;
        unsigned int temp, voltage, capacity, status, serial, cksum;
        if( 7 != sscanf( stickSubStr + 28, ",08,%X,09,%X,0A,%X,%*s $B%*d,0C,%*X,0D,%*X,0E,%*X,0F,%X,%*s $B%*d,13,%*X,14,%*X,15,%*X,16,%X,%*s $B%*d,1A,%*X,1B,%*X,1C,%X%%%X",
                         &temp, &voltage, &current, &capacity, &status, &serial, &cksum ) )
        {
            logger_.syslog( name_ + " failed to parse battery stick message.", Syslog::ERROR );
            parseWriteSuccess = false;
            break;
        }

        for( int j = 0; j < numBattSticks_; j++ )
        {
            // Find the stick that this data pertains to
            //printf( "Looking for stick %d SERIAL:%04X to match %04X\n", i, serial, battSticks_[j].serialNumber_); // *** DEBUG
            if( battSticks_[j].serialNumber_ == serial )
            {
                //printf( "Matched %d SERIAL:%04X to %d %04X\n", i, serial, j, battSticks_[j].serialNumber_); // *** DEBUG
                float tmpCurrent = ( int16_t )current * 0.001;
                // Store the data
                battSticks_[j].temp_ = temp;
                battSticks_[j].voltage_ = voltage;
                battSticks_[j].current_ = tmpCurrent;
                battSticks_[j].capacity_ = capacity;
                battSticks_[j].status_ = status;
                battSticks_[j].lastCheckInTime_ = Timestamp::Now();

                // Write the data.
                battSticks_[j].battTempWriter_->write( Units::KELVIN, ( float )temp * 0.1 ); // Yup, that's Kelvin they're outputting
                battSticks_[j].battVoltageWriter_->write( Units::MILLIVOLT, ( float )voltage );
                battSticks_[j].battCurrentWriter_->write( Units::AMPERE, tmpCurrent );
                battSticks_[j].battCapacityWriter_->write( Units::MILLIAMPERE_HOUR, ( float )capacity );
                battSticks_[j].battStatusWriter_->write( Units::NONE_USHORT, ( int )status );

                // If any stick this query cycle thinks we're charging, we're charging
                charging_ |= battSticks_[j].isCharging();

                // No sense is staying on this merry-go-round.
                break;
            }
            // Still here? No serial number match has been found.
            else if( j == numBattSticks_ - 1 )
            {
                char stickSerialStr[5];
                sprintf( stickSerialStr, "%04X", serial );
                logger_.syslog( name_ + ": No match for serial number " + stickSerialStr + " in " + name_ + "'s battery stick inventory (sticks " + Str( stickOffset_ ) + "-" + Str( stickOffset_ + numBattSticks_ - 1 ) + " in onboard configuration file).", Syslog::ERROR );
                parseWriteSuccess = false;
            }
        }
    }

    return parseWriteSuccess;
}

void BattBank::setWritersInvalid( bool invalid )
{
    // First all the stick's individual writes
    for( int i = 0; i < numBattSticks_; i++ )
    {
        battSticks_[i].battTempWriter_->setInvalid( invalid );
        battSticks_[i].battVoltageWriter_->setInvalid( invalid );
        battSticks_[i].battCurrentWriter_->setInvalid( invalid );
        battSticks_[i].battCapacityWriter_->setInvalid( invalid );
        battSticks_[i].battStatusWriter_->setInvalid( invalid );
    }
}

/// Sets the voltage for each battery stick to null as a
/// flag to indicate that recent data has not been obtained
/// for a given stick
void BattBank::setVoltagesNull()
{
    for( int i = 0; i < numBattSticks_; i++ )
    {
        battSticks_[i].voltage_ = nanf( "" );
    }
}

/// Resets the internal battery bank state
void BattBank::initializeBankState()
{
    bankRunState_ = INITIALIZE;
}


/// Enables battary bank UART
bool BattBank::enableUART()
{
    uart_.enableUART();
    uart_.open();
    if( uart_.hasError() )
    {
        logger_.syslog( name_ + " failed to open uart with error: " + uart_.errorString(), Syslog::ERROR );
        return false;
    }
    return true;
}


/// Disables battary bank UART
void BattBank::closeUART()
{
    uart_.disableUART();
    uart_.close();
}

/// Flushes battary bank UART
void BattBank::flushUART()
{
    uart_.flush();
}


/// Sends the break (space) character to the Intelligent Battery and Power System
void BattBank::sendIBPSBreak()
{
    uart_ << ' ';
}


/// Sends the X character to the Intelligent Battery and Power System
void BattBank::requestHexData()
{
    uart_ << 'X';
}


/// Returns true if both ports are initialized to the banner menu offering hex output
bool BattBank::menuInitialized()
{
    if( uart_.canReadUntil( IPBS_MENU_MSG ) )
    {
        // We got the IBPS menu, clear any data errors
        dataError_ = false;
        return true;
    }
    return false;
}