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

#include "CalibrateSparton.h"
#include "CalibrateSpartonIF.h"

#include "controlModule/HorizontalControlIF.h"
#include "controlModule/SpeedControlIF.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "sensorModule/AHRS_sp3003D.h"
#include "utils/AuvMath.h"
#include "units/Units.h"

/*
 * CalibrateSparton .
 *
 */

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

CalibrateSparton::~CalibrateSparton()
{
}

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

    initialized_ =  AHRS_sp3003D::SetCalMode( AHRS_sp3003D::CAL_RESET );
    if( initialized_ )
    {
        initialized_ = AHRS_sp3003D::SetCalMode( AHRS_sp3003D::CAL_AUTOVAR );
    }
    if( initialized_ )
    {
        initialized_ = AHRS_sp3003D::SetCalMode( AHRS_sp3003D::CAL_MANUAL );
    }
}

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

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

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

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