/** \file
 *
 *  Contains the AHRS_sp3003D class declaration.
 *
 *  AHRS_sp3003D.h should only be included by AHRS_sp3003D.cpp
 *  Other classes should include AHRS_sp3003DIF.h
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef SP3003D_H
#define SP3003D_H

#include "component/SyncComponent.h"
#include "io/LoadControl.h"
#include "io/UartStream.h"
#include "logger/Logger.h"

#include "data/Matrix3x3.h"

class UniversalDataReader;
class UniversalDataWriter;

/**
 *  Provides software interface to the Sparton SP3003D AHRS.
 *  Can also use SimSlate to provide simulated AHRS values when
 *  they are available.
 *
 *  AHRS_sp3003D.h should only be included by AHRS_sp3003D.cpp
 *  Other classes should include AHRS_sp3003DIF.h
 *  *** Exception is classCalibrateSparton, which is in the same module.
 *
 *  \ingroup modules_sensor
 */
class AHRS_sp3003D : public SyncSensorComponent
{
public:

    AHRS_sp3003D( const Module* module );

    virtual ~AHRS_sp3003D();

    virtual void run();

    /// Do what needs to be done to run
    /// Similar to initialize, in old init/run/uninit sequence
    virtual RunState start();

    /// Might follow a STOP...START sequence
    virtual RunState starting();

    /// Pause for a short period (indicated by pauseTime)
    virtual RunState pause();

    /// Should eventually follow a PAUSE request: should set continueTime
    virtual RunState paused();

    /// Resume from PAUSE
    virtual RunState resume();

    /// Might follow a PAUSE...RESUME sequence
    virtual RunState resuming();

    /// Should eventually follow a START request or RESETTING
    virtual RunState runnable();

    /// Might occur in case of Error
    virtual RunState resetting()
    {
        return stop();
    }

    /// Initial state -- can be later followed by START
    virtual RunState stop();

    virtual RunState stopping();

    /// Initial state -- can be later followed by START
    virtual RunState stopped();

    virtual void uninitialize();

    typedef enum
    {
        CAL_OFF,     // Normal Operation, no calibration underway
        CAL_RESET,   // Normal Operation, but calibration reset to factory default
        CAL_AUTOVAR, // Normal Operation, but magnetic variability calculated
        CAL_MANUAL,  // Manual calibration mode, outputs disabled
        CAL_AUTO,    // Auto calibration mode, outputs disabled
    } CalibrateSpartonMode;

    /// Sets the state and commands the Sparton
    /// Retuns true if successful
    static bool SetCalMode( CalibrateSpartonMode calMode );

    /// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
    virtual ConfigURI getConfigURI( ConfigOption configOption ) const;

private:

    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    AHRS_sp3003D( const AHRS_sp3003D& old ); // disallow copy constructor

    /// Pointer to the singleton-like instance of this component
    static AHRS_sp3003D* Instance_;

    /// The current mode the behavior is in
    CalibrateSpartonMode calMode_;

    /// If true, bail out of calibration mode and restart
    bool calibrationFailure_;

    // Slate inputs
    UniversalDataReader* latitudeReader_;
    UniversalDataReader* longitudeReader_;
    UniversalDataReader* depthReader_;

    // Slate outputs
    DataWriter* compassHeadingWriter_;
    DataWriter* compassTemperatureWriter_;
    DataWriter* mxWriter_;
    DataWriter* myWriter_;
    DataWriter* mzWriter_;
    DataWriter* mtWriter_;
    DataWriter* axWriter_;
    DataWriter* ayWriter_;
    DataWriter* azWriter_;
    DataWriter* atWriter_;

    UniversalDataWriter* magneticHeadingWriter_;
    UniversalDataWriter* trueHeadingWriter_;
    UniversalDataWriter* pitchWriter_;
    UniversalDataWriter* rollWriter_;
    UniversalBlobWriter* rotationMatrixWriter_;

    ConfigReader* readAccelerationsCfgReader_;
    ConfigReader* readMagneticsCfgReader_;
    ConfigReader* readMountingCfgReader_;
    ConfigReader* magDeviationCfgReader_;
    ConfigReader* pitchOffsetCfgReader_;
    ConfigReader* rollOffsetCfgReader_;

    // returns true if data is requested from one of the readers
    bool isDataRequested();

    // Debugging outputs
    bool debug_;
    bool configSet_;
    int verbosity_;

    // Instance of load controller
    LoadControl loadControl_;

    // True if we have alerted user that we are using Simulator instead of hardware.
    bool usingSimWarned_;

    /// Deviation of this compass in the vehicle.
    /// compass orientation +/- deviation due to vehicle = Magnetic orientation
    /// magnetic orientation +/- variation from map = True orientation
    float magDeviation_;
    float magVariation_;

    /// Offset for mounting. In degrees.
    float pitchOffset_;
    float rollOffset_;

    Timespan poTimeout_;
    Timestamp startTime_, dataTimestamp_;
    int readAccelerationsCfg_;
    int readMagneticsCfg_;
    int readMountingCfg_;

    // Stores the current response from the modem
    char deviceResponse_[96];

    // Holds the communications device
    UartStream uart_;

    Matrix3x3 rotationFromVehicleToNavigationFrame_;

    // Interact with uart_
    bool tryUartComms( const char* textToSend, const char* errorPrefix,
                       Syslog::Severity severity = Syslog::ERROR );

    // scan configuraiton settings
    void readConfig();

    // Returns magnetic heading or nan if not available/error
    float getCompassHdg();

    // Returns true heading or nan if not available/error
    float getTrueHdg();

    // Returns pitch/roll or nan if not available/error
    void getPitchRoll( float &pitch, float &roll );

    // Sends a $xxXDR command and interprets the $HCXDR response
    bool readTransducers( float &compassHeading,
                          float &pitch, float &roll,
                          float &temperature );

    // Just sends a $xxXDR command
    void requestTransducers();

    // Configures for a vertical mounting scheme
    void setMountVertical();

    // Just interprets the $HCXDR response
    bool receiveTransducers( float &compassHeading,
                             float &pitch, float &roll, float &temperature );

    // Parses a $HCXDR response
    static bool ParseHCXDR( const char* response, float &compassHeading,
                            float &pitch, float &roll, float &temperature );

    //Only Send: 3 Byte (0xA4, 0x09, 0xA0)
    void requestHeadingMagBin();

    //Only Read & Parse Response: 5 Bytes (0xA4, 0x09, Heading as a 16-bit signed integer, 0xA0)
    bool receiveHeadingMagBin( float &compassHeading );

    //Send: 3 Byte (0xA4, 0x09, 0xA0)
    //Response: 5 Bytes (0xA4, 0x09, Heading as a 16-bit signed integer, 0xA0)
    bool readHeadingMagBin( float &compassHeading );

    //Only Send: 3 Byte (0xA4, 0x06, 0xA0)
    void requestPitchRollBin();

    //Only Read & Parse Response: 7 Bytes (0xA4, 0x06, Pitch, Roll as 16-bit signed integers, 0xA0)
    bool receivePitchRollBin( float &pitch, float &roll );

    //Send: 3 Byte (0xA4, 0x06, 0xA0)
    //Response: 7 Bytes (0xA4, 0x06, Pitch, Roll as 16-bit signed integers, 0xA0)
    bool readPitchRollBin( float &pitch, float &roll );

    // Sends a $PSPA,M command and interprets the response
    bool readMagnetics( float &mX, float &mY, float &mZ, float &mT );

    // Parses the response to the $PSPA,M command
    static bool ParsePSPAM( const char* response, float &mX, float &mY, float &mZ, float &mT );

    // Send: 3 Byte (0xA4, 0x01, 0xA0)
    // Response: 9 Bytes (0xA4, 0x01, Mx, My, Mz as 16-bit integers, 0xA0)
    bool readMagneticsBin( float &mX, float &mY, float &mZ );

    // Sends a $PSPA,A command and interprets the response
    bool readAccelVec( float &aX, float &aY, float &aZ, float &aT );

    // Parses the response to the $PSPA,A command
    static bool ParsePSPAA( const char* response, float &aX, float &aY, float &aZ, float &aT );

    //Send: 3 Byte (0xA4, 0x07, 0xA0)
    //Response: 11 Bytes (0xA4, 0x07, Ax, Ay, Az, Atotal as 16-bit integers, 0xA0)
    bool readAccelVecBin( float &aX, float &aY, float &aZ, float &aT );

    // Parses the quality of the adaptive magnetic calibration process.
    static bool ParseMagErr( const char* response, int& magErr );

    // Calculates a NMEA checksum
    static int CalcNMEAChecksum( const char* response );

    /// Sets the fail state of the slate writers during calibration
    void setWritersInvalid( bool invalid );

    /// Sets the state and commands the Sparton
    /// Retuns true if successful
    bool setCalMode( CalibrateSpartonMode calMode );

    // Flips endian and converts 2 bytes to short
    short extractShort( char * convertFloat );

};

#endif /*SP3003D_H*/