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

#include "ElevatorServo.h"
#include "ElevatorServoIF.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"


ElevatorServo::ElevatorServo( const Module* module )
    : EZServoServo( ElevatorServoIF::NAME, ElevatorServoIF::UART, ElevatorServoIF::BAUD, module, ELEVATOR ),
      loadControl_( ElevatorServoIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startup_( INITIALIZE ),
      // indicates whether things are ok to run
      ok_( false ),
      // Actual and commanded elevator angles
      cmdElevatorAngle_( 0.0 ),
      actualElevatorAngle_( 0.0 ),
      // Debugging outputs
      debug_( false )
{

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

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

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

    // Config settings shared by Elevator + Rudder:
    offsetAngleCfgReader_ = newConfigReader( ElevatorServoIF::OFFSET_ANGLE );
    countsPerDegCfgReader_ = newConfigReader( ElevatorServoIF::COUNTS_PER_DEGREE ); // motor "ticks" per degree of control surface motion
    mtrCenterCfgReader_ = newConfigReader( ElevatorServoIF::MTR_CENTER );          // 0 degrees "centered" control surface
    deviationAngleCfgReader_ = newConfigReader( ElevatorServoIF::DEVIATION_ANGLE ); // Number of degrees deviation allowed between expected and actual

    // Elevator-only Config readers
    elevatorDeadbandCfgReader_ = newConfigReader( VerticalControlIF::ELEVATOR_DEADBAND_CFG );

    // Create the outputs
    elevatorAngleWriter_ = newUniversalWriter( UniversalURI::PLATFORM_ELEVATOR_ANGLE );
    elevatorAngleWriter_->setAccuracy( Units::DEGREE, 0.25 );

    // Create the inputs from Dynamic Control
    elevatorAngleReader_ = newDataReader( VerticalControlIF::ELEVATOR_ANGLE_ACTION ); //, this, Units::RADIAN( 0.0 ) );
    elevatorAngleReader_->setImplementor( true );

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

}


ElevatorServo::~ElevatorServo()
{}


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

    // Elevator values

    ok &= elevatorDeadbandCfgReader_->read( Units::ANGULAR_DEGREE, elevDeadbandCfg_ );
    // Make sure the deviation isn't larger than the deadband.
    if( ( deviationAngleCfg_ > elevDeadbandCfg_ ) && ( elevDeadbandCfg_ >= 0 ) )
    {
        deviationAngleCfg_ = elevDeadbandCfg_;
    }

    return ok;

}

void ElevatorServo::run( void )
{
}


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

// Elevator 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 ElevatorServo::initElevator( void )
{
    bool retVal( true );
    // set up elevator initialization command and store it in EEPROM (s0) so that it will execute on subsequent power-up
    if( !simulateHardware() )
    {
        // Note: if you send "N3" after setting the X (integrator) value, it will reset it back to zero
        // w and y (P,D) are safe.
        uart_ << '/' << controlAddress_ << "s0N3"
              << 'w' << ( int )pidWCfg_
              << 'x' << ( int )pidXCfg_
              << 'y' << ( int )pidYCfg_
              << 'm' << ( int )currLimitCfg_ << "u1000R\r";

        uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
        if( uart_.hasError() )
        {
            logger_.syslog( "Elevator initialization uart error I:", 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 ElevatorServo::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );
    this->setAllowableFailures( 3 );
    if( !simulateHardware() )
    {
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( Str( "Error: Elevator load controller failed to power up.\n" ), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return START;
        }

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

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

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

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    switch( startup_ )
    {
    case INITIALIZE:
        if( initElevator() )
        {
            startTime_ = Timestamp::Now();
            startup_ = WAIT;
        }
        else
        {
            logger_.syslog( Str( "Elevator 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( "Elevator 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 ElevatorServo::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 ElevatorServo::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 ElevatorServo::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    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 ElevatorServo::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    return RUNNABLE; // State not used at this time
}


Component::RunState ElevatorServo::runnable()
{
    if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );
    readConfig();
    // Processing for the elevator
    if( elevatorAngleReader_->isActive() )
    {
        if( elevatorAngleReader_->read( Units::DEGREE, cmdElevatorAngle_ ) && !isnan( cmdElevatorAngle_ ) )
        {
            // Query the motor controller for position
            bool haveActualAngle = 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 motorPosCmd;
                actualElevatorAngle_ = getPosition();
                if( !uart_.hasError() )
                {
                    elevatorRdy_ = checkResponse( true ); // Validate the response
                    // Write value to the slate
                    if( !isnan( actualElevatorAngle_ ) )
                    {
                        actualElevatorAngle_ = ( ( actualElevatorAngle_ - mtrCenterCfg_ ) / countsPerDegCfg_ );
                        haveActualAngle = true;
                    }
                    else // Here the value is null but there isn't a serial port error
                    {
                        logger_.syslog( "Elevator reporting null position", Syslog::ERROR );
                        // For now, just reset and try again later
                        // No response will be given after a processor reset
                        uart_ << '/' << controlAddress_ << "ar5073R\r";
                        return RUNNABLE;
                    }
                }
                else
                {
                    logger_.syslog( "Elevator uart error - getPosition.", uart_.errorString(), Syslog::FAULT );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    return STOP;
                }

                if( elevatorRdy_ )
                {
                    if( fabs( ( actualElevatorAngle_ - offsetAngleCfg_ ) - cmdElevatorAngle_ ) < deviationAngleCfg_
                            && fabs( actualElevatorAngle_ ) > deviationAngleCfg_ * 4.0 )
                    {
                        this->resetFailCount();
                    }

                    //Calculate commanded elevator position to send to motor controller
                    motorPosCmd = limit( mtrCenterCfg_ + ( ( cmdElevatorAngle_ + offsetAngleCfg_ ) * countsPerDegCfg_ ) );

                    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() )
                    {
                        elevatorRdy_ = checkResponse( true );
                    }
                    else
                    {
                        logger_.syslog( "Elevator 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::ELEVATOR_ANGLE_RADIAN, actualElevatorAngle_ ) )
                {
                    haveActualAngle = true;
                    actualElevatorAngle_ = R2D( actualElevatorAngle_ );
                }
            }
            if( haveActualAngle )
            {
                // Write the position minus offset to reflect the real position of the physical elevator
                elevatorAngleWriter_->write( Units::DEGREE, ( actualElevatorAngle_ - offsetAngleCfg_ ) );
            }
        }
    }

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

    return RUNNABLE;
}


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

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


bool ElevatorServo::isNeeded()
{
    bool retVal = true;
    if( elevatorAngleReader_->isActive() )
    {
        if( elevatorAngleReader_->read( Units::DEGREE, cmdElevatorAngle_ ) && !isnan( cmdElevatorAngle_ ) )
        {
            if( fabs( ( actualElevatorAngle_ - offsetAngleCfg_ ) - cmdElevatorAngle_ ) < deviationAngleCfg_ ) // TODO: greaterThanDeviation() -- seems like all of the servo classes could use this, and possibly inherit from a superclass...
            {
                retVal = false;
            }
        }
    }
    return retVal;
}