/// \file
///
/// Header file for the EZServoServo class
///
/// Copyright (c) 2007 MBARI
/// MBARI Proprietary Information.  All Rights Reserved
///

#include "EZServoServo.h"

#include <cstdlib>

#include "data/ConfigReader.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "supervisor/CycleStarter.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

EZServoServo::EZServoServo( const Str& name, const ConfigURI& uartCfg, const ConfigURI& baudCfg,
                            const Module* module, EZServoServoType motorServoType,
                            int divisor, int modulo )
    : SyncServoComponent( name, module, divisor, modulo ),
      uart_( uartCfg, baudCfg, 0.3, logger_, 4095 ),
      controlAddress_( 1 ),
      motorServoType_( motorServoType ),
      ezServoError_( NO_ERROR ),
      eepromWriteTimeout_( 1 ),
      startTime_( Timestamp::NOT_SET_TIME ),

      powerOnTimeoutCfg_( nanf( "" ) ),
      currLimitCfg_( nanf( "" ) ),
      limitHiCfg_( nanf( "" ) ),
      limitLoCfg_( nanf( "" ) ),
      pidWCfg_( nanf( "" ) ),
      pidXCfg_( nanf( "" ) ),
      pidYCfg_( nanf( "" ) ),
      overloadTimeoutCfg_( nanf( "" ) ),
      accelCfg_( nanf( "" ) ),
      velocityCfg_( nanf( "" ) ),
      offsetAngleCfg_( nanf( "" ) ),
      countsPerDegCfg_( nanf( "" ) ),
      mtrCenterCfg_( nanf( "" ) ),
      deviationAngleCfg_( nanf( "" ) ),

      powerOnTimeoutCfgReader_( NULL ), // Time to allow system to power up before commanding
      powerOffTimeoutCfgReader_( NULL ), // Time to allow system to fully power down before restarting
      currLimitCfgReader_( NULL ),      // Percent of current allowed
      limitHiCfgReader_( NULL ),        // High physical limit for motor controller
      limitLoCfgReader_( NULL ),        // Low physical limit for motor controller
      pidWCfgReader_( NULL ),           // Proportional gain
      pidXCfgReader_( NULL ),           // Integral gain
      pidYCfgReader_( NULL ),           // Differential gain
      overloadTimeoutCfgReader_( NULL ), // Timeout to wait before throwing overload error
      accelCfgReader_( NULL ),          // Encoder ticks / 32.768 per second squared
      velocityCfgReader_( NULL ),       // Encoder ticks / 32.768 per second squared
      offsetAngleCfgReader_( NULL ),
      countsPerDegCfgReader_( NULL ),   // motor "ticks" per degree of control surface motion
      mtrCenterCfgReader_( NULL ),      // 0 degrees "centered" control surface
      deviationAngleCfgReader_( NULL )  // Number of degrees deviation allowed between expected and actual
{

    uartResponse_[0] = 0;

}

EZServoServo::~EZServoServo() {}

void EZServoServo::initializeStart( void )
{
    logger_.syslog( "Initializing EZServoServo." );

    if( !simulateHardware() )
    {
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Initialization error opening port: ", uart_.errorString(), Syslog::FAULT );
        }
    }
}

void EZServoServo::uninitializeStart( void )
{}

void EZServoServo::uninitializeEnd( void )
{}

float EZServoServo::getPosition()
{
    float position;
    float retVal = nanf( "" );

    uart_ << '/' << controlAddress_ << "?8\r";

    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
    if( uart_.hasError() )
    {
        logger_.syslog( "getPosition uart error ", uart_.errorString(), Syslog::ERROR );
    }
    // Check for a "/0" and ignore the leading 0xFF RS485 line turn around character. Also make sure the string is at least the minimum length
    else if( strstr( uartResponse_, "/0" ) == uartResponse_ + 1 && strlen( uartResponse_ ) > 4 )
    {
        position = strtod( uartResponse_ + 4, NULL );
        // Check for a literal "0" since strtod returns 0 if no float can be constructed
        if( ( position != 0 ) || ( strncmp( uartResponse_ + 4, "0", 1 ) == 0 ) )
        {

            // Check that the position is within the desired range
            if( checkPosition( position ) )
            {
                retVal = position;
            }
            else
            {
                // Critical error
                logger_.syslog( Str( "Reading outside of valid range:" + ( Str ) position + "\n" ), Syslog::FAULT );
            }
        }
    }
    return retVal;
}

// returns the value of ADC 4,3,2,or 1
int EZServoServo::getADC( int channel )
{
    int adVal4, adVal3, adVal2, adVal1;
    int retVal = -1;
    int scanRes = 0;

    uart_ << '/' << controlAddress_ << "?aa\r";

    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
    if( uart_.hasError() )
    {
        logger_.syslog( "getADC uart error ", uart_.errorString(), Syslog::ERROR );
    }
    // Check for a "/0" and ignore the leading 0xFF RS485 line turn around character. Also make sure the string is at least the minimum length
    else if( strstr( uartResponse_, "/0" ) == uartResponse_ + 1 && strlen( uartResponse_ ) > 4 )
    {
        // Grab the values from all channels
        scanRes = sscanf( uartResponse_ + 4, "%d,%d,%d,%d", &adVal4, &adVal3, &adVal2, &adVal1 );
        if( scanRes == 4 )
        {
            // Send back the requested channel.
            switch( channel )
            {
            case 4:
                retVal = adVal4;
                break;
            case 3:
                retVal = adVal3;
                break;
            case 2:
                retVal = adVal2;
                break;
            case 1:
                retVal = adVal1;
                break;
            default:
                logger_.syslog( "Unknown ADC channel: " + ( Str ) channel, Syslog::FAULT );
                this->setFailure( FailureMode::COMMUNICATIONS );
                break;
            }
        }
        else
        {
            logger_.syslog( "only read " + Str( scanRes ) + " of 4 ADC values", Syslog::ERROR );
        }
    }
    return retVal;
}

// Returns nan if there is no/invalid response.
// Otherwise returns the value from the command
// send to the EZ Servo
float EZServoServo::isCommunicating( char *cmd )
{
    float response;
    float retVal = nanf( "" );

    uart_ << '/' << controlAddress_ << cmd << "\r\n";

    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
    if( !uart_.hasError() )
    {
        // Check for a "/0" and ignore the leading 0xFF RS485 line turn around character. Also make sure the string is at least the minimum length
        if( strstr( uartResponse_, "/0" ) == uartResponse_ + 1 && strlen( uartResponse_ ) > 4 )
        {
            response = strtod( uartResponse_ + 4, NULL );
            // Check for a literal "0" since strtod returns 0 if no float can be constructed
            if( ( response != 0 ) || ( strncmp( uartResponse_ + 4, "0", 1 ) == 0 ) )
            {
                retVal = response;
            }
        }
    }
    else
    {
        logger_.syslog( "isCommunicating uart error ", uart_.errorString(), Syslog::ERROR );
    }
    return retVal;
}


// Returns the last commanded velocity that the motor controller received
float EZServoServo::getVelocityCmd( )
{
    float position;
    float retVal = nanf( "" );

    uart_ << '/' << controlAddress_ << "?2\r\n";

    uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );
    if( !uart_.hasError() )
    {
        // Check for a "/0" and ignore the leading 0xFF RS485 line turn around character. Also make sure the string is at least the minimum length
        if( strstr( uartResponse_, "/0" ) == uartResponse_ + 1 && strlen( uartResponse_ ) > 4 )
        {
            position = strtod( uartResponse_ + 4, NULL );
            if( position != 0 )
            {
                retVal = position;
            }
            // Check for a literal zero value
            else if( strncmp( uartResponse_ + 4, "0", 1 ) == 0 )
            {
                retVal = position;
            }
        }
    }
    else
    {
        logger_.syslog( "getVelocity uart error ", uart_.errorString(), Syslog::FAULT );
    }
    return retVal;
}

// Verify that motor controller position is within physical limits
bool EZServoServo::checkPosition( float position )
{
    bool retVal = false;

    if( ( position >= limitLoCfg_ + 1 ) && ( position <= limitHiCfg_ - 1 ) )
    {
        retVal = true;
    }

    return retVal;
}

/**
 * Escapes a regular string so any unprintable characters are shown with typical
 * sequences for human readibility.
 * Introduced as a quick fix to #16 "shore.log download only?"
 * NOTE Pretty much a copy of ESPComm::escape !
 * TODO put this utility in some common place for reuse.
 */
static Str _escapeString( const char* buffer, size_t bufferSize )
{
    Str result = "";
    char aux[64];

    for( size_t i = 0; i < bufferSize; ++i )
    {
        if( buffer[i] == '\n' )
        {
            result += "\\n";
        }
        else if( buffer[i] == '\r' )
        {
            result += "\\r";
        }
        else if( buffer[i] == '\t' )
        {
            result += "\\t";
        }
        else if( buffer[i] == '"' )
        {
            result += "\\\"";
        }
        else if( buffer[i] == '\0' || ( ( unsigned char ) buffer[i] ) & 0200 )
        {
            sprintf( aux, "\\%03o", ( unsigned char ) buffer[i] );
            result += aux;
        }
        else
        {
            sprintf( aux, "%c", buffer[i] );
            result += aux;
        }
    }

    return result;
}

// Looks at EZServo status and sets errors as applicable.
// If checkForReady is true, returns true if controller is reporting
// ready and false if not.
bool EZServoServo::checkResponse( bool checkForReady )
{
    bool retVal = false;
    char* strTokPtr;
    strtok_r( uartResponse_, "/0", &strTokPtr );
    char* response = strtok_r( NULL, "/0", &strTokPtr );
    if( response != NULL )
    {
        int status = response[ 0 ] >> 4;
        ezServoError_ = ( EZServoError )( response[ 0 ] & 0x0F );

        if( checkForReady )
        {
            if( status & RECEIVE_READY )
            {
                retVal = true;
            }
        }
        else
        {
            if( ezServoError_ == NO_ERROR )
            {
                retVal = true;
            }
        }

        // Log any errors
        switch( ezServoError_ )
        {
        case NO_ERROR:
            break;

        // Initialization Error
        case INIT_ERROR:
            logger_.syslog( "InitError", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            break;

        // Bad Command
        case BAD_COMMAND_ERROR:
            logger_.syslog( "Bad Command Error", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            break;

        // Bad Operand
        case BAD_OPERAND_ERROR:
            logger_.syslog( "Bad Operand Error", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            break;

        // Communications Error
        case COMM_ERROR:
            logger_.syslog( "Communications Error", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            break;

        // Not Initialized
        case NOT_INIT_ERROR:
            logger_.syslog( "Not Initialized Error", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            break;

        // Overload Error
        case OVERLOAD_ERROR:
            logger_.syslog( "Overload Error", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            break;

        // Move not allowed
        case NOT_ALLOWED_ERROR:
            logger_.syslog( "Move Not Allowed Error", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            break;

        // Command Overflow error
        case COMMAND_OVERFLOW_ERROR:
            logger_.syslog( "Command Overflow Error", Syslog::FAULT );
            break;

        default:
            logger_.syslog( "Unknown EZ Servo Error: " + ( Str ) ezServoError_, Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            break;
        }
    }
    else
    {
        // Critical error
        logger_.syslog( "Invalid EZ Servo response:\"" +
                        _escapeString( uartResponse_, strnlen( uartResponse_, sizeof( uartResponse_ ) ) ) +
                        "\"", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
    }

    return retVal;
}

// Limits the value to the range limitLo_+1 to limitHi_-1
float EZServoServo::limit( float value )
{
    return AuvMath::Limit( value, limitHiCfg_ - 1, limitLoCfg_ + 1 );
}

// Reads configuration settings
bool EZServoServo::readConfig()
{
    bool ok( true );

    /// Config settings shared by all Servos:
    if( NULL != powerOnTimeoutCfgReader_ )
    {
        ok &= powerOnTimeoutCfgReader_-> read( Units::SECOND, powerOnTimeoutCfg_ ); // Time to allow system to power up before commanding
    }
    if( NULL != powerOffTimeoutCfgReader_ )
    {
        ok &= powerOffTimeoutCfgReader_-> read( Units::SECOND, powerOffTimeoutCfg_ ); // Time to allow system to fully power down before restarting
    }
    if( NULL != currLimitCfgReader_ )
    {
        ok &= currLimitCfgReader_->read( Units::PERCENT, currLimitCfg_ );    // Percent of current allowed
    }

    // Config settings shared by all except Thruster:
    if( NULL != limitHiCfgReader_ )
    {
        ok &= limitHiCfgReader_->read( Units::COUNT, limitHiCfg_ );         // High physical limit for motor controller
    }
    if( NULL != limitLoCfgReader_ )
    {
        ok &= limitLoCfgReader_->read( Units::COUNT, limitLoCfg_ );         // Low physical limit for motor controller
    }

    // Config settings shared by all except Mass:
    if( NULL != pidWCfgReader_ )
    {
        ok &= pidWCfgReader_->read( Units::COUNT, pidWCfg_ );            // Proportional gain
    }
    if( NULL != pidXCfgReader_ )
    {
        ok &= pidXCfgReader_->read( Units::COUNT, pidXCfg_ );            // Integral gain
    }
    if( NULL != pidYCfgReader_ )
    {
        ok &= pidYCfgReader_->read( Units::COUNT, pidYCfg_ );            // Differential gain
    }

    // Config settings shared by all except Elevator + Rudder:
    if( NULL != overloadTimeoutCfgReader_ )
    {
        ok &= overloadTimeoutCfgReader_->read( Units::MILLISECOND, overloadTimeoutCfg_ ); // Timeout to wait before throwing overload error
    }

    // Config settings shared by all except Elevator + Rudder:
    if( NULL != accelCfgReader_ )
    {
        ok &= accelCfgReader_->read( Units::NONE, accelCfg_ );           // Encoder ticks / 32.768 per second squared
    }

    // Config settings shared by Mass + Buoyancy:
    if( NULL != velocityCfgReader_ )
    {
        ok &= velocityCfgReader_->read( Units::NONE, velocityCfg_ );        // Encoder ticks / 32.768 per second
    }

    // Config settings shared by Elevator + Rudder:
    if( NULL != offsetAngleCfgReader_ )
    {
        ok &= offsetAngleCfgReader_->read( Units::DEGREE, offsetAngleCfg_ ); //
    }
    if( NULL != countsPerDegCfgReader_ )
    {
        ok &= countsPerDegCfgReader_->read( Units::COUNT_PER_ANGULAR_DEGREE, countsPerDegCfg_ );   //motor "ticks" per degree of control surface motion
    }
    if( NULL != mtrCenterCfgReader_ )
    {
        ok &= mtrCenterCfgReader_->read( Units::COUNT, mtrCenterCfg_ );      // 0 degrees "centered" control surface
    }
    if( NULL != deviationAngleCfgReader_ )
    {
        ok &= deviationAngleCfgReader_->read( Units::DEGREE, deviationAngleCfg_ ); // Number of degrees deviation allowed between expected and actual
    }
    return ok;
}