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

#include "CalibrateAHRS_M2.h"
#include "CalibrateAHRS_M2IF.h"

#include "sensorModule/AHRS_M2.h"
#include "units/Units.h"

/*
 * CalibrateAHRS_M2 .
 *
 */

CalibrateAHRS_M2::CalibrateAHRS_M2( const Str& prefix, const Module* module )
    : Behavior( prefix + CalibrateAHRS_M2IF::NAME, module, true, true ),
      initialized_( false )
{
    logger_.syslog( "Construct CalibrateSparton." );
}

CalibrateAHRS_M2::~CalibrateAHRS_M2()
{
}

/// Starts compass in AUTO cal mode
void CalibrateAHRS_M2::initialize( void )
{
    logger_.syslog( "Initialize CalibrateSpartonComponent.", Syslog::INFO );

    initialized_ = AHRS_M2::SetCalMode( AHRS_M2::CAL_AUTO );

}

/// Run just waits while the mission circles
void CalibrateAHRS_M2::run()
{
    if( !initialized_ )
    {
        initialize();
    }
}

/// Kicks compass into AUTO cal mode
void CalibrateAHRS_M2::preempted()
{
    logger_.syslog( "Preempted CalibrateSpartonComponent.", Syslog::INFO );
    if( !initialized_ )
    {
        initialize();
    }
    AHRS_M2::SetCalMode( AHRS_M2::CAL_AUTO );
}

/// Turns cal mode OFF, reports result.
void CalibrateAHRS_M2::uninitialize( void )
{
    logger_.syslog( "Uninitialize CalibrateSpartonComponent.", Syslog::INFO );
    AHRS_M2::SetCalMode( AHRS_M2::CAL_OFF );
    initialized_ = false;
}

/// Mission Component factory interface
Behavior* CalibrateAHRS_M2::CreateBehavior( const Str& prefix, const Module* module )
{
    return new CalibrateAHRS_M2( prefix, module );
}