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

#include "LoadControl.h"
#include "io/LPC3Reg.h"
#include "utils/AuvMath.h"

static const float VOLTS_PER_COUNT = 0.02514f;
static const float MA_PER_COUNT = 3.67f;

LoadControl::LoadControl( const ConfigURI& loadControlCfg, bool useHardware, Logger& logger, Component* component, const Str& name )
    : dataState_( REQUESTVOLTAGE ),
      io_( loadControlCfg, useHardware, logger ),
      powerState_( ON ),
      initialized_( !useHardware ), // initialize hardware only when it's enabled
      requestTimeout_( 0.5 ), // measured to be a minimum of ~200 msec on bench
      errorStatus_( 0 ),
      voltageWriter_( NULL ),
      avgVoltageWriter_( NULL ),
      currentWriter_( NULL ),
      avgCurrentWriter_( NULL ),
      component_( component )
{
    registerLoadControlWriters( name );

    if( !initialized_ )
    {
        initialize();
    }
}

LoadControl::~LoadControl() {}


void LoadControl::initialize( void )
{
    // Check to see if the backplane is already powered. LCBs require it on for all functions below
    if( LPC3Reg::QueryBackplanePower() )
    {
        initialized_ = true;
        dataState_ = REQUESTVOLTAGE;
    }
    // If it's not on, request it be turned on.
    else if( LPC3Reg::PowerOnLoads() )
    {
        initialized_ = true;
        dataState_ = REQUESTVOLTAGE;
    }
    // Well, I'm gonna have to call my manager. This is now above the pay grade of load control.
    else
    {
        // No power on the backplane. CBIT will take care of it.
        initialized_ = false;
    }
}


/// Turns on the device.
bool LoadControl::powerUp()
{
    bool retVal = false;
    if( initialized_ )
    {
        errorStatus_ = 0; // The LCB clears its faults on startup so we will too
        io_.write( "1", 1 );
//        char buffer[5] = "    ";
//        Timespan::Milliseconds( 10 ).sleepFor();
//        int i = io_.read( buffer, 1 ).bytesRead();
//        if ( i == 1 )
//        {
//            //printf( "%s onGOT%d:0x%02x%02x\n", io_.getName(), i, ( int )buffer[0], ( int )buffer[1] ); //*** DEBUG
        retVal = true;
        powerState_ = POWER_UP;
//        }
//        else
//        {
//            //printf( "%s onFAIL. GOT:%s:\nBytesRead:%d\n", io_.getName(), buffer, i ); //*** DEBUG
//            retVal = false;
//        }
    }
    return retVal;

}

/// Turns off the device;
bool LoadControl::powerDown()
{
    bool retVal = false;
    if( initialized_ )
    {
        io_.write( "0", 1 );
//        char buffer[5] = "    ";
//        Timespan::Milliseconds( 10 ).sleepFor();
//        int i = io_.read( buffer, 1 ).bytesRead();
//        if ( i == 1 )
//        {
//            //printf( "%s offGOT%d:0x%02x%02x\n", io_.getName(), i, ( int )buffer[0], ( int )buffer[1] ); //*** DEBUG
        retVal = true;
        powerState_ = POWER_DOWN;
        logVoltage( 0, 0 );
        logCurrent( 0, 0 );

//        }
//        else
//        {
//            //printf( "%s offFAIL. GOT:%s:\nBytesRead:%d\n", io_.getName(), buffer, i ); //*** DEBUG
//            retVal = false;
//        }
    }
    return retVal;
}


/// Turns off and isolated the device;
bool LoadControl::isolateLoad()
{
    bool retVal = false;
    if( initialized_ )
    {
        io_.write( "i", 1 );
//        char buffer[5] = "    ";
//        Timespan::Milliseconds( 10 ).sleepFor();
//        int i = io_.read( buffer, 1 ).bytesRead();
//        if ( i == 1 )
//        {
//            //printf( "%s offGOT%d:0x%02x%02x\n", io_.getName(), i, ( int )buffer[0], ( int )buffer[1] ); //*** DEBUG
        retVal = true;
        powerState_ = POWER_DOWN;
//        }
//        else
//        {
//            //printf( "%s offFAIL. GOT:%s:\nBytesRead:%d\n", io_.getName(), buffer, i ); //*** DEBUG
//            retVal = false;
//        }
    }
    return retVal;
}

/// Turns off and de-isolates device (i.e. closes relay)
bool LoadControl::deisolateLoad()
{
    bool retVal = false;
    if( initialized_ )
    {
        io_.write( "d", 1 );
//        char buffer[5] = "    ";
//        Timespan::Milliseconds( 10 ).sleepFor();
//        int i = io_.read( buffer, 1 ).bytesRead();
//        if ( i == 1 )
//        {
//            //printf( "%s offGOT%d:0x%02x%02x\n", io_.getName(), i, ( int )buffer[0], ( int )buffer[1] ); //*** DEBUG
        retVal = true;
        powerState_ = POWER_DOWN;
//        }
//        else
//        {
//            //printf( "%s offFAIL. GOT:%s:\nBytesRead:%d\n", io_.getName(), buffer, i ); //*** DEBUG
//            retVal = false;
//        }
    }
    return retVal;
}


/// Turns on discrete;
bool LoadControl::discreteOn()
{
#ifdef __LPC3180_TARGET
    bool retVal = false;
    if( initialized_ )
    {
        io_.write( "a", 1 );
//        char buffer[5] = "    ";
//        Timespan::Milliseconds( 10 ).sleepFor();
//        if ( io_.read( buffer, 1 ).bytesRead() == 1 )
//        {
        retVal = true;
//        }
    }
    return retVal;
#else
    return true;
#endif  //__LPC3180_TARGET    
}


/// Turns off discrete;
bool LoadControl::discreteOff()
{
#ifdef __LPC3180_TARGET
    bool retVal = false;
    if( initialized_ )
    {
        io_.write( "b", 1 );
//        char buffer[5] = "    ";
//        Timespan::Milliseconds( 10 ).sleepFor();
//        if ( io_.read( buffer, 1 ).bytesRead() == 1 )
//        {
        retVal = true;
//        }
    }
    return retVal;
#else
    return true;
#endif  //__LPC3180_TARGET    
}


void LoadControl::requestVoltageAndCurrent()
{
    if( initialized_ )
    {
        switch( dataState_ )
        {
        case REQUESTVOLTAGE:
            io_.write( "v", 1 );
            startTime_ = Timestamp::Now(); // Start the clock
            dataState_ = PARSEVOLTAGE;
            break;

        case PARSEVOLTAGE:
            if( startTime_.elapsed() > requestTimeout_ )
            {
                dataState_ = REQUESTCURRENT;
                char buffer[23] = "                     ";
                unsigned int i = io_.read( buffer, 22 ).bytesRead();
                if( i == 22 )
                {
                    if( buffer[( strlen( buffer ) - 2 )] != 'v' )  // From LCB: second to last byte is command 'v'. Last byte is faults.
                    {
                        return;
                    }

                    errorStatus_ = buffer[( strlen( buffer ) - 1 )]; // Grab the status since it's here.

                    for( unsigned int i = 0; i < ( strlen( buffer ) - 1 ); i++ )
                    {
                        if( ( buffer[i] & 0x40 ) != 0x40 )
                        {
                            return;
                        }
                    }
                    float voltsNow = ( float )extractCount( buffer, 0 );
                    float voltsAvg = ( float )extractCount( buffer, 2 );

                    voltsNow = voltsNow * VOLTS_PER_COUNT;
                    voltsAvg = voltsAvg * VOLTS_PER_COUNT;

                    logVoltage( voltsNow, voltsAvg );
                }
            }
            break;

        case REQUESTCURRENT:
            io_.write( "c", 1 );
            startTime_ = Timestamp::Now(); // Start the clock
            dataState_ = PARSECURRENT;
            break;

        case PARSECURRENT:
            if( startTime_.elapsed() > requestTimeout_ )
            {
                dataState_ = REQUESTVOLTAGE;
                char buffer[25] = "                       ";
                unsigned int i = io_.read( buffer, 24 ).bytesRead();
                if( i == 24 )
                {
                    if( buffer[( strlen( buffer ) - 2 )] != 'c' )  // From LCB: second to last byte is command 'c'. Last byte is faults.
                    {
                        return;
                    }

                    errorStatus_ = buffer[( strlen( buffer ) - 1 )]; // Grab the status since it's here

                    for( unsigned int i = 0; i < ( strlen( buffer ) - 1 ); i++ )
                    {
                        if( ( buffer[i] & 0x40 ) != 0x40 )
                        {
                            return;
                        }
                    }
                    float currentNow = ( float )extractCount( buffer, 0 );
                    float currentAvg = ( float )extractCount( buffer, 2 );

                    currentNow = currentNow * MA_PER_COUNT;
                    currentAvg = currentAvg * MA_PER_COUNT;

                    logCurrent( currentNow, currentAvg );
                }
            }
            break;

        case DONE:
            dataState_ = REQUESTVOLTAGE;
            break;
        }
    }
}


// Call this to request the faults and then call readFaults > 200 ms later to read them back.
void LoadControl::requestFaults()
{
    if( initialized_ )
    {
        io_.write( "f", 1 );
    }
}


// To be read > 200 ms after request faults has been called
void LoadControl::readFaults()
{
    if( initialized_ )
    {
        char buffer[15] = "              ";
        unsigned int i = io_.read( buffer, 2 ).bytesRead(); // 13 cycles is the max unless the high bit from the LCB is set indicating end of transaction
        if( i == 1 )  // We still only expect io_.read to return 1 byte even though there may have been empty transmissions.
        {
            errorStatus_ = buffer[0];
        }
    }
}


Str LoadControl::errorString()
{
    Str retVal = "";

    if( errorStatus_ & WDT_RESET )
    {
        retVal = retVal + "LCB Watchdog Reset.";
    }

    if( errorStatus_ & INVALID_CMD )
    {
        retVal = retVal + " Invalid Command.";
    }

    if( errorStatus_ & HW_OVERCURRENT_SHUTDOWN )
    {
        retVal = retVal + " Hardware Overcurrent Shutdown.";
    }

    /* IGNORE software overcurrent faults (bit 0x08) at this time */
    //if( errorStatus_ & SW_OVERCURRENT_WARNING )
    //{
    //    retVal = retVal + " Software Overcurrent.";
    //}

    if( errorStatus_ & CL_ACTIVATED )
    {
        retVal = retVal + " Current Limiter Activated.";
    }

    return retVal;
};


// Converts 10 bits of 2 bytes to short (5 LSBs of each byte)
unsigned short LoadControl::extractCount( char * convertCount, int pos )
{
    unsigned short retVal;
    retVal = convertCount[pos] & 0x1F;
    retVal = retVal << 5; // make room for the next 5
    retVal = retVal | ( convertCount[pos + 1] & 0x1F );
    return retVal;
}

void LoadControl::registerLoadControlWriters( const Str& nameSuffix )
{
    Str name;
    Str suffix = Str::EMPTY_STR;
    if( nameSuffix != Str::EMPTY_STR )
    {
        suffix = "_" + nameSuffix;
    }

    if( NULL == voltageWriter_ )
    {
        name = "component_voltage" + suffix;
        const DataURI voltageURI( component_->getName(), name.cStr(), Units::VOLT, FLOAT2 );
        voltageWriter_ = component_->newDataWriter( voltageURI );
    }
    if( NULL == avgVoltageWriter_ )
    {
        name = "component_avgVoltage" + suffix;
        const DataURI avgVoltageURI( component_->getName(), name.cStr(), Units::VOLT, FLOAT2 );
        avgVoltageWriter_ = component_->newDataWriter( avgVoltageURI );
    }
    if( NULL == currentWriter_ )
    {
        name = "component_current" + suffix;
        const DataURI currentURI( component_->getName(), name.cStr(), Units::MILLIAMPERE, FLOAT2 );
        currentWriter_ = component_->newDataWriter( currentURI );
    }
    if( NULL == avgCurrentWriter_ )
    {
        name = "component_avgCurrent" + suffix;
        const DataURI avgCurrentURI( component_->getName(), name.cStr(), Units::MILLIAMPERE, FLOAT2 );
        avgCurrentWriter_ = component_->newDataWriter( avgCurrentURI );
    }
}

void LoadControl::logVoltage( float voltage, float averageVoltage )
{
    Timestamp dataTime = Timestamp::Now();

    avgVoltageWriter_->write( Units::VOLT, averageVoltage, dataTime );
    voltageWriter_->write( Units::VOLT, voltage, dataTime );
}

void LoadControl::logCurrent( float current, float averageCurrent )
{
    Timestamp dataTime = Timestamp::Now();

    avgCurrentWriter_->write( Units::MILLIAMPERE, averageCurrent, dataTime );
    currentWriter_->write( Units::MILLIAMPERE, current, dataTime );
}