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


#include "ThrusterServo.h"
#include "ThrusterServoIF.h"

#include <cstdlib>

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

ThrusterServo::ThrusterServo( const Module* module )
    : EZServoServo( ThrusterServoIF::NAME, ThrusterServoIF::UART, ThrusterServoIF::BAUD, module, THRUSTER ),
      loadControl_( ThrusterServoIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startup_( INITIALIZE ),
      // indicates whether things are ok to run
      ok_( false ),
      // Debugging outputs
      debug_( false ),
      lastCmdPropOmega_( nanf( "" ) ), // Stores the previous command
      cmdPropOmega_( nanf( "" ) ), // Stores the commanded propeller rotational velocity
      motorPosCmd_( nanf( "" ) ), // Stores the commanded motor speed
      controllerVelocityCmd_( nanf( "" ) ), // Stores the commanded velocity
      // Allowable Number of bad velocity reads
      allowableBadVelocityCfg_( 5 ),
      // Number of bad velocity reads
      badVelocityCount_( 0 ),
      // the given motor controller is ready to receive commands
      thrustRdy_( false ),
      // Current speed is within the bounds of what was commanded
      thrustSpdGood_( false ),
      // Velocity good after the command has been issues and the thruster is run this long without fault
      velocityTimeout_( 10.0 )

{
    // Create the outputs
    propOmegaWriter_ = newUniversalWriter( UniversalURI::PLATFORM_PROPELLER_ROTATION_RATE, Units::RADIAN_PER_SECOND, 0.25 );

    // Create the inputs from Dynamic Control
    propOmegaReader_ = newDataReader( SpeedControlIF::PROP_OMEGA_ACTION );
    propOmegaReader_->setImplementor( true );

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

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

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

    // ThrusterServo-Specific Configuration
    encoderTksCfgReader_ = newConfigReader( ThrusterServoIF::ENCODER_TKS_CFG );                      // encoder tick/second multiplier
    tksPerRevCfgReader_ = newConfigReader( ThrusterServoIF::TKS_PER_REV_CFG );                       // encoder ticks/revolution as seen by the controller from HALL A&B
    deviationCfgReader_ = newConfigReader( ThrusterServoIF::DEVIATION_CFG );                         // Number of encoder ticks deviation allowed between expected and actual
    allowableBadVelocityCfgReader_ = newConfigReader( ThrusterServoIF::ALLOWABLE_BAD_VELOCITY_CFG ); // Allowable Number of bad velocity reads

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


ThrusterServo::~ThrusterServo()
{}


// Load Thruster Servo parameters
bool ThrusterServo::readConfig( void )
{
    // Check if all the parameters are read correctly
    bool ok = EZServoServo::readConfig();

    // Thruster values
    ok &= encoderTksCfgReader_->read( Units::COUNT, encoderTksCfg_ );                     // encoder tick/second multiplier
    ok &= tksPerRevCfgReader_->read( Units::COUNT_PER_REVOLUTION, tksPerRevCfg_ );        // encoder ticks/revolution as seen by the controller from HALL A&B
    ok &= deviationCfgReader_->read( Units::COUNT, deviationCfg_ );                       // Number of encoder ticks deviation allowed between expected and actual
    ok &= allowableBadVelocityCfgReader_->read( Units::COUNT, allowableBadVelocityCfg_ ); // Allowable Number of bad velocity reads


    return ok;
}


void ThrusterServo::run( void )
{
}


void ThrusterServo::uninitialize( void )
{
    logger_.syslog( "Uninitialize Thruster 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 );
        }
        startup_ = INITIALIZE;
    }
    uninitializeEnd();
}

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

// Thruster 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 ThrusterServo::initThruster( void )
{
    bool retVal( true );

    if( !simulateHardware() )
    {
        thrustRdy_ = false;

        uart_.flush();

        uart_ << '/' << controlAddress_ << "TR\r";
        uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
        if( uart_.hasError() )
        {
            logger_.syslog( "Thruster halt for initialization uart error ", uart_.errorString(), Syslog::ERROR );
            retVal = false;
        }

        uart_.flush();

        // set up thruster initialization command and store it in EEPROM (s0) so that it will execute on subsequent power-up
        uart_ << '/' << controlAddress_ << "s0"
              << 'm' << ( int )currLimitCfg_
              << 'u' << ( int )overloadTimeoutCfg_
              << "N1w" << ( int )pidWCfg_
              << 'x' << ( int )pidXCfg_
              << 'y' << ( int )pidYCfg_ << 'L' << ( int )accelCfg_ << "V0R\r"; // Note that direction is not set here intentionally so we can go either way later.

        uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
        if( uart_.hasError() )
        {
            logger_.syslog( "Thruster 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 ThrusterServo::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

    this->setAllowableFailures( 5 );
    this->setRetryTimeout( 30 );

    if( !simulateHardware() )
    {
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( Str( "Error: Thruster load controller failed to power up.\n" ), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return START;
        }

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

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


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

    badVelocityCount_ = 0;

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

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    thrustSpdGood_ = false;

    switch( startup_ )
    {
    case INITIALIZE:
        if( initThruster() )
        {
            startTime_ = Timestamp::Now();
            startup_ = WAIT;
        }
        else
        {
            logger_.syslog( Str( "Thruster 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_ ) );
        // Omitting checkResponse here since a command overflow "error" will always be reported.
        // Program e0 executes on startup putting the EZ Servo into velocity control mode (e0 is stored in EEPROM)
        // Issuing a command will simply produce an overflow error but we do it above just in case it's the first time that
        // this EZ Servo has been installed. Perhaps this can be removed?
        if( !uart_.hasError() )
        {
            startup_ = DONE;
        }
        else
        {
            logger_.syslog( "Thruster initialization uart error:", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        break;
    case VERIFY:
        break;
    case DONE:
        startup_ = INITIALIZE;
        return RUNNABLE;
        break;
    }
    return STARTING;
}


/// Pause for a short period (indicated by pauseTime)
Component::RunState ThrusterServo::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    if( !simulateHardware() )
    {
        if( !simulateHardware() && !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        uart_.close();
    }
    propOmegaWriter_->write( Units::RADIAN_PER_SECOND, 0 );
    thrustSpdGood_ = false; // Let's force a command to be sent the next time we resume.

    return PAUSED;
}


/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState ThrusterServo::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isNeeded() )
    {
        return resume();
    }
    return PAUSED;
}


Component::RunState ThrusterServo::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 ThrusterServo::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );

    if( startTime_.elapsed() < powerOnTimeoutCfg_ )
    {
        return RESUMING;
    }
    startTime_ = Timestamp::Now();
    return RUNNABLE;
}


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

    // Processing for the thruster
    if( propOmegaReader_->isActive() )
    {
        propOmegaReader_->read( Units::RADIAN_PER_SECOND, cmdPropOmega_ );
        if( !isnan( cmdPropOmega_ ) )
        {
            float actualPropOmega;
            bool haveActualPropOmega = 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;
                }

                // Calculate commanded motor velocity
                motorPosCmd_ = ( ( R2D( cmdPropOmega_ ) / 360 ) * ( tksPerRevCfg_ * encoderTksCfg_ ) );
                // Send to controller if necessary. If the controller is ready to receive a command, it
                // has stopped executing the last one. In velocity mode, that means the prop may not be
                // moving.
                if( ( !thrustSpdGood_ || thrustRdy_ ) )
                {
                    // Start the clock so we know when the last velocity command was issued
                    startTime_ = Timestamp::Now();
                    uart_ << '/' << controlAddress_ << "V" << ( int )fabs( motorPosCmd_ ) << "R\r"; // Set the speed here
                    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
                    // No need to do a checkresponse here as we would expect a "command overload" error

                    // And now set direction here
                    if( motorPosCmd_ >= 0 )
                    {
                        //Direction P0 is "forward" for the vehicle and positive for the EZ Servo
                        uart_ << '/' << controlAddress_ << "P0R\r";
                    }
                    else
                    {
                        //Direction D0 is "reverse" for the vehicle and positive for the EZ Servo
                        uart_ << '/' << controlAddress_ << "D0R\r";
                    }

                    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
                    // No need to do a checkresponse here as we would expect a "command overload" error

                    if( uart_.hasError() )
                    {
                        logger_.syslog( "Thruster uart error: ", uart_.errorString(), Syslog::ERROR );
                        this->setFailure( FailureMode::COMMUNICATIONS );
                        return STOP;
                    }
                    thrustSpdGood_ = true; // Set to true unless proven otherwise below
                    lastCmdPropOmega_ = cmdPropOmega_; // Store the last command that was sent
                }

                // Otherwise, check velocity if no velocity command is going to be sent
                else
                {
                    controllerVelocityCmd_ = getVelocityCmd();
                    // If we're under way, try once more before failing so we don't stop the prop due to a single comms error
                    if( isnan( controllerVelocityCmd_ ) )
                    {
                        ++badVelocityCount_;
                    }

                    if( !uart_.hasError() )
                    {
                        thrustRdy_ = checkResponse( true );

                        // First check for overload error and bail out if we got one
                        // EZServoServo will handle throwing the fault
                        if( ezServoError_ == OVERLOAD_ERROR )
                        {
                            return STOP;
                        }

                        // Check to see if the controller is busy executing the command.
                        // If it indicates "ready" the command is not executing
                        if( thrustRdy_ )
                        {
                            logger_.syslog( Str( "Thruster reporting ready to receive command while a command should still be executing. Uninitializing and Re-Initializing.\n" ), Syslog::FAULT );
                            this->setFailure( FailureMode::HARDWARE );
                            actualPropOmega = 0; // We'll assume the thruster isn't moving at this point
                            return STOP;
                        }

                        if( !isnan( controllerVelocityCmd_ ) )
                        {
                            actualPropOmega = D2R( controllerVelocityCmd_ / ( tksPerRevCfg_ * encoderTksCfg_ ) * 360 );
                            haveActualPropOmega = true;
                            // Check to see that we're within the deviation bounds of the commanded speed of the motor
                            // and that the command hasn't flipped to the same value with a different sign.
                            if( ( ( fabs( controllerVelocityCmd_ - fabs( motorPosCmd_ ) ) <= deviationCfg_ ) ) && ( cmdPropOmega_ == lastCmdPropOmega_ ) )
                            {
                                thrustSpdGood_ = true;
                                badVelocityCount_ = 0;
                            }
                            else
                            {
                                // Need to adjust speed next time through
                                thrustSpdGood_ = false;
                            }
                        }
                        // getVelocityCmd is returning nan so attempt to re-send the prop command
                        else
                        {
                            thrustSpdGood_ = false;
                        }
                    }
                    // There was a UART error reported
                    else
                    {
                        logger_.syslog( "Thruster uart error: ", uart_.errorString(), Syslog::FAULT );
                        this->setFailure( FailureMode::COMMUNICATIONS );
                        return STOP;
                    }
                    if( badVelocityCount_ > allowableBadVelocityCfg_ )
                    {
                        // Attempt to adjust speed next time through
                        thrustSpdGood_ = false;
                        return STOP;
                    }
                }
            }
            else
            {
                haveActualPropOmega = SimSlate::Read( SimSlate::PROPELLER_OMEGA_RADIAN_PER_SECOND, actualPropOmega );
            }

            if( haveActualPropOmega )
            {
                propOmegaWriter_->write( Units::RADIAN_PER_SECOND, actualPropOmega );

                // Reset the fail count if we're able to get the prop to turn.
                // Since velocity is just echoed back, make sure some time has passed without a failure
                if( !isnan( controllerVelocityCmd_ ) && controllerVelocityCmd_ > 0 && startTime_.elapsed() > velocityTimeout_ )
                {
                    this->resetFailCount();
                }
            }
            // If we don't have actualPropOmega, maybe the controller is turned off?
            else if( loadControl_.getPowerState() == LoadControl::OFF )
            {
                propOmegaWriter_->write( Units::RADIAN_PER_SECOND, 0 );
            }
        }
    }


    // Pause if we don't want data
    if( !isNeeded() )
    {
        return PAUSE;
    }

    return RUNNABLE;
}


Component::RunState ThrusterServo::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );
    if( !simulateHardware() )
    {
        uninitialize(); // First power down then query for faults next cycle
    }
    return STOPPING;
}


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

    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 );
        }

    }
    // Currently used to delay one more cycle to allow the EZ Servo to fully power down
    return STOPPED;
}


Component::RunState ThrusterServo::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 ThrusterServo::isDataRequested()
{
    return propOmegaWriter_->isDataRequested();
}


bool ThrusterServo::isNeeded()
{
    bool retVal = true;
    if( propOmegaReader_->isActive() && propOmegaReader_->read( Units::RADIAN_PER_SECOND, cmdPropOmega_ ) && !isnan( cmdPropOmega_ ) )
    {
        motorPosCmd_ = ( ( R2D( cmdPropOmega_ ) / 360 ) * ( tksPerRevCfg_ * encoderTksCfg_ ) ); // Calculate commanded motor velocity
        if( motorPosCmd_ == 0 ) retVal = false;
    }
    return retVal;
}