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

#include "IBIT.h"
#include "IBITIF.h"

#include <limits.h>
#include <stdlib.h>

#include "bitModule/CBITIF.h"
#include "bitModule/SBITIF.h"
#include "controlModule/VerticalControlIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "data/ConfigReader.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "sensorModule/NAL9602IF.h"
#include "sensorModule/OnboardIF.h"
#include "servoModule/ElevatorServoIF.h"
#include "servoModule/RudderServoIF.h"
#include "units/Units.h"
#include "bitModule/CBIT.h"

#define ATMOSPHERE_PSI (14.696 )

IBIT::IBIT( const Module* module )
    : SyncTestComponent( IBITIF::NAME, module ),
      ibitPass_( true ),
      ctrlTimeout_( 15 ),
      commsTimeout_( 65 ),
      sigQuality_( 0 ),
      goodFix_( false ),
      ibitState_( IDLE )
{
    logger_.syslog( "Construct Initiated Built In Test." );
    ibitRunningStateWriter_ = newDataWriter( IBITIF::IBIT_RUNNING_STATE );

    verticalModeWriter_      = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    elevatorAngleCmdWriter_  = newDataWriter( VerticalControlIF::ELEVATOR_ANGLE_CMD );
    elevatorAngleReader_     = newUniversalReader( UniversalURI::PLATFORM_ELEVATOR_ANGLE );
    timeFixReader_           = newUniversalReader( UniversalURI::TIME_FIX ); // TODO: default was 0.01?
    latitudeFixReader_       = newUniversalReader( UniversalURI::LATITUDE_FIX ); // TODO: default was 0.0001?
    longitudeFixReader_      = newUniversalReader( UniversalURI::LONGITUDE_FIX ); // TODO: default was 0.0001?
    batteryChargeReader_     = newUniversalReader( UniversalURI::PLATFORM_BATTERY_CHARGE ); // TODO: default was 0.25?
    batteryVoltageReader_    = newUniversalReader( UniversalURI::PLATFORM_BATTERY_VOLTAGE ); // TODO: default was 0.25?

    sBITRunningReader_       = newDataReader( SBITIF::SBIT_RUNNING_STATE );
    sigQualityReader_        = newDataReader( NAL9602IF::SIG_QUALITY_READING );
    goodFixReader_           = newDataReader( NAL9602IF::GOOD_FIX_STATE );

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

    pressureReader_          = newDataReader( OnboardIF::PRESSURE_READING );
    humidityReader_          = newDataReader( OnboardIF::HUMIDITY_READING );

    // AHRS readers
    headingReader_ = newUniversalReader( UniversalURI::PLATFORM_ORIENTATION );
    pitchReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE );
    rollReader_ = newUniversalReader( UniversalURI::PLATFORM_ROLL_ANGLE );


    // Configuration Readers
    // IBIT
    batteryCapacityThresholdCfgReader_ = newConfigReader( IBITIF::BATTERY_CAPACITY_THRESHOLD ); // Amount of charge left at which we return to the surface
    batteryVoltageThresholdCfgReader_ = newConfigReader( IBITIF::BATTERY_VOLTAGE_THRESHOLD );   // Amount of voltage left at which we return to the surface

    // CBIT
    abortDepthCfgReader_ = newConfigReader( CBITIF::ABORT_DEPTH_CFG );               // Depth at which we drop the weight. Should be greater than all depth envelopes
    stopDepthCfgReader_ = newConfigReader( CBITIF::STOP_DEPTH_CFG );                 // Depth at which we stop the mission. Should be greater than all depth envelopes and less than abort depth
    humidityThresholdCfgReader_ = newConfigReader( CBITIF::HUMIDITY_THRESHOLD_CFG ); // relative humidity
    pressureThresholdCfgReader_ = newConfigReader( CBITIF::PRESSURE_THRESHOLD_CFG ); // Onboard pressure must measure greater than this offset from 1 ATM

    // Control
    buoyancyNeutralCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_NEUTRAL_CFG );
    elevatorLimitCfgReader_  = newConfigReader( VerticalControlIF::ELEVATOR_LIMIT_CFG );
    rudderLimitCfgReader_    = newConfigReader( HorizontalControlIF::RUD_LIMIT_CFG );
    surfaceThresholdCfgReader_    = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );
    massDefaultCfgReader_    = newConfigReader( VerticalControlIF::MASS_DEFAULT_CFG );

    // Servo
    elevDeviationCfgReader_ = newConfigReader( ElevatorServoIF::DEVIATION_ANGLE );
    ruddDeviationCfgReader_ = newConfigReader( RudderServoIF::DEVIATION_ANGLE );
}

IBIT::~IBIT()
{}


// Initialize function
void IBIT::initialize( void )
{
    logger_.syslog( "Initialize IBIT Component.", Syslog::INFO );
    this->setAllowableFailures( 0 );
    ok_ = true;

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

    ibitRunningStateWriter_->write( Units::BOOL, false );
}

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

    // IBIT
    ok &= batteryCapacityThresholdCfgReader_->read( Units::AMPERE_HOUR, batteryCapacityThreshold_ );
    ok &= batteryVoltageThresholdCfgReader_->read( Units::VOLT, batteryVoltageThreshold_ );

    // CBIT
    ok &= abortDepthCfgReader_->read( Units::METER, abortDepth_ );
    ok &= stopDepthCfgReader_->read( Units::METER, stopDepth_ );
    ok &= humidityThresholdCfgReader_->read( Units::PERCENT, bitHumidityThreshold_ );
    ok &= pressureThresholdCfgReader_->read( Units::POUND_PER_SQUARE_INCH, bitPressureThreshold_ );

    // Control
    ok &= elevatorLimitCfgReader_->read( Units::DEGREE, elevatorLimit_ );
    ok &= rudderLimitCfgReader_->read( Units::DEGREE, rudderLimit_ );
    ok &= buoyancyNeutralCfgReader_->read( Units::CUBIC_CENTIMETER, buoyancyNeutral_ );
    ok &= surfaceThresholdCfgReader_->read( Units::METER, surfaceThreshold_ );
    ok &= massDefaultCfgReader_->read( Units::CENTIMETER, massDefault_ );

    // Servo
    ok &= elevDeviationCfgReader_->read( Units::ANGULAR_DEGREE, elevDeviation_ );
    ok &= ruddDeviationCfgReader_->read( Units::ANGULAR_DEGREE, ruddDeviation_ );

    return ok;
}

/// The actual "payload" of the component
void IBIT::run()
{
    if( ibitRunningStateWriter_->isDataRequested() )
    {
        bool sbitRunning( false );
        if( sBITRunningReader_->read( sbitRunning ) && sbitRunning ) // Don't run if SBIT is already running
        {
            logger_.syslog( "Cannot run IBIT while SBIT is in progress.", Syslog::FAULT );
            ibitRunningStateWriter_->write( Units::BOOL, true );
            ibitState_ = DONE;
            ibitPass_ = false;
            return;
        }
        else
        {
            ibitState_ = START;
        }
    }

    float pressure, humidity, pitch, roll, heading;
    bool readPressure, readHumidity;
    switch( ibitState_ )
    {
    case IDLE:
        break;

    case START:

        // Update configuration settings
        readConfig();

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

        ibitPass_ = true;
        ibitRunningStateWriter_->write( Units::BOOL, true );
        logger_.syslog( "Beginning Initiated BIT", Syslog::IMPORTANT );

        // Kick off a GPS read and Satellite signal query
        timeFixReader_->requestData( true );
        sigQualityReader_->requestData( true );

        startTime_ = Timestamp::Now();
        ibitState_ = CTRLHI;

        logger_.syslog( "Beginning control surface checks.", Syslog::IMPORTANT );
        break;

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

        // 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_ ) )
            {
                logger_.syslog( "Control surface position failure.", Syslog::FAULT );
                ibitPass_ = false;
            }
            ibitState_ = CTRLLO;
            startTime_ = Timestamp::Now();
        }
        break;

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

        // Rudder
        horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::RUDDER_ANGLE );
        rudderAngleCmdWriter_->write( Units::DEGREE, -rudderLimit_ );
        // Wait to allow control surfaces to transit
        if( startTime_.elapsed() > ctrlTimeout_ )
        {
            if( !ctrlSurfaceCheckPos( -elevatorLimit_, -rudderLimit_ ) )
            {
                logger_.syslog( "Control surface position failure.", Syslog::FAULT );
                ibitPass_ = false;
            }
            ibitState_ = CTRLCTR;
            startTime_ = Timestamp::Now();
        }
        break;

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

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

    case COMMS:
        // Check communications
        goodFix_ = false;
        sigQuality_ = 0 ;
        if( goodFixReader_->read( goodFix_ ) && sigQualityReader_->read( Units::COUNT, sigQuality_ ) && goodFix_ && ( sigQuality_ > 0 ) )
        {
            float latitude( 0 );
            float longitude( 0 );
            timeFixReader_->requestData( false );
            sigQualityReader_->requestData( false );

            latitudeFixReader_->read( Units::DEGREE, latitude );
            longitudeFixReader_->read( Units::DEGREE, longitude );

            logger_.syslog( Str( "Communications Status:" ) + \
                            "\nFix Status: " + Str( goodFix_ ) + \
                            "\nIridium Signal Strength: " + Str( sigQuality_ ) + \
                            "\nLatitude: " + Str( latitude ) + \
                            " Longitude: " + Str( longitude ), Syslog::IMPORTANT );
            ibitState_ = BATT;
        }

        // See if the timeout has elapsed
        if( startTime_.elapsed() > commsTimeout_ )
        {
            logger_.syslog( Str( "Error acquiring IBIT communications status. Timeout expired." ), Syslog::FAULT );
            ibitPass_ = false;
            ibitState_ = BATT;
        }
        break;

    case BATT:
        if( ( batteryChargeReader_->isActive() ) && ( batteryVoltageReader_->isActive() ) )
        {
            float charge( 0 );
            float voltage( 0 );
            batteryChargeReader_->read( Units::AMPERE_HOUR, charge );
            batteryVoltageReader_->read( Units::VOLT, voltage );
            logger_.syslog( Str( "Battery Status:\nBattery Charge (AH): " ) + Str( charge ) + "\nVoltage: " + Str( voltage ), Syslog::IMPORTANT );
            logger_.syslog( "batteryCapacityThreshold: " + Str( batteryCapacityThreshold_ ) + " Ah", Syslog::IMPORTANT );
            logger_.syslog( "batteryVoltageThreshold: " + Str( batteryVoltageThreshold_ ) + " V", Syslog::IMPORTANT );
        }
        else
        {
            logger_.syslog( Str( "Warning: Battery Data not active. Expected only when running primaries. Threshold checking not active." ), Syslog::FAULT );
        }
        ibitState_ = ENV;
        break;

    case ENV:
        logger_.syslog( "bitHumidityThreshold: " + Str( bitHumidityThreshold_ ) + " %", Syslog::IMPORTANT );
        logger_.syslog( "bitPressureThreshold: " + Str( bitPressureThreshold_ ) + " psi", Syslog::IMPORTANT );
        readPressure = pressureReader_->read( Units::POUND_PER_SQUARE_INCH, pressure );
        readHumidity = humidityReader_->read( Units::PERCENT, humidity );
        if( readPressure && fabs( pressure - ATMOSPHERE_PSI ) < bitPressureThreshold_ )
        {
            logger_.syslog( Str( "Pressure failed. Onboard reading:" + Str( pressure ) + " PSI" ), Syslog::ERROR );
            ibitPass_ = false;
        }
        else if( readPressure )
        {
            logger_.syslog( Str( "Pressure:" + Str( pressure ) + " PSI" ), Syslog::IMPORTANT );
        }
        else
        {
            logger_.syslog( Str( "Could not read pressure" ), Syslog::IMPORTANT );
        }
        if( readHumidity && humidity > bitHumidityThreshold_ )
        {
            logger_.syslog( Str( "Humidity failed. Onboard reading:" + Str( humidity ) + " %" ), Syslog::FAULT );
            ibitPass_ = false;
        }
        else if( readHumidity )
        {
            logger_.syslog( Str( "Humidity:" + Str( humidity ) + " %" ), Syslog::IMPORTANT );
        }
        else
        {
            logger_.syslog( Str( "Could not read humidity" ), Syslog::IMPORTANT );
        }
        ibitState_ = ORIENTATION;
        break;

    case ORIENTATION:
        if( pitchReader_->read( Units::DEGREE, pitch ) && rollReader_->read( Units::DEGREE, roll ) && headingReader_->read( Units::DEGREE, heading ) )
        {
            logger_.syslog( Str( "Vehicle Pitch:" + Str( pitch ) + " degrees" ), Syslog::IMPORTANT );
            logger_.syslog( Str( "Vehicle  Roll:" + Str( roll ) + " degrees" ), Syslog::IMPORTANT );
            logger_.syslog( Str( "Vehicle  Heading:" + Str( heading ) + " degrees" ), Syslog::IMPORTANT );
        }
        else
        {
            logger_.syslog( Str( "Unable to read vehicle orientation values from AHRS" ), Syslog::FAULT );
            ibitPass_ = false;
        }
        ibitState_ = DONE;
        break;

    case DONE:
        logger_.syslog( "surfaceThreshold: " + Str( surfaceThreshold_ ) + " m", Syslog::IMPORTANT );
        logger_.syslog( "buoyancyNeutral: " + Str( buoyancyNeutral_ ) + " cc", Syslog::IMPORTANT );
        logger_.syslog( "massDefault: " + Str( massDefault_ ) + " cm", Syslog::IMPORTANT );
        logger_.syslog( "stopDepth: " + Str( stopDepth_ ) + " m", Syslog::IMPORTANT );
        logger_.syslog( "abortDepth: " + Str( abortDepth_ ) + " m", Syslog::IMPORTANT );
        if( ibitPass_ )
        {
            logger_.syslog( "IBIT PASSED", Syslog::IMPORTANT );
        }
        else
        {
            logger_.syslog( "IBIT FAILED", Syslog::IMPORTANT );
        }
        timeFixReader_->requestData( false );
        sigQualityReader_->requestData( false );
        ibitRunningStateWriter_->write( Units::BOOL, false );
        ibitState_ = IDLE;
        break;

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


bool IBIT::ctrlSurfaceCheckPos( float elevExpect, float ruddExpect )
{
    bool retVal = true;

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

    float rudderIn;
    if( !rudderAngleReader_->read( Units::DEGREE, rudderIn ) )
    {
        logger_.syslog( "Could not read rudderAngleReader_.", Syslog::FAULT );
        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( ruddExpect - rudderIn ) > ruddDeviation_ * 4 )
        {
            logger_.syslog( Str( "Rudder: EXPECTED:" + ( Str )ruddExpect + Str( " ACTUAL:" ) + ( Str )rudderIn ), Syslog::FAULT );
            retVal = false;
        }
    }

    return retVal;
}


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