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

#include "BuoyancyServo.h"
#include "BuoyancyServoIF.h"

#include <cstdlib>

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

BuoyancyServo::BuoyancyServo( const Module* module )
    : EZServoServo( BuoyancyServoIF::NAME, BuoyancyServoIF::UART, BuoyancyServoIF::BAUD, module, BUOYANCY ),
      loadControl_( BuoyancyServoIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startup_( INITIALIZE ),
      initialized_( false ),
      // indicates whether things are ok to run
      ok_( false ),
      cmdBuoyancyPos_( nanf( "" ) ),
      actualBuoyancyPos_( 500.0 ),
      lastCmdPos_( nanf( "" ) ),
      lastActPos_( nanf( "" ) ),
      // Timeouts for checking buoyancy position when we're "OFF"
      checkingTimeout_( 5 ),
      // Debugging outputs
      debug_( false ),
      valveOpen_( false ),
      fastPumpInit_( false ),
      checkingTimeoutCfg_( checkingTimeout_.asFloat() )
{

    // Config settings shared by all Servos:
    powerOnTimeoutCfgReader_ = newConfigReader( BuoyancyServoIF::POWER_ON_TIMEOUT_CFG ); // Time to allow system to power up before commanding
    powerOffTimeoutCfgReader_ = newConfigReader( BuoyancyServoIF::POWER_OFF_TIMEOUT_CFG ); // Time to allow system to fully power down before restarting
    currLimitCfgReader_ = newConfigReader( BuoyancyServoIF::CURR_LIMIT_CFG );            // Percent of current allowed

    // Config settings shared by all except Thruster:
    limitHiCfgReader_ = newConfigReader( BuoyancyServoIF::LIMIT_HI_CFG );        // High physical limit for motor controller
    limitLoCfgReader_ = newConfigReader( BuoyancyServoIF::LIMIT_LO_CFG );        // Low physical limit for motor controller

    // Config settings shared by all except Mass:
    pidWCfgReader_ = newConfigReader( BuoyancyServoIF::PID_W_CFG );           // Proportional gain
    pidXCfgReader_ = newConfigReader( BuoyancyServoIF::PID_X_CFG );           // Integral gain
    pidYCfgReader_ = newConfigReader( BuoyancyServoIF::PID_Y_CFG );           // Differential gain

    // Config settings shared by all except Elevator + Rudder:
    overloadTimeoutCfgReader_ = newConfigReader( BuoyancyServoIF::OVERLOAD_TIMEOUT_CFG ); // Timeout to wait before throwing overload error
    accelCfgReader_ = newConfigReader( BuoyancyServoIF::ACCEL_CFG );          // Encoder ticks / 32.768 per second squared

    // Config settings shared by Mass + Buoyancy:
    velocityCfgReader_ = newConfigReader( BuoyancyServoIF::VELOCITY_CFG );       // Encoder ticks / 32.768 per second

    // BuoyancyServo specific config readers
    countsPerCCCfgReader_ = newConfigReader( BuoyancyServoIF::COUNTS_PER_CC_CFG );        // counts on the A/D per ml of fluid pumped
    deviationVolumeCfgReader_ = newConfigReader( BuoyancyServoIF::DEVIATION_VOLUME_CFG ); // Number of ml deviation allowed between expected and actual
    checkingTimeoutCfgReader_ = newConfigReader( BuoyancyServoIF::CHECKING_TIMEOUT_CFG ); // If the buoyancy is idling, this is how often to check for actual position. If there's a leak. This is when we'll find it.
    offsetVolumeCfgReader_ = newConfigReader( BuoyancyServoIF::OFFSET_VOLUME_CFG );       // Offset of position of motor controller
    fastPumpDepthCfgReader_ = newConfigReader( BuoyancyServoIF::FAST_PUMP_DEPTH_CFG );    // Dpeth definition to inform pumping rate
    fastPumpCoefficientCfgReader_ = newConfigReader( BuoyancyServoIF::FAST_PUMP_COEFFICIENT_CFG ); // Specifies how many times faster than normal the pump should operate in fast mode

    // External configurations
    buoyancyDefaultCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_DEFAULT_CFG );
    buoyancyPumpDepthCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_PUMP_DEPTH_CFG );
    //surfaceThresholdCfgReader_ = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );
    bool ok = readConfig();

    // Universal Readers
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    // Create the outputs
    buoyancyPosWriter_ = newUniversalWriter( UniversalURI::PLATFORM_BUOYANCY_POSITION );
    buoyancyPosWriter_->setAccuracy( Units::CUBIC_CENTIMETER, 0.25 );

    // Create the inputs
    if( !ok )
    {
        buoyancyDefaultCfg_ = 500.0;
    }
    buoyancyPosReader_ = newDataReader( VerticalControlIF::BUOYANCY_ACTION ); //, this, Units::CUBIC_CENTIMETER( buoyancyDefault_ ) );
    buoyancyPosReader_->setImplementor();

    // This configures the advanced run modes.
    setRunState( START );
    persistentPause_ = true;
}


BuoyancyServo::~BuoyancyServo()
{}

// Read configuration
bool BuoyancyServo::readConfig()
{
    bool ok = EZServoServo::readConfig();

    ok &= countsPerCCCfgReader_->read( Units::CUBIC_CENTIMETER, countsPerCCCfg_ );       // counts on the A/D per ml of fluid pumped
    ok &= deviationVolumeCfgReader_->read( Units::CUBIC_CENTIMETER, deviationVolumeCfg_ ); // Number of ml deviation allowed between expected and actual
    float timeoutTemp;
    ok &= checkingTimeoutCfgReader_->read( Units::SECOND, timeoutTemp ); // If the buoyancy is idling, this is how often to check for actual position. If there's a leak. This is when we'll find it.
    if( checkingTimeoutCfg_ != timeoutTemp )
    {
        checkingTimeout_ = checkingTimeoutCfg_ = timeoutTemp;
    }
    offsetVolumeCfgReader_->read( Units::CUBIC_CENTIMETER, offsetVolumeCfg_ );

    ok &= buoyancyDefaultCfgReader_->read( Units::CUBIC_CENTIMETER, buoyancyDefaultCfg_ );
    ok &= buoyancyPumpDepthCfgReader_->read( Units::METER, buoyancyPumpDepth_ );
    ok &= fastPumpDepthCfgReader_->read( Units::METER, fastPumpDepthCfg_ );
    ok &= fastPumpCoefficientCfgReader_->read( Units::NONE, fastPumpCoeffCfg_ );
    return ok;
}


bool BuoyancyServo::abovePumpDepth()
{
    bool retVal = false;
    float depth( nanf( "" ) );
    if( depthReader_->read( Units::METER, depth ) && depth <= buoyancyPumpDepth_ )
    {
        retVal = true;
    }
    return retVal;
}


bool BuoyancyServo::aboveFastPumpDepth()
{
    bool retVal = false;
    float depth( nanf( "" ) );
    if( depthReader_->read( Units::METER, depth ) && depth <= fastPumpDepthCfg_ )
    {
        retVal = true;
    }
    return retVal;
}


void BuoyancyServo::run( void )
{
}


void BuoyancyServo::uninitialize( void )
{
    logger_.syslog( "Uninitialize Buoyancy Servo." );
    uninitializeStart();
    if( !simulateHardware() )
    {
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        uart_.close().flush();
    }
    startup_ = INITIALIZE;
    uninitializeEnd();
    initialized_ = false;
}

/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI BuoyancyServo::getConfigURI( ConfigOption configOption ) const
{
    return configOption == CONFIG_SIMULATE_HARDWARE ? BuoyancyServoIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI;
}

// Motor Servo Initialization Routines
// m - sets max current allowed during a move (100% = 5A)
// u - sets overload timeout
// N - sets mode (velocity control or potentiometer encoder)
// D or P - sets direction in velocity mode
// w, x, y - sets PID gains (respectively)
// s - stores a program in EEPROM
// e - executes a program from EEPROM
// R - executes command sequence from buffer
// T - terminates current command
bool BuoyancyServo::initBuoyancy( void )
{
    bool retVal( true );
    if( !simulateHardware() )
    {
        uart_.flush();


        lastActPos_ = 0;
        lastCmdPos_ = 0;
        valveOpen_ = false;

        // Set up fast or slow pump rate
        int velocity;
        if( fastPumpInit_ )
        {
            // High speed
            velocity = velocityCfg_ * fastPumpCoeffCfg_;
            if( debug_ ) logger_.syslog( "Initializing high speed", Syslog::INFO );
        }
        else
        {
            // Normal speed
            velocity = velocityCfg_;
            if( debug_ ) logger_.syslog( "Initializing normal speed", Syslog::INFO );
        }

        // TBD load switching here
        // set up initialization command and store it in EEPROM (s0) so that it will execute on subsequent power-up
        uart_ << '/' << controlAddress_ << "s0"       // Program string 0
              << "J0"                                       // Power drivers off
              << 'w' << ( int )pidWCfg_                // Set Servo Proportional gain
              << 'x' << ( int )pidXCfg_                // Set Servo Set Integral gain.
              << 'y' << ( int )pidYCfg_                // Set Servo Set Differential gain.
              << 'm' << ( int )currLimitCfg_           // Sets Max current allowed during a move.
              << "N3"                                       // Sets Mode: 3 = Uses Potentiometer 1 as an encoder
              << 'u' << ( int )overloadTimeoutCfg_        // Overload Timeout (Milliseconds)
              << 'V' << ( int )velocity            // In Velocity Mode (N0) Set Spin Speed Of Motor
              << 'L' << ( int )accelCfg_               // Sets Acceleration in (encoder ticks/32.768) per second squared
              << "R\r";

        uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
        if( uart_.hasError() )
        {

            logger_.syslog( "Buoyancy initialization uart error ", uart_.errorString(), Syslog::ERROR );
            retVal = false;
        }
    }
    return retVal;
}


/// Do what needs to be done to run
/// Similar to initialize, in old init/run/uninit sequence
Component::RunState BuoyancyServo::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );
    this->setAllowableFailures( 10 );
    this->setRetryTimeout( 120 );
    if( !simulateHardware() )
    {
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( Str( "Failed to power up buoyancy load controller." ), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return START;
        }

        initializeStart();
    }
    logger_.syslog( "Initializing BuoyancyServo." );

    ok_ = true;
    if( !readConfig() )
    {
        ok_ = false;
        // Critical error
        logger_.syslog( Str( "Failed to load parameters in initialization routine." ), Syslog::CRITICAL );
        return START;
    }
    startTime_ = Timestamp::Now();
    return STARTING;
}


/// Might follow a STOP...START sequence
Component::RunState BuoyancyServo::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    // Wait until PO reset has occurred
    if( startTime_.elapsed() < powerOnTimeoutCfg_ )
    {
        return STARTING;
    }

    if( simulateHardware() )
    {
        initialized_ = true;
        return RUNNABLE;
    }


    switch( startup_ )
    {
    case INITIALIZE:
        if( initBuoyancy() )
        {
            startTime_ = Timestamp::Now();
            startup_ = WAIT;
        }
        else
        {
            logger_.syslog( Str( "Buoyancy failed to initialize" ), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        break;
    case WAIT:
        if( startTime_.elapsed() >= eepromWriteTimeout_ )
        {
            startup_ = EXECUTE;
        }
        break;
    case HOME:
        break;
    case EXECUTE:
        // Now execute the stored initialization program
        uart_ << '/' << controlAddress_ << "e0R\r";

        uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
        if( !uart_.hasError() && checkResponse( false ) )
        {
            startup_ = DONE;
        }
        else
        {
            logger_.syslog( "Buoyancy initialization uart error:", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        break;
    case VERIFY:
        break;
    case DONE:
        initialized_ = true;
        startup_ = INITIALIZE;
        startTime_ = Timestamp::Now(); // Start the clock
        return RUNNABLE;
        break;
    }
    return STARTING;
}


/// Pause for a short period (indicated by pauseTime)
Component::RunState BuoyancyServo::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );

    if( !simulateHardware() )
    {
        // Command valve closed
        uart_ << '/' << controlAddress_ << "J0R\r";
    }

    startTime_ = Timestamp::Now(); // Start the clock so we know we're fully powered down with valve closed.
    return PAUSING;
}

Component::RunState BuoyancyServo::pausing()
{
    if( debug_ ) logger_.syslog( "Pausing", Syslog::INFO );
    // Wait until valve is closed
    if( startTime_.elapsed() < powerOffTimeoutCfg_ )
    {
        return PAUSING;
    }

    valveOpen_ = false;
    if( !simulateHardware() )
    {
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        uart_.close().flush();
    }
    return PAUSED;
}


/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState BuoyancyServo::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isNeeded() )
    {
        if( !simulateHardware() )
        {
            if( !loadControl_.powerUp() )
            {
                logger_.syslog( "Failed to power up", Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
                return STOP;
            }
        }
        return RESUME;
    }
    return PAUSED;
}


Component::RunState BuoyancyServo::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    if( !simulateHardware() )
    {
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error opening port on resume: ", uart_.errorString() );
        }
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }

    startTime_ = Timestamp::Now();
    return RESUMING;
}


Component::RunState BuoyancyServo::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );

    if( startTime_.elapsed() < powerOnTimeoutCfg_ )
    {
        return RESUMING;
    }

    startTime_ = Timestamp::Now();
    return RUNNABLE;
}


Component::RunState BuoyancyServo::runnable()
{
    if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );
    readConfig();

    float motorPosCmd;

    // Processing for the buoyancy system
    if( buoyancyPosReader_->isActive() )
    {
        if( buoyancyPosReader_->read( Units::CUBIC_CENTIMETER, cmdBuoyancyPos_ ) && !isnan( cmdBuoyancyPos_ ) )
        {
            // Query the motor controller for position
            bool haveActualPos = false;
            if( !simulateHardware() )
            {
                // Log voltage and current and check for any faults
                loadControl_.requestVoltageAndCurrent();
                if( loadControl_.hasError() )
                {
                    logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
                    this->setFailure( FailureMode::HARDWARE );
                    return STOP;
                }

                float position = getPosition();
                if( !uart_.hasError() )
                {
                    buoyancyRdy_ = checkResponse( true ); // Validate the response
                    // Write value to the slate
                    if( !isnan( position ) )
                    {
                        actualBuoyancyPos_ = ( ( position / countsPerCCCfg_ ) - offsetVolumeCfg_ );
                        haveActualPos = true;
                        startTime_ = Timestamp::Now(); // Restart the clock
                    }
                    else // Here the value is null but there isn't a serial port error
                    {
                        logger_.syslog( "Buoyancy engine reporting null position", Syslog::FAULT );
                        // For now, just reset and try again later
                        this->setFailure( FailureMode::HARDWARE );
                        return STOP;
                    }
                }
                else
                {
                    logger_.syslog( "Buoyancy getPosition uart error.", uart_.errorString(), Syslog::FAULT );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    return STOP;
                }

                if( buoyancyRdy_ )
                {
                    float diffBuoyancyPos = actualBuoyancyPos_ - cmdBuoyancyPos_;

                    // We've gotten where we want so reset failures
                    if( fabs( diffBuoyancyPos ) < deviationVolumeCfg_ )
                    {
                        this->resetFailCount();
                    }
                    // We're not where we are commanded so decide if we can pump at high
                    // speed and make sure we haven't already initialized that way.
                    else if( diffBuoyancyPos > 1 || ( aboveFastPumpDepth() && ( fabs( diffBuoyancyPos ) > 1 ) ) ) // In this case pump fast anytime we can.
                    {
                        if( !fastPumpInit_ )
                        {
                            fastPumpInit_ = true;
                            return STARTING;
                        }
                    }
                    else
                    {
                        if( fastPumpInit_ )
                        {
                            fastPumpInit_ = false;
                            return STARTING;
                        }
                    }

                    //Calculate commanded piston position and send to motor controller
                    motorPosCmd = limit( ( cmdBuoyancyPos_ + offsetVolumeCfg_ ) * countsPerCCCfg_ );

                    uart_.read( uartResponse_, 2 ); // clean up the LF and 485 turnaround char

                    if( !valveOpen_ )
                    {
                        // Open the solenoid, send the command, and check response
                        uart_ << '/' << controlAddress_ << "J1A" << ( int )motorPosCmd << "R\r";
                        valveOpen_ = true;
                    }
                    else
                    {
                        uart_ << '/' << controlAddress_ << "A" << ( int )motorPosCmd << "R\r";
                    }

                    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
                    if( !uart_.hasError() )
                    {
                        buoyancyRdy_ = checkResponse( true );
                    }
                    else
                    {
                        logger_.syslog( "Buoyancy uart error: ", uart_.errorString(), Syslog::FAULT );
                        this->setFailure( FailureMode::COMMUNICATIONS );
                        return STOP;
                    }
                }
                // Buoyancy engine is in the middle of a transit. Check to see if the new commanded direction
                // is the other way.
                else
                {
                    if( ( ( cmdBuoyancyPos_ - lastCmdPos_ ) * ( actualBuoyancyPos_ - lastActPos_ ) ) < 0 )
                    {
                        uart_.read( uartResponse_, 2 ); // clean up the LF and 485 turnaround char

                        uart_ << '/' << controlAddress_ << "TR\r";
                        uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
                        if( !uart_.hasError() )
                        {
                            buoyancyRdy_ = checkResponse( true );
                            if( !buoyancyRdy_ )
                            {
                                logger_.syslog( "Buoyancy stop command failure.", Syslog::FAULT );
                                this->setFailure( FailureMode::HARDWARE );
                                return STOP;
                            }
                        }
                        else
                        {
                            logger_.syslog( "Buoyancy pump stop uart error: ", uart_.errorString(), Syslog::FAULT );
                            this->setFailure( FailureMode::COMMUNICATIONS );
                            return STOP;
                        }
                    }

                }
            }
            else
            {
                // We only use controller simulated values on the host
                if( SimSlate::Read( SimSlate::BUOYANCY_POSITION_CUBIC_METER, actualBuoyancyPos_ ) )
                {
                    actualBuoyancyPos_ *= 1.0e6; // Convert m3 to cm3
                    haveActualPos = true;
                }
            }
            if( haveActualPos )
            {
                buoyancyPosWriter_->write( Units::CUBIC_CENTIMETER, actualBuoyancyPos_ );
                // Now, set last to current
                // Note: actualBuoyancyPos_ was already checked for null and would have broken out
                // so there's no need to test it here again
                lastActPos_ = actualBuoyancyPos_;
            }
            lastCmdPos_ = cmdBuoyancyPos_;
        }
    }

    // Pause if we don't want data or can't pump due to depth
    if( !isNeeded() || !abovePumpDepth() )
    {
        return PAUSE;
    }

    return RUNNABLE;
}


Component::RunState BuoyancyServo::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );

    if( !simulateHardware() )
    {
        // Command valve closed
        uart_ << '/' << controlAddress_ << "J0R\r";
    }

    startTime_ = Timestamp::Now(); // Start the clock so we know we're fully powered down with valve closed.
    return STOPPING;
}


Component::RunState BuoyancyServo::stopping()
{
    if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO );

    // Wait until valve is closed
    if( startTime_.elapsed() < powerOffTimeoutCfg_ )
    {
        return STOPPING;
    }

    valveOpen_ = false;
    if( initialized_ )
    {
        uninitialize(); // First power down then query for faults next cycle
        return STOPPING;
    }

    if( !simulateHardware() )
    {
        loadControl_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
    }
    return STOPPED;
}


Component::RunState BuoyancyServo::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );

    if( isNeeded() )
    {
        return START;
    }

    if( ( loadControl_.getPowerState() != LoadControl::OFF ) && ( loadControl_.getPowerState() != LoadControl::POWER_DOWN ) )
    {
        return stop();
    }

    // Close if the uart if it is open
    if( uart_.isReadable() )
    {
        uart_.close();
    }
    return STOPPED;
}


bool BuoyancyServo::isDataRequested()
{
    return buoyancyPosWriter_->isDataRequested();
}


bool BuoyancyServo::isNeeded()
{
    if( !abovePumpDepth() ) return false;

    if( buoyancyPosReader_->read( Units::CUBIC_CENTIMETER, cmdBuoyancyPos_ ) && !isnan( cmdBuoyancyPos_ ) )
    {
        if( fabs( actualBuoyancyPos_ - cmdBuoyancyPos_ ) < deviationVolumeCfg_ )
        {
            if( startTime_.elapsed() < checkingTimeoutCfg_ )
            {
                return false;
            }
        }
    }

    return true;
}