/** \file
 *
 *  Contains the PNI_TCM class declaration.
 *
 *  PNI_TCM.h should only be included by PNI_TCM.cpp
 *  Other classes should include PNI_TCMIF.h
 *
 *  Copyright (c) 2014 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */


#ifndef PNI_TCM_H
#define PNI_TCM_H

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

class UniversalDataReader;
class UniversalDataWriter;

/**
 *  Provides software interface to the PNI TCM compass.
 *  Can also use SimSlate to provide simulated PNI values when
 *  they are available.
 *
 *  PNI_TCM.h should only be included by PNI_TCM.cpp
 *  Other classes should include PNI_TCMIF.h
 *
 *  \ingroup modules_sensor
 */
class PNI_TCM : public SyncSensorComponent
{
public:

    PNI_TCM( const Module* module );

    virtual ~PNI_TCM();

    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();

    /// 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.
    PNI_TCM( const PNI_TCM& old ); // disallow copy constructor

    // scan configuraiton settings
    void readConfig();

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

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

    // Slate outputs
    DataWriter* compassHeadingWriter_;
    DataWriter* compassTemperatureWriter_;
    DataWriter* mxWriter_;
    DataWriter* myWriter_;
    DataWriter* mzWriter_;

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

    ConfigReader* verbosityCfgReader_;
    ConfigReader* magDeviationCfgReader_;
    ConfigReader* pitchOffsetCfgReader_;
    ConfigReader* rollOffsetCfgReader_;

// DEBUG : These need to be replaced by real frame IDs with a calculated CRC after EcoHAB spring is finished. For now they remain hard coded.
    static const unsigned int CMD_SIZE = 24;
    static const unsigned int DATA_RESP_SIZE = 41; // byte count (2), Frame ID (1), payload (36), CRC (2)
    // Commands
    static char SetConfigAutoCal_[CMD_SIZE];
    static char SetConfigCalNumPts_[CMD_SIZE];
    static char SetConfigMagCoeff_[CMD_SIZE];
    static char SetConfigMountingRef_[CMD_SIZE];
    static char SetConfigBaudRate_[CMD_SIZE];
    static char SetBigEndian_[CMD_SIZE];
    static char SetDataComponents_[CMD_SIZE];
    static char StartCalFullRange_[CMD_SIZE];
    static char StartContinuousMode_[CMD_SIZE];
    static char StopContinuousMode_[CMD_SIZE];
    static char SetAcqParams_[CMD_SIZE];
    static char Save_[CMD_SIZE];

    static const float HEADING_LIMIT[];
    static const float PITCH_LIMIT[];
    static const float ROLL_LIMIT[];

    typedef enum
    {
        SETDATA,   // Sets the data to be output from the device
        MOUNTING,  // Sets the mounting reference
        ENDIAN,    // Sets the endian
        PARAMS,    // Sets the aquisition parameters
        PARAMRESPONSE,
        DONE,      // Ready to run
    } StartupSequence;

    StartupSequence startup_;

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

    // Debugging outputs
    bool debug_;
    int verbosity_;

    // Instance of load controller
    LoadControl loadControl_;

    bool checksum_matches_;
    float heading_, pitch_, roll_, magHeading_, trueHeading_; // TODO: make rotation matrix member variable
    float mX_, mY_, mZ_; // TODO: make vector member variable
    float temperature_;
    Matrix3x3 rotationFromVehicleToNavigationFrame_;

    /// 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_, validTimeout_;
    Timestamp startTime_;

    int readAccelerationsCfg_;
    int readMagneticsCfg_;

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

    // Holds the communications device
    UartStream uart_;


    // getDataResp returning Heading (magnetic), Pitch, Roll, MagX, MagY, MagZ, Temperature. Values are float32
    bool receiveGetDataResp( void );

    void processData( void );

    // Writes the data
    void writeData( void );

    // Parses the calibration score which may take > 1 minute to be calculated.
    bool parseCalScore( float &magScore, float &distError, float &tiltError, float &tiltRange );

    unsigned short getCRC( const char* data, unsigned int len );

    // Flips endian and converts 4 bytes to floating point
    float extractFloat( char * convertFloat );

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

    // Sets the writer's validity to valid or invalid
    void setWritersInvalid( bool invalid );

    // Gets the magnetic variation
    void getMagneticVariation();

};

#endif /*PNI_TCM_H*/