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

#include "MassServo.h"
#include "MassServoIF.h"

#include <cstdlib>

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

MassServo::MassServo( const Module* module )
    : EZServoServo( MassServoIF::NAME, MassServoIF::UART, MassServoIF::BAUD, module, MASS_SHIFTER ),
      loadControl_( MassServoIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startup_( INITIALIZE ),
      // indicates whether things are ok to run
      ok_( false ),
      // Actual and commanded positions
      actualMassPosition_( 0 ),
      cmdMassPosition_( 0 ),
      // Debugging outputs
      debug_( false ),
      // Timeouts
      homingTimeout_( 60 ),
      // Configuration
      totalTksCfg_( nanf( "" ) ),  // Total encoder ticks for full mass shift travel
      tksPerMMCfg_( nanf( "" ) ),  // Number of ticks for 1 mm of travel //*** DEBUG real motor is ~ 272 ticks/mm
      deviationDistCfg_( nanf( "" ) ),// Number of encoder ticks deviation allowed between expected and actual
      // Mass shift variables
      lastCmdMassPos_( nanf( "" ) ),   // Stores the previous commanded position from control
      lastActMassPos_( nanf( "" ) ),   // Stores the previous actual position of the mass shifter
      massShiftHomed_( false )
{
    // Config Readers

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

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

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

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

    // MassServo specific config readers
    totalTksCfgReader_ = newConfigReader( MassServoIF::TOTAL_TKS_CFG );          // Total encoder ticks for full mass shift travel
    tksPerMMCfgReader_ = newConfigReader( MassServoIF::TKS_PER_MM_CFG );          // Number of ticks for one rotation (1 mm of travel)
    deviationDistanceCfgReader_ = newConfigReader( MassServoIF::DEVIATION_DISTANCE ); // Deviation allowed between expected and actual
    massDeadbandCfgReader_ = newConfigReader( VerticalControlIF::MASS_DEADBAND_CFG ); //

    // Create the outputs
    massPositionWriter_ = newUniversalWriter( UniversalURI::PLATFORM_MASS_POSITION, Units::METER, 0.25 );

    // Create the inputs from Dynamic Control
    massPositionReader_ = newDataReader( VerticalControlIF::MASS_POSITION_ACTION ); //, this, Units::METER( 0.0 ) );
    massPositionReader_->setImplementor( true );

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

MassServo::~MassServo()
{}

void MassServo::run( void )
{
}


bool MassServo::readConfig()
{
    bool ok = EZServoServo::readConfig();

    ok &= totalTksCfgReader_->read( Units::COUNT, totalTksCfg_ );             // Total encoder ticks for full mass shift travel
    ok &= tksPerMMCfgReader_->read( Units::COUNT_PER_MILLIMETER, tksPerMMCfg_ );    // Number of ticks for one rotation (1 mm of travel)
    ok &= deviationDistanceCfgReader_->read( Units::METER, deviationDistCfg_ ); // Deviation allowed between expected and actual
    ok &= massDeadbandCfgReader_->read( Units::METER, massDeadbandCfg_ );
    // Make sure the deviation isn't larger than the deadband.
    if( ( deviationDistCfg_ > massDeadbandCfg_ ) && ( massDeadbandCfg_ >= 0 ) )
    {
        deviationDistCfg_ = massDeadbandCfg_;
    }
    return ok;
}

bool MassServo::waitForHoming( void )
{
    bool retVal = false;
    if( !simulateHardware() )
    {
        float response;
        response = isCommunicating( ( char * )"?8" );
        checkResponse( false ); // Make sure there are no errors
        if( !isnan( response ) && !uart_.hasError() )
        {
            // Occasionally the EZ Servo reports something _close_ to 0 when homed. This gets us within a hundredth of a mm.
            // and lets the check for ready wait the rest of the time.
            if( fabs( response ) < tksPerMMCfg_ * .01 )
            {
                retVal = true;
            }
        }
        else
        {
            logger_.syslog( "Mass Shifter error waiting for homing. Uart error: ", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }
    else
    {
        retVal = true;
    }
    return retVal;
}


void MassServo::uninitialize( void )
{
    logger_.syslog( "Uninitialize Mass 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 MassServo::getConfigURI( ConfigOption configOption ) const
{
    return configOption == CONFIG_SIMULATE_HARDWARE ? MassServoIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI;
}

// Mass 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 MassServo::initMass( void )
{
    bool retVal = true;
    if( !simulateHardware() )
    {
        // set to false and wait for run routine to verify that the shifter has centered
        massShiftHomed_ = false;
        lastActMassPos_ = 0;
        lastCmdMassPos_ = 0;
        // TBD load switching here

        // set up mass shift 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_
              << 'L' << ( int )accelCfg_
              << "N1V" << ( int )velocityCfg_ << "R\r";
    }
    return retVal;
}

// Calls EZServoServo::checkResponse for status and sets errors as applicable.
// If ezServoError_ == OVERLOAD_ERROR, calls initMass().
bool MassServo::checkResponse( bool checkForReady )
{
    bool retVal = EZServoServo::checkResponse( checkForReady );
    if( ezServoError_ == OVERLOAD_ERROR )
    {
        // If an overload has occured attempt to re-center the mass
        return STOP;
    }
    return retVal;
}



/// Do what needs to be done to run
/// Similar to initialize, in old init/run/uninit sequence
Component::RunState MassServo::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

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

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

    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 MassServo::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

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

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    float tenthTicks = .1 * tksPerMMCfg_;            // How many ticks per loop when homing
    float loops = ( ( totalTksCfg_ * .75 ) / tenthTicks ); // How many times to execute the loop.

    switch( startup_ )
    {
    case INITIALIZE:
        if( initMass() )
        {
            startTime_ = Timestamp::Now();
            startup_ = WAIT;
        }
        else
        {
            logger_.syslog( Str( "Mass failed to initialize" ), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        break;
    case WAIT:
        if( startTime_.elapsed() >= eepromWriteTimeout_ )
        {
            uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );

            if( uart_.hasError() )
            {
                logger_.syslog( "Mass shifter EEPROM initialization uart error ", uart_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::COMMUNICATIONS );
                return STOP;
            }
            startup_ = EXECUTE;
        }
        break;
    case HOME:
        break;
    case EXECUTE:
        // Now write the following program 13 to the mass shift controller ("s13 command")
        // gS03PvalueGvalue - this is the loop to home if the opto is "broken". S03 skips the "P" instruction when input 3 (opto 1) is low.
        // Zvalue is the command to home to the opto when the opto is not "broken".

        //Now execute the stored initialization program and homing
        // gS03PvalueGvalue - this is the loop to home if the opto is "broken". S03 skips the "P" instruction when input 3 (opto 1) is low.
        // Zvalue is the command to home to the opto when the opto is not "broken".
        // e0 executes the initialization program stored above in EEPROM in case it's the first time to be loaded
        uart_ << '/' << controlAddress_ << "gS03"
              << 'P' << ( int )tenthTicks
              << 'G' << ( int )loops
              << 'Z' << ( int )totalTksCfg_ << "e0R\r";

        uart_.flushCRLF().setStartTimeout( 0.25 ).readLine( uartResponse_, sizeof( uartResponse_ ) );
        uart_.resetStartTimeout();
        if( !uart_.hasError() && checkResponse( false ) )
        {
            startup_ = DONE;
        }
        else
        {
            logger_.syslog( "Mass 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 MassServo::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );

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


/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState MassServo::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 MassServo::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    startTime_ = Timestamp::Now();
    if( !simulateHardware() )
    {
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error opening port on resume: ", uart_.errorString() );
        }
    }
    return RESUMING; // State not used at this time
}


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

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

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

    // Write the last location prior to power off
    float lastKnownPosition = lastActMassPos_ * 1000.0 * tksPerMMCfg_;

    // The A0 is necessary due to firmware in EZ Servo
    uart_ << '/' << controlAddress_ << "A0z" << ( int )lastKnownPosition << "R\r";

    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
    if( !uart_.hasError() )
    {
        uart_.read( uartResponse_, 2 ); // clean up the LF and 485 turnaround char
        return RUNNABLE;
    }
    else
    {
        logger_.syslog( "Mass Shifter uart error: ", uart_.errorString(), Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    return RUNNABLE;
}


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

    readConfig();
    float motorPosCmd;

    // Processing for the mass
    // Check to see if the mass has reached it's initial homing position
    if( !( massShiftHomed_ ) && !( waitForHoming() ) )  // Short circuit waitForHoming() call with massShiftHomed_
    {
        if( startTime_.elapsed() > homingTimeout_ )
        {
            logger_.syslog( "Failed to complete homing within timeout", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }

        return RUNNABLE;
    }
    if( massPositionReader_->isActive() )
    {
        if( massPositionReader_->read( Units::METER, cmdMassPosition_ ) && !isnan( cmdMassPosition_ ) )
        {
            // Query the motor controller for position
            bool haveActualMassPosition = 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;
                }

                actualMassPosition_ = getPosition();
                massRdy_ = checkResponse( true ); // Validate the response
                if( !isnan( actualMassPosition_ ) && !uart_.hasError() )
                {
                    // Check for mass shifter to be homed to center. Init should leave it centered
                    if( !massShiftHomed_ )
                    {
                        if( massRdy_ )
                        {
                            massShiftHomed_ = true;
                        }
                        else
                        {
                            return RUNNABLE;
                        }
                    }

                    // Write value to the slate
                    if( !isnan( actualMassPosition_ ) )
                    {
                        // Ticks per mm divided into meters
                        actualMassPosition_ = ( ( actualMassPosition_ / tksPerMMCfg_ ) / 1000.000 );
                        haveActualMassPosition = true;
                    }
                    else // Here the value is null but there isn't a serial port error
                    {
                        logger_.syslog( "Mass shift reporting null position", Syslog::FAULT );
                        this->setFailure( FailureMode::HARDWARE );
                        return STOP;
                    }
                }

                // Send the controller a new command if the current position is outside of the deviation and the controller is ready
                if( ( massRdy_ ) && ( fabs( cmdMassPosition_ - actualMassPosition_ ) >= deviationDistCfg_ ) ) // TODO: member method isOutsideDeviationDistance
                {
                    //Calculate commanded mass position and send to motor controller
                    motorPosCmd = limit( ( ( cmdMassPosition_ * 1000 ) * tksPerMMCfg_ ) );

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

                    uart_ << '/' << controlAddress_ << 'A' << ( int )motorPosCmd << "R\r";

                    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
                    if( !uart_.hasError() )
                    {
                        massRdy_ = checkResponse( true );
                    }
                    else
                    {
                        logger_.syslog( "Mass Shifter uart error: ", uart_.errorString(), Syslog::FAULT );
                        this->setFailure( FailureMode::COMMUNICATIONS );
                        return STOP;
                    }
                }
                // Mass shifter is busy transiting to a new position
                else
                {
                    // Tell it to stop if the commanded position and the direction its heading are diverging
                    if( ( ( cmdMassPosition_ - lastCmdMassPos_ ) * ( actualMassPosition_ - lastActMassPos_ ) ) < 0 )
                    {
                        uart_.read( uartResponse_, 2 ); // clean up the LF and 485 turnaround char
                        uart_ << '/' << controlAddress_ << "T\r";

                        uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
                        if( !uart_.hasError() )
                        {
                            massRdy_ = checkResponse( true );
                        }
                        else
                        {
                            logger_.syslog( "Mass Shifter stop uart error: ", uart_.errorString(), Syslog::FAULT );
                            this->setFailure( FailureMode::COMMUNICATIONS );
                            return STOP;
                        }
                    }
                }
            }
            else // if( !simulateHardware() )
            {
                // We only use controller simulated values on the host
                haveActualMassPosition = SimSlate::Read( SimSlate::MASS_POSITION_METER, actualMassPosition_ );
            }
            if( haveActualMassPosition )
            {
                massPositionWriter_->write( Units::METER, actualMassPosition_ );
                // Now, set last to current
                // Note: actualMassPosition was already checked for null and would have broken out
                // so there's no need to test it here again
                lastActMassPos_ = actualMassPosition_;
            }
            lastCmdMassPos_ = cmdMassPosition_;
        }
    }

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

    return RUNNABLE;
}


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


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


bool MassServo::isNeeded()
{
    bool retVal = true;
    if( massPositionReader_->read( Units::METER, cmdMassPosition_ ) && !isnan( cmdMassPosition_ ) )
    {
        if( fabs( actualMassPosition_ - cmdMassPosition_ ) < deviationDistCfg_ ) retVal = false;
    }
    return retVal;
    // TODO: replace with... return massPositionReader_->read( Units::METER, cmdMassPosition_ ) && !isnan( cmdMassPosition_ ) && ( fabs( actualMassPosition_ - cmdMassPosition_ ) >= deviationDistCfg_ )
}