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

#include "SBIT.h"
#include "SBITIF.h"

#include <limits.h>
#include <stdlib.h>
#include <sys/utsname.h>

#include "bitModule/CBIT.h"
#include "controlModule/VerticalControlIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "data/ConfigReader.h"
#include "module/Config.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "servoModule/ElevatorServoIF.h"
#include "servoModule/MassServoIF.h"
#include "servoModule/RudderServoIF.h"
#include "utils/AuvMath.h"
#include "units/Units.h"
#include "supervisor/CommandLine.h"

#define MASS_SPEED_METER_PER_SEC (0.0005 )

SBIT::SBIT( const Module* module )
    : SyncTestComponent( SBITIF::NAME, module ),
      sbitPass_( true ),
      ctrlTimeout_( 13 ),
      startDelay_( 20 ),
      sbitState_( PRESTART )
{
    logger_.syslog( "Construct Startup Built In Test." );
    sbitRunningStateWriter_ = newDataWriter( SBITIF::SBIT_RUNNING_STATE ); // TODO: How can you specify the default value using DataURI and newDataWriter?

    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    elevatorAngleCmdWriter_  = newDataWriter( VerticalControlIF::ELEVATOR_ANGLE_CMD );
    elevatorAngleReader_ = newUniversalReader( UniversalURI::PLATFORM_ELEVATOR_ANGLE );

    massPosCmdWriter_ = newDataWriter( VerticalControlIF::MASS_POSITION_CMD );
    massPosReader_  =  newUniversalReader( UniversalURI::PLATFORM_MASS_POSITION );

    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    rudderAngleCmdWriter_ = newDataWriter( HorizontalControlIF::RUDDER_ANGLE_CMD );
    rudderAngleReader_ = newUniversalReader( UniversalURI::PLATFORM_RUDDER_ANGLE );

    kernelReleaseCfgReader_ = newConfigReader( SBITIF::KERNEL_RELEASE_CFG );
    kernelVersionCfgReader_ = newConfigReader( SBITIF::KERNEL_VERSION_CFG );
    elevatorLimitCfgReader_ = newConfigReader( VerticalControlIF::ELEVATOR_LIMIT_CFG );
    massLimitFwdCfgReader_ = newConfigReader( VerticalControlIF::MASS_POSITION_LIMIT_FWD_CFG );
    massLimitAftCfgReader_ = newConfigReader( VerticalControlIF::MASS_POSITION_LIMIT_AFT_CFG );
    massDefaultCfgReader_ = newConfigReader( VerticalControlIF::MASS_DEFAULT_CFG );
    rudderLimitCfgReader_ = newConfigReader( HorizontalControlIF::RUD_LIMIT_CFG );

    // Servo
    elevDeviationCfgReader_ = newConfigReader( ElevatorServoIF::DEVIATION_ANGLE );
    massDeviationCfgReader_ = newConfigReader( MassServoIF::DEVIATION_DISTANCE );
    ruddDeviationCfgReader_ = newConfigReader( RudderServoIF::DEVIATION_ANGLE );

}

SBIT::~SBIT()
{}


// Initialize function
void SBIT::initialize( void )
{
    struct utsname unameData; // For getting kernel information

    logger_.syslog( "Initialize SBIT Component.", Syslog::INFO );
    this->setAllowableFailures( 0 );
    ok_ = true;

    if( !readConfig() )
    {
        ok_ = false;
        logger_.syslog( "Error: Error loading parameters in initialization routine.", Syslog::CRITICAL );
        return;
    }

    // Print out the file data to have some idea of what tag/revision we're running
    logger_.syslog( "git: " + Str( _GIT_DESCRIBE ), Syslog::IMPORTANT );
    logger_.syslog( "git hash: " + Str( _GIT_HASH ), Syslog::INFO );

// Grab the kernel version next and check against configuration
    if( 0 == uname( &unameData ) )
    {
        // Check configuration versus expected kernel release
        Str kernelRelease( kernelReleaseCfgReader_->asString( Units::NONE ) );
        if( kernelRelease.compare( unameData.release ) == 0 )
        {
            logger_.syslog( "Kernel Release: " + kernelRelease, Syslog::IMPORTANT );
        }
        else
        {
            if( simulateHardware() ) logger_.syslog( "Kernel Reporting Different Release From Configuration.\nKernel Expected: " + kernelRelease + "\nKernel Reported: " + ( Str )unameData.release, Syslog::INFO );
            else logger_.syslog( "Kernel Reporting Different Release From Configuration.\nKernel Expected: " + kernelRelease + "\nKernel Reported: " + ( Str )unameData.release, Syslog::FAULT );
        }
        // Check configuration versus expected kernel version
        Str kernelVersion( kernelVersionCfgReader_->asString( Units::NONE ) );
        if( kernelVersion.compare( unameData.version ) == 0 )
        {
            logger_.syslog( "Kernel Version:" + kernelVersion, Syslog::IMPORTANT );
        }
        else
        {
            if( simulateHardware() ) logger_.syslog( "Kernel Reporting Different Version From Configuration.\nKernel Expected: " + kernelVersion + "\nKernel Reported: " + ( Str )unameData.version, Syslog::INFO );
            else logger_.syslog( "Kernel Reporting Different Version From Configuration.\nKernel Expected: " + kernelVersion + "\nKernel Reported: " + ( Str )unameData.version, Syslog::FAULT );
        }
    }
    else
    {
        logger_.syslog( "Could not get kernel information.", Syslog::FAULT );
    }

    // Prevent missions from being loaded by the command line
    sbitRunningStateWriter_->write( Units::BOOL, true );

    // Set the delay time needed at the beginning to allow the mass to home and get into default position
    startDelay_ += abs( ( massDefault_ * 2 ) / MASS_SPEED_METER_PER_SEC );
    logger_.syslog( Str( "Beginning SBIT in " + Str( startDelay_.asDouble() ) + " seconds." ), Syslog::INFO );

    startTime_ = Timestamp::Now();
}

// Read configruation
bool SBIT::readConfig( void )
{
    // Check if all the parameters are read correctly
    bool ok = true;

    ok &= elevatorLimitCfgReader_->read( Units::DEGREE, elevatorLimit_ );
    ok &= massLimitFwdCfgReader_->read( Units::METER, massLimitFwd_ );
    ok &= massLimitAftCfgReader_->read( Units::METER, massLimitAft_ );
    ok &= massDefaultCfgReader_->read( Units::METER, massDefault_ );
    ok &= rudderLimitCfgReader_->read( Units::DEGREE, rudderLimit_ );

    ok &= elevDeviationCfgReader_->read( Units::DEGREE, elevDeviation_ );
    ok &= massDeviationCfgReader_->read( Units::METER, massDeviation_ );
    ok &= ruddDeviationCfgReader_->read( Units::DEGREE, ruddDeviation_ );

    return ok;
}


/// The actual "payload" of the component
void SBIT::run()
{
    double massCmd = 0;

    if( sbitRunningStateWriter_->isDataRequested()	)
    {
        sbitState_ = PRESTART;
    }

    switch( sbitState_ )
    {
    case IDLE:
        break;
    case PRESTART:
        if( startTime_.elapsed() > startDelay_ )
        {
            sbitState_ = START;
        }
        break;
    case START:
        // Update configuration
        readConfig();

        // Kick off a GF scan
        CBIT::SetGFScanForce( true );

        sbitRunningStateWriter_->write( Units::BOOL, true );
        logger_.syslog( "Beginning Startup BIT", Syslog::IMPORTANT );
        startTime_ = Timestamp::Now();
        sbitState_ = CTRLHI;
    // tell static analysis no break intended

    case CTRLHI:
        // Elevator
        verticalModeWriter_->write( Units::ENUM, VerticalControlIF::MASS_AND_ELEVATOR );
        elevatorAngleCmdWriter_->write( Units::DEGREE, elevatorLimit_ );

        // Mass
        massCmd = AuvMath::Limit( massDefault_ + ( massLimitFwd_ / 5 ), massLimitFwd_, massLimitAft_ );
        massPosCmdWriter_->write( Units::METER,  massCmd ); // Just check for 1/5 travel due to time

        // Rudder
        horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::RUDDER_ANGLE );
        rudderAngleCmdWriter_->write( Units::DEGREE, rudderLimit_ );

        // Wait to allow control surfaces to transit and then check position
        if( startTime_.elapsed() > ctrlTimeout_ )
        {
            if( !ctrlSurfaceCheckPos( elevatorLimit_, rudderLimit_, massCmd ) )
            {
                sbitPass_ = false;
            }
            sbitState_ = SETLO;
            // Mass -- needs a head start
            massCmd = AuvMath::Limit( massDefault_ + ( massLimitAft_ / 7 ), massLimitAft_, massLimitFwd_ );
            massPosCmdWriter_->write( Units::METER, massCmd ); // Just check for 1/7 travel due to time. Using a lower value here to accomodate shorter primary battery packs.
        }
        break;

    case SETLO:
        sbitState_ = CTRLLO;
        startTime_ = Timestamp::Now();
    // tell static analysis no break intended

    case CTRLLO:
        // Elevator
        verticalModeWriter_->write( Units::ENUM, VerticalControlIF::MASS_AND_ELEVATOR );
        elevatorAngleCmdWriter_->write( Units::DEGREE, -elevatorLimit_ );

        // Mass
        massCmd = AuvMath::Limit( massDefault_ + ( massLimitAft_ / 7 ), massLimitAft_, massLimitFwd_ );
        massPosCmdWriter_->write( Units::METER, massCmd ); // Just check for 1/7 travel due to time.Using a lower value here to accomodate shorter primary battery packs.

        // Rudder
        horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::RUDDER_ANGLE );
        rudderAngleCmdWriter_->write( Units::DEGREE, -rudderLimit_ );

        // Wait to allow control surfaces to transit
        if( startTime_.elapsed() > ( ctrlTimeout_ + ctrlTimeout_ ) )
        {
            if( !ctrlSurfaceCheckPos( -elevatorLimit_, -rudderLimit_, massCmd ) )
            {
                logger_.syslog( "Control surface position failure.", Syslog::FAULT );
                sbitPass_ = false;
            }
            sbitState_ = SETCTR;
        }
        break;

    case SETCTR:
        sbitState_ = CTRLCTR;
        startTime_ = Timestamp::Now();
    // tell static analysis no break intended

    case CTRLCTR:
        // Elevator
        verticalModeWriter_->write( Units::ENUM, VerticalControlIF::MASS_AND_ELEVATOR );
        elevatorAngleCmdWriter_->write( Units::DEGREE, 0 );

        // Mass (not center, but default position - center trim)
        massPosCmdWriter_->write( Units::METER, massDefault_ );

        // Rudder
        horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::RUDDER_ANGLE );
        rudderAngleCmdWriter_->write( Units::DEGREE, 0 );
        if( startTime_.elapsed() > ctrlTimeout_ )
        {
            if( !ctrlSurfaceCheckPos( 0, 0, massDefault_ ) )
            {
                logger_.syslog( "Control surface position failure.", Syslog::FAULT );
                sbitPass_ = false;
            }
            sbitState_ = DONE;
        }
        break;

    case DONE:
        if( sbitPass_ )
        {
            logger_.syslog( "SBIT PASSED", Syslog::IMPORTANT );
        }
        else
        {
            logger_.syslog( "SBIT FAILED", Syslog::CRITICAL );
        }

        // Run configSet list to display any overrides to the config
        Config::LoadPersistedConfigSets( logger_, false, true );

        sbitRunningStateWriter_->write( Units::BOOL, false );
        sbitState_ = IDLE;
        break;

    default:
        logger_.syslog( Str( "Invalid SBIT state." ), Syslog::ERROR );
        break;
    }
}


bool SBIT::ctrlSurfaceCheckPos( double elevExpect, double ruddExpect, double massExpect )
{
    bool retVal = true;

    double elevatorIn;
    if( !elevatorAngleReader_->read( Units::DEGREE, elevatorIn ) )
    {
        logger_.syslog( "Could not read elevatorAngleReader_.", Syslog::ERROR );
        retVal = false;
    }

    double rudderIn;
    if( !rudderAngleReader_->read( Units::DEGREE, rudderIn ) )
    {
        logger_.syslog( "Could not read rudderAngleReader_.", Syslog::ERROR );
        retVal = false;
    }

    double massIn;
    if( !massPosReader_->read( Units::METER, massIn ) )
    {
        logger_.syslog( "Could not read massPosReader_.", Syslog::ERROR );
        retVal = false;
    }

    if( retVal )
    {
        if( fabs( elevExpect - elevatorIn ) > elevDeviation_ * 4 )
        {
            logger_.syslog( Str( "Elevator: EXPECTED:" + ( Str )elevExpect + Str( " ACTUAL:" ) + ( Str )elevatorIn ), Syslog::FAULT );
            retVal = false;
        }

        if( fabs( massExpect - massIn ) > massDeviation_ )
        {
            logger_.syslog( Str( "Mass: EXPECTED:" + ( Str )massExpect + Str( " ACTUAL:" ) + ( Str )massIn ), Syslog::FAULT );
            retVal = false;
        }

        if( fabs( ruddExpect - rudderIn ) > ruddDeviation_ * 4 )
        {
            logger_.syslog( Str( "Rudder: EXPECTED:" + ( Str )ruddExpect + Str( " ACTUAL:" ) + ( Str )rudderIn ), Syslog::FAULT );
            retVal = false;
        }
    }

    return retVal;
}


/// Uninit function
void SBIT::uninitialize( void )
{
//    sbitRunningStateWriter_->write( Units::BOOL, false );
    logger_.syslog( "Uninitialize SBIT Component." );
}

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