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

#include "DockingServo.h"
#include "DockingServoIF.h"

#include <cstdlib>

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

DockingServo::DockingServo( const Module* module )
    : EZServoServo( DockingServoIF::NAME, DockingServoIF::UART, DockingServoIF::BAUD, module, DOCKING ),
      loadControl_( DockingServoIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startup_( INITIALIZE ),
      // indicates whether things are ok to run
      ok_( false ),
      // Actual and commanded angles
      actualArmAngle_( nanf( "" ) ),
      haveActualAngle_( false ),
      cmdArmAngle_( -15 ), // Default to closed
      openAngle_( 15 ),
      closedAngle_( -15 ),
      // Debugging outputs
      debug_( false ),
      // the given motor controller is ready to receive commands
      armRdy_( false ),
      lastMode_( 0 ),              // Store the last known commanded mode. Init to "UNKNOWN"
      modeChanged_( false ),       // Lets us know if the mode has changed this cycle to allow command change mid transit
      armOpen_( false ),           // True means ready to dock
      cablePresent_( false ),      // Reads the IR sensor
      cableStatus_( 10000 ),       // Init to no cable present
      mode_( DockIF::STANDBY ),    // We'll start in stanbdy mode
      modeCmd_( DockIF::STANDBY )  // This is the commanded mode. We'll start in stanbdy mode unless we hear otherwise.
{
    /// Config settings shared by all Servos:
    powerOnTimeoutCfgReader_ = newConfigReader( DockingServoIF::POWER_ON_TIMEOUT_CFG ); // Time to allow system to power up before commanding
    currLimitCfgReader_ = newConfigReader( DockingServoIF::CURR_LIMIT_CFG );       // Percent of current allowed

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

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

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

    // Configuration inputs just for here
    openAngleCfgReader_ = newConfigReader( DockingServoIF::OPEN_ANGLE_CFG );
    closedAngleCfgReader_ = newConfigReader( DockingServoIF::CLOSED_ANGLE_CFG );
    modeCmdReader_  = newDataReader( DockIF::DOCKING_STATE_CMD );


    // Create the outputs
    armAngleWriter_ = newDataWriter( DockingServoIF::ARM_ANGLE );
    cablePresentWriter_ = newDataWriter( DockIF::DOCK_CABLE_PRESENT );
    cableValueWriter_ = newDataWriter( DockingServoIF::CABLE_VALUE_READING );
    modeWriter_ = newDataWriter( DockIF::DOCKING_STATE );

    // Create the inputs from Dynamic Control
    armAngleReader_ = newDataReader( DockingServoIF::ARM_ANGLE_ACTION );
    armAngleReader_->setImplementor( true );

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

DockingServo::~DockingServo()
{}

bool DockingServo::readConfig( void )
{
    // Check if all the parameters are read correctly
    bool ok = EZServoServo::readConfig();
    ok &= openAngleCfgReader_->read( Units::DEGREE, openAngle_ );
    ok &= closedAngleCfgReader_->read( Units::DEGREE, closedAngle_ );
    return ok;
}

void DockingServo::run( void )
{
}


void DockingServo::uninitialize( void )
{
    uninitializeStart();
    logger_.syslog( "Uninitialize Docking Servo." );

    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 DockingServo::getConfigURI( ConfigOption configOption ) const
{
    return configOption == CONFIG_SIMULATE_HARDWARE ? DockingServoIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI;
}

// Docking arm 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 DockingServo::initServo( void )
{
    bool retVal( true );

    if( !simulateHardware() )
    {
        // set up initialization command and store it in EEPROM (s0) so that it will execute on subsequent power-up
        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( "Docking arm initialization uart error ", uart_.errorString(), Syslog::ERROR );
            retVal = false;
        }
    }
    return retVal;
}



Component::RunState DockingServo::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );
    this->setAllowableFailures( 15 );
    this->setRetryTimeout( 30 );

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

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

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

    cmdArmAngle_ = closedAngle_; // Initialize to a known state in case we're coming out of a failure

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


// Might follow a STOP...START sequence
Component::RunState DockingServo::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( initServo() )
        {
            startTime_ = Timestamp::Now();
            startup_ = WAIT;
        }
        else
        {
            logger_.syslog( Str( "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( "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
Component::RunState DockingServo::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 DockingServo::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isDataRequested() )
    {
        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 DockingServo::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 DockingServo::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    return RUNNABLE; // State not used at this time
}


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

    readConfig();

    // See what mode might be requested if it's being written.
    //if( modeCmdReader_->isActive() && modeCmdReader_->wasTouchedSinceLastRun( this ) )
    if( modeCmdReader_->isActive() )
    {
        // Do a little conversion to actually read in the enum properly
        modeCmd_ = ( DockIF::DockingState )modeCmdReader_->asInt( Units::ENUM );

        // Here we can do things on the edge between states if desired.
        if( lastMode_ != modeCmd_ )
        {
            logger_.syslog( "Changing to mode: " + Str( modeCmd_ ), Syslog::INFO );
            modeChanged_ = true;
            startTime_ = Timestamp::Now(); // since a mode change is going to involve a hardware change, let's start the clock.

            switch( modeCmd_ )
            {
            case DockIF::STANDBY:
                if( debug_ ) logger_.syslog( "Standby mode.", Syslog::INFO );
                cmdArmAngle_ = closedAngle_;
                break;

            case DockIF::ARM:
                if( debug_ ) logger_.syslog( "Armed mode.", Syslog::INFO );
                cmdArmAngle_ = openAngle_;
                break;

            case DockIF::DETACH:
                if( debug_ ) logger_.syslog( "Detach mode.", Syslog::INFO );
                cmdArmAngle_ = openAngle_;
                break;

            case DockIF::SLIDE:
                logger_.syslog( "Slide mode not implemented for docking servo. Going to standby.", Syslog::FAULT );
                mode_ = DockIF::STANDBY;
                break;

            default:
                logger_.syslog( "No such mode! Going to standby.", Syslog::FAULT );
                mode_ =  DockIF::STANDBY;
                break;
            }
        }
    }

    if( simulateHardware() )
    {
        if( getSimulatedMeasurements() )
        {
            publishData();
            lastMode_ = modeCmd_; // reset the last known to current
            this->resetFailCount();
        }
        else
        {
            logger_.syslog( "Could not read simulated measurements from SimSlate.", Syslog::ERROR );
        }
    }
    else
    {
        // 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;
        }
    }


    if( !isnan( cmdArmAngle_ ) )
    {
        // Query the motor controller for position
        haveActualAngle_ = false;
        if( !simulateHardware() )
        {
            float motorPosCmd;
            actualArmAngle_ = getPosition(); // get the actual angle every time
            if( !uart_.hasError() )
            {
                armRdy_ = checkResponse( true ); // Validate the response
                // Write value to the slate
                if( !isnan( actualArmAngle_ ) )
                {
                    actualArmAngle_ = ( ( actualArmAngle_ - mtrCenterCfg_ ) / countsPerDegCfg_ );
                    haveActualAngle_ = true;
                }
                else // Here the value is null but there isn't a serial port error
                {
                    logger_.syslog( "Docking arm 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( "uart error - getPosition..", uart_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::COMMUNICATIONS );
                return STOP;
            }

            if( armRdy_ ) // only send if it's ready to be commanded, not in the middle of a move.
            {
                // Here the ready is because it has arrived
                if( fabs( ( actualArmAngle_ - offsetAngleCfg_ ) - cmdArmAngle_ ) < deviationAngleCfg_
                        && fabs( actualArmAngle_ ) > deviationAngleCfg_ * 4.0 )
                {
                    this->resetFailCount();
                    mode_ = modeCmd_;

                }

                //Calculate commanded position and send to motor controller
                motorPosCmd = limit( mtrCenterCfg_ + ( ( cmdArmAngle_ + 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() )
                {
                    armRdy_ = checkResponse( true );
                }
                else
                {
                    logger_.syslog( "uart error: ", uart_.errorString(), Syslog::FAULT );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    return STOP;
                }
            }

            logCableStatus(); // Grab the cable status

        }
        else // Simulate a really fast servo response
        {
            haveActualAngle_ = true;
            actualArmAngle_ = cmdArmAngle_ ;
        }

    }

    lastMode_ = modeCmd_; // reset the last known to current
    modeChanged_ = false; // reset the mode changed flag
    publishData();

    // both functions need to be called every cycle in order to determine state change
    bool dataRequested = isDataRequested();
    bool needed = isNeeded();

    // If we're not moving and no data is requested we can pause
    if( !dataRequested && !needed )
    {
        return PAUSE; // Pause if we don't want data
    }

    return RUNNABLE;
}


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


Component::RunState DockingServo::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 DockingServo::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 DockingServo::isDataRequested()
{
    //if( debug_ && (armAngleWriter_->isDataRequested() || modeWriter_->isDataRequested()) ) logger_.syslog( "Data is Requested", Syslog::INFO );
    if( debug_ && ( modeWriter_->isDataRequested() ) ) logger_.syslog( "Data is Requested", Syslog::INFO );
    //return armAngleWriter_->isDataRequested() || modeWriter_->isDataRequested();
    return modeWriter_->isDataRequested();
}


bool DockingServo::isNeeded()
{
    bool retVal = true;
    if( !isnan( cmdArmAngle_ ) )
    {
        // We made it to the commanded angle so we're no longer needed.
        if( fabs( ( actualArmAngle_ - offsetAngleCfg_ ) - cmdArmAngle_ ) < deviationAngleCfg_ )
        {
            retVal = false;
            mode_ = modeCmd_; // report that we're in the commanded mode.
        }
    }
    if( debug_ ) logger_.syslog( "Is needed returning:" + Str( retVal ), Syslog::INFO );
    return retVal;
}


bool DockingServo::getSimulatedMeasurements()
{
    // More like data stubs at this point...
    switch( modeCmd_ )
    {
    case DockIF::STANDBY:
        armOpen_ = false;
        cablePresent_ = false;
        mode_ = DockIF::STANDBY;
        return true;

    case DockIF::ARM:
        armOpen_ = true;
        cablePresent_ = false;
        mode_ = DockIF::ARM;
        return true;

    case DockIF::DETACH:
        armOpen_ = true;
        cablePresent_ = false;
        mode_ = DockIF::DETACH;
        return true;

    default:
        break;
    }

    return false;
}


void DockingServo::publishData( void )
{
    // Write the position minus offset to reflect the real position of the physical servo
    if( haveActualAngle_ )
    {
        armAngleWriter_->write( Units::RADIAN, D2R( actualArmAngle_ - offsetAngleCfg_ ) );
    }
    cablePresentWriter_->write( Units::BOOL, cablePresent_ );
    cableValueWriter_->write( Units::COUNT, cableStatus_ );
    modeWriter_->write( Units::COUNT, mode_ );
}


void DockingServo::logCableStatus( void )
{
    uart_.flush();
    uart_ << '/' << controlAddress_ << "?aa\r"; // Query all A/D channels
    Timespan::Milliseconds( 5 ).sleepFor();
    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) ); // The response will be chan 4,3,2,1. The cable sensor is on A/D chan 2
    if( debug_ ) logger_.syslog( "Cable Status Query:" + Str( uartResponse_ ), Syslog::INFO );
    if( 1 == sscanf( uartResponse_, "%*[^,],%*i,%i,%*i", &cableStatus_ ) )
    {
        if( cableStatus_ < 8000 )
        {
            cablePresent_ = true;
            uart_ << '/' << controlAddress_ << "J3R\r";
            uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
        }
        else
        {
            cablePresent_ = false;
            uart_ << '/' << controlAddress_ << "J0R\r";
            uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
        }
    }
    else
    {
        logger_.syslog( "Could not parse cable state. Received:" + Str( uartResponse_ ), Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
    }
}