/*
 *  Software License Agreement
 *
 * Copyright (C) Cross The Road Electronics.  All rights
 * reserved.
 *
 * Cross The Road Electronics (CTRE) licenses to you the right to
 * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
 * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
 *
 * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
 * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
 * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
 * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
 * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
 * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
 * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
 * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
 * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
 * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
 * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
 */
package com.ctre.phoenix.sensors;
import com.ctre.phoenix.ErrorCode;
import com.ctre.phoenix.ParamEnum;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.ErrorCollection;
import com.ctre.phoenix.CustomParamConfigUtil;

//import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
//import edu.wpi.first.wpilibj.hal.HAL;

/**
 * Pigeon IMU Class. Class supports communicating over CANbus and over
 * ribbon-cable (CAN Talon SRX).
 */
public class PigeonIMU extends BasePigeon {

        /** Data object for holding fusion information. */
        public static class FusionStatus {
                /**
                 * Fused Heading
                 */
                public double heading;
                /**
                 * Whether the fusion is valid
                 */
                public boolean bIsValid;
                /**
                 * Whether the pigeon is fusing
                 */
                public boolean bIsFusing;

                /**
                 * Same as getLastError()
                 */
                public ErrorCode lastError;

                /**
                 * @return status of fusion as a string 
                 */
                public String toString() {
                        String description;
                        if (lastError != ErrorCode.OK) {
                                description = "Could not receive status frame.  Check wiring and Phoenix Tuner.";
                        } else if (bIsValid == false) {
                                description = "Fused Heading is not valid.";
                        } else if (bIsFusing == false) {
                                description = "Fused Heading is valid.";
                        } else {
                                description = "Fused Heading is valid and is fusing compass.";
                        }
                        return description;
                }
        }

        /** 
         * Various calibration modes supported by Pigeon. 
         *
         * Note that you can instead use Phoenix Tuner to accomplish certain calibrations.
         *
         */
        public enum CalibrationMode {
                /**
                 * Boot-Calibrate the pigeon
                 */
                BootTareGyroAccel(0), 
                /**
                 * Temperature-Calibrate the pigeon
                 */
                Temperature(1), 
                /**
                 * Magnetometer-Calibrate the pigeon using the 12pt process
                 */
                Magnetometer12Pt(2), 
                /**
                 * Magnetometer-Calibrate the pigeon using 360 turns
                 */
                Magnetometer360(3), 
                /**
                 * Calibrate the pigeon accelerometer
                 */
                Accelerometer(5), 
                /**
                 * Unknown calibration
                 */
                Unknown(-1);

                /**
                 * Value of CalibrationMode
                 */
                public final int value;

                private CalibrationMode(int initValue) {
                        this.value = initValue;
                }

                /**
                 * Get a CalibrationMode of specified value
                 * @param value Value of CalibrationMode
                 * @return CalibrationMode of specified value
                 */
                public static CalibrationMode valueOf(int value) {
                        for (CalibrationMode e : CalibrationMode.values()) {
                                if (e.value == value) {
                                        return e;
                                }
                        }
                        return Unknown;
                }
        };

        /** Overall state of the Pigeon. */
        public enum PigeonState {
                /**
                 * No communications with Pigeon
                 */
                NoComm(0), 
                /**
                 * Pigeon is initializing
                 */
                Initializing(1), 
                /**
                 * Pigeon is ready
                 */
                Ready(2), 
                /**
                 * Pigeon is calibrating due to user
                 */
                UserCalibration(3), 
                /**
                 * Unknown state
                 */
                Unknown(-1);

                /**
                 * Value of PigeonState
                 */
                public final int value;

                private PigeonState(int initValue) {
                        this.value = initValue;
                }

                /**
                 * Get a PigeonState of specified value
                 * @param value Value of PigeonState
                 * @return PigeonState of specified value
                 */
                public static PigeonState valueOf(int value) {
                        for (PigeonState e : PigeonState.values()) {
                                if (e.value == value) {
                                        return e;
                                }
                        }
                        return Unknown;
                }
        };

        /**
         * Data object for status on current calibration and general status.
         *
         * Pigeon has many calibration modes supported for a variety of uses. The
         * modes generally collects and saves persistently information that makes
         * the Pigeon signals more accurate. This includes collecting temperature,
         * gyro, accelerometer, and compass information.
         *
         * For FRC use-cases, typically compass and temperature calibration is not
         * required.
         *
         * Additionally when motion driver software in the Pigeon boots, it will
         * perform a fast boot calibration to initially bias gyro and setup
         * accelerometer.
         *
         * These modes can be enabled with the EnterCalibration mode.
         *
         * When a calibration mode is entered, caller can expect...
         *
         * - PigeonState to reset to Initializing and bCalIsBooting is set to true.
         * Pigeon LEDs will blink the boot pattern. This is similar to the normal
         * boot cal, however it can an additional ~30 seconds since calibration
         * generally requires more information. currentMode will reflect the user's
         * selected calibration mode.
         *
         * - PigeonState will eventually settle to UserCalibration and Pigeon LEDs
         * will show cal specific blink patterns. bCalIsBooting is now false.
         *
         * - Follow the instructions in the Pigeon User Manual to meet the
         * calibration specific requirements. When finished calibrationError will
         * update with the result. Pigeon will solid-fill LEDs with red (for
         * failure) or green (for success) for ~5 seconds. Pigeon then perform
         * boot-cal to cleanly apply the newly saved calibration data.
         */
        public static class GeneralStatus {
                /**
                 * The current state of the motion driver. This reflects if the sensor
                 * signals are accurate. Most calibration modes will force Pigeon to
                 * reinit the motion driver.
                 */
                public PigeonState state;
                /**
                 * The currently applied calibration mode if state is in UserCalibration
                 * or if bCalIsBooting is true. Otherwise it holds the last selected
                 * calibration mode (when calibrationError was updated).
                 */
                public CalibrationMode currentMode;
                /**
                 * The error code for the last calibration mode. Zero represents a
                 * successful cal (with solid green LEDs at end of cal) and nonzero is a
                 * failed calibration (with solid red LEDs at end of cal). Different
                 * calibration
                 */
                public int calibrationError;
                /**
                 * After caller requests a calibration mode, pigeon will perform a
                 * boot-cal before entering the requested mode. During this period, this
                 * flag is set to true.
                 */
                public boolean bCalIsBooting;
                /**
                 * Temperature in Celsius
                 */
                public double tempC;
                /**
                 * Number of seconds Pigeon has been up (since boot). This register is
                 * reset on power boot or processor reset. Register is capped at 255
                 * seconds with no wrap around.
                 */
                public int upTimeSec;
                /**
                 * Number of times the Pigeon has automatically rebiased the gyro. This
                 * counter overflows from 15 -> 0 with no cap.
                 */
                public int noMotionBiasCount;
                /**
                 * Number of times the Pigeon has temperature compensated the various
                 * signals. This counter overflows from 15 -> 0 with no cap.
                 */
                public int tempCompensationCount;
                /**
                 * Same as getLastError()
                 */
                public ErrorCode lastError;

                /**
                 * general string description of current status
                 */
                public String toString() {
                        String description;
                        /* build description string */
                        if (lastError != ErrorCode.OK) { // same as NoComm
                                description = "Status frame was not received, check wired connections and Phoenix Tuner.";
                        } else if (bCalIsBooting) {
                                description = "Pigeon is boot-caling to properly bias accel and gyro.  Do not move Pigeon.  When finished biasing, calibration mode will start.";
                        } else if (state == PigeonState.UserCalibration) {
                                /* mode specific descriptions */
                                switch (currentMode) {
                                case BootTareGyroAccel:
                                        description = "Boot-Calibration: Gyro and Accelerometer are being biased.";
                                        break;
                                case Temperature:
                                        description = "Temperature-Calibration: Pigeon is collecting temp data and will finish when temp range is reached. \n";
                                        description += "Do not move Pigeon.";
                                        break;
                                case Magnetometer12Pt:
                                        description = "Magnetometer Level 1 calibration: Orient the Pigeon PCB in the 12 positions documented in the User's Manual.";
                                        break;
                                case Magnetometer360:
                                        description = "Magnetometer Level 2 calibration: Spin robot slowly in 360' fashion.  ";
                                        break;
                                case Accelerometer:
                                        description = "Accelerometer Calibration: Pigeon PCB must be placed on a level source.  Follow User's Guide for how to level surfacee.  ";
                                        break;
                                default:
                                case Unknown:
                                        description = "Unknown status";
                                        break;
                                }
                        } else if (state == PigeonState.Ready) {
                                /*
                                 * definitely not doing anything cal-related. So just instrument
                                 * the motion driver state
                                 */
                                description = "Pigeon is running normally.  Last CAL error code was ";
                                description += Integer.toString(calibrationError);
                                description += ".";
                        } else if (state == PigeonState.Initializing) {
                                /*
                                 * definitely not doing anything cal-related. So just instrument
                                 * the motion driver state
                                 */
                                description = "Pigeon is boot-caling to properly bias accel and gyro.  Do not move Pigeon.";
                        } else {
                                description = "Not enough data to determine status.";
                        }
                        return description;
                }
        };

        private double[] _generalStatus = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
        private double[] _fusionStatus = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

        /**
         * Create a Pigeon object that communicates with Pigeon on CAN Bus.
         *
         * @param deviceNumber
         *            CAN Device Id of Pigeon [0,62]
         */
        public PigeonIMU(int deviceNumber) {
                super(deviceNumber, "v1");
        }

    //public static void DestroyAllPigeonIMUs() {
    //    PigeonImuJNI.JNI_destroy_AllPigeonImus();
    //}

        /**
         * Create a Pigeon object that communciates with Pigeon through the
         * Gadgeteer ribbon cable connected to a Talon on CAN Bus.
         *
         * @param talonSrx
         *            Object for the TalonSRX connected via ribbon cable.
         */
        public PigeonIMU(TalonSRX talonSrx) {
                super(PigeonImuJNI.JNI_new_PigeonImu_Talon(talonSrx.getDeviceID(), "v1"));
        }

        /**
         * Sets the Fused Heading to the specified value.
         *
         * @param angleDeg  New fused-heading in degrees [+/- 23,040 degrees]
         * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setFusedHeading(double angleDeg, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetFusedHeading(getHandle(), angleDeg, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the Fused Heading to the specified value.
         *
         * @param angleDeg  New fused-heading in degrees [+/- 23,040 degrees]
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setFusedHeading(double angleDeg) {
                int timeoutMs = 0;
                return setFusedHeading( angleDeg,  timeoutMs);
        }
        /**
         * Atomically add to the Fused Heading register.
         *
         * @param angleDeg  Degrees to add to the Fused Heading register.
         * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode addFusedHeading(double angleDeg, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_AddFusedHeading(getHandle(), angleDeg, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Atomically add to the Fused Heading register.
         *
         * @param angleDeg  Degrees to add to the Fused Heading register
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode addFusedHeading(double angleDeg) {
                int timeoutMs = 0;
                return addFusedHeading(angleDeg, timeoutMs);
        }
        /**
         * Sets the Fused Heading register to match the current compass value.
         *
         * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setFusedHeadingToCompass(int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetFusedHeadingToCompass(getHandle(), timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the Fused Heading register to match the current compass value.
         *
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setFusedHeadingToCompass() {
                int timeoutMs = 0;
                return setFusedHeadingToCompass(timeoutMs);
        }

    /**
     * Disable/Enable Temp compensation. Pigeon has this on/False at boot.
     *
     * @param bTempCompDisable Set to "False" to enable temperature compensation.
     * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
        public ErrorCode setTemperatureCompensationDisable(boolean bTempCompDisable, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetTemperatureCompensationDisable(getHandle(), bTempCompDisable ? 1 : 0, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
    /**
     * Disable/Enable Temp compensation. Pigeon has this on/False at boot.
     *
     * @param bTempCompDisable Set to "False" to enable temperature compensation.
     * @return Error Code generated by function. 0 indicates no error.
     */
        public ErrorCode setTemperatureCompensationDisable(boolean bTempCompDisable) {
                int timeoutMs = 0;
                return setTemperatureCompensationDisable(bTempCompDisable, timeoutMs);
        }

        /**
         * Set the declination for compass. Declination is the difference between
         * Earth Magnetic north, and the geographic "True North".
         *
         * @param angleDegOffset  Degrees to set Compass Declination to.
         * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setCompassDeclination(double angleDegOffset, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetCompassDeclination(getHandle(), angleDegOffset, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Set the declination for compass. Declination is the difference between
         * Earth Magnetic north, and the geographic "True North".
         *
         * @param angleDegOffset  Degrees to set Compass Declination to.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setCompassDeclination(double angleDegOffset) {
                int timeoutMs = 0;
                return setCompassDeclination( angleDegOffset, timeoutMs);
        }

        /**
         * Sets the compass angle. Although compass is absolute [0,360) degrees, the
         * continuous compass register holds the wrap-arounds.
         *
         * @param angleDeg  Degrees to set continuous compass angle to.
         * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setCompassAngle(double angleDeg, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetCompassAngle(getHandle(), angleDeg, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the compass angle. Although compass is absolute [0,360) degrees, the
         * continuous compass register holds the wrap-arounds.
         *
         * @param angleDeg  Degrees to set continuous compass angle to.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setCompassAngle(double angleDeg) {
                int timeoutMs = 0;
                return setCompassAngle(angleDeg, timeoutMs);
        }

        // ----------------------- Calibration routines -----------------------//
        /**
         * Enters the Calbration mode.  See the Pigeon IMU documentation for More
         * information on Calibration.
         *
         * Note that you can instead use Phoenix Tuner to accomplish this.
         * Note you should NOT be calling this during normal robot operation.
         *
         * @param calMode  Calibration to execute
         * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode enterCalibrationMode(CalibrationMode calMode, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_EnterCalibrationMode(getHandle(), calMode.value, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Enters the Calbration mode.  See the Pigeon IMU documentation for More
         * information on Calibration.
         *
         * Note that you can instead use Phoenix Tuner to accomplish this.
         * Note you should not be using this in your normal robot application.
         *
         * @param calMode  Calibration to execute
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode enterCalibrationMode(CalibrationMode calMode) {
                int timeoutMs = 0;
                return enterCalibrationMode(calMode, timeoutMs);
        }

        /**
         * Get the status of the current (or previousley complete) calibration.
         *
         * @param toFill Container for the status information.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode getGeneralStatus(GeneralStatus toFill) {
                int retval = PigeonImuJNI.JNI_GetGeneralStatus(getHandle(), _generalStatus);
                toFill.state = PigeonState.valueOf((int) _generalStatus[0]);
                toFill.currentMode = CalibrationMode.valueOf((int) _generalStatus[1]);
                toFill.calibrationError = (int) _generalStatus[2];
                toFill.bCalIsBooting = _generalStatus[3] != 0;
                toFill.tempC = _generalStatus[4];
                toFill.upTimeSec = (int) _generalStatus[5];
                toFill.noMotionBiasCount = (int) _generalStatus[6];
                toFill.tempCompensationCount = (int) _generalStatus[7];
                toFill.lastError = ErrorCode.valueOf(retval);
                return toFill.lastError;
        }

        /**
         * Gets the current Pigeon state
         *
         * @return PigeonState enum
         */
        public PigeonState getState() {
                int retval = PigeonImuJNI.JNI_GetState(getHandle());
                return PigeonState.valueOf(retval);
        }

        /**
         * Get the current Fusion Status (including fused heading)
         *
         * @param toFill        object reference to fill with fusion status flags.
         *                                      Caller may pass null if flags are not needed.
         * @return The fused heading in degrees.
         */
        public double getFusedHeading(FusionStatus toFill) {
                int errorCode = PigeonImuJNI.JNI_GetFusedHeading(getHandle(), _fusionStatus);

                if (toFill != null) {
                        toFill.heading = _fusionStatus[0];
                        toFill.bIsFusing = (_fusionStatus[1] != 0);
                        toFill.bIsValid = (_fusionStatus[2] != 0);
                        toFill.lastError = ErrorCode.valueOf(errorCode);
                }

                return _fusionStatus[0];
        }
        /**
         * Gets the Fused Heading
         *
         * @return The fused heading in degrees.
         */
        public double getFusedHeading() {
                PigeonImuJNI.JNI_GetFusedHeading(getHandle(), _fusionStatus);

                return _fusionStatus[0];
        }
        
        /**
         * Get Accelerometer tilt angles.
         *
         * @param tiltAngles Array to fill with x[0], y[1], and z[2] angles in degrees.
         * @return The last ErrorCode generated.
         */
        public ErrorCode getAccelerometerAngles(double[] tiltAngles) {
                int retval = PigeonImuJNI.JNI_GetAccelerometerAngles(getHandle(), tiltAngles);
                return ErrorCode.valueOf(retval);
        }

    /**
     * Configures all persistent settings.
     *
         * @param allConfigs        Object with all of the persistant settings
     * @param timeoutMs
     *              Timeout value in ms. If nonzero, function will wait for
     *              config success and report an error if it times out.
     *              If zero, no blocking or checking is performed.
     *
     * @return Error Code generated by function. 0 indicates no error. 
     */
        public ErrorCode configAllSettings(PigeonIMUConfiguration allConfigs, int timeoutMs) {
        ErrorCollection errorCollection = new ErrorCollection();
                
                errorCollection.NewError(configFactoryDefault(timeoutMs));
                
                if(CustomParamConfigUtil.customParam0Different(allConfigs))
                        errorCollection.NewError(configSetCustomParam(allConfigs.customParam0, 0, timeoutMs));
                if(CustomParamConfigUtil.customParam1Different(allConfigs))
                        errorCollection.NewError(configSetCustomParam(allConfigs.customParam1, 1, timeoutMs));
        

        return errorCollection._worstError;

        }
    /**
     * Configures all persistent settings (overloaded so timeoutMs is 50 ms).
     *
         * @param allConfigs        Object with all of the persistant settings
     *
     * @return Error Code generated by function. 0 indicates no error. 
     */
        public ErrorCode configAllSettings(PigeonIMUConfiguration allConfigs) {
                int timeoutMs = 0;
                return  configAllSettings(allConfigs, timeoutMs);
        }
    /**
     * Gets all persistant settings.
     *
         * @param allConfigs        Object with all of the persistant settings
     * @param timeoutMs
     *              Timeout value in ms. If nonzero, function will wait for
     *              config success and report an error if it times out.
     *              If zero, no blocking or checking is performed.
     */
    public void getAllConfigs(PigeonIMUConfiguration allConfigs, int timeoutMs) {

        allConfigs.customParam0 = (int) configGetParameter(ParamEnum.eCustomParam, 0,  timeoutMs);
        allConfigs.customParam1 = (int) configGetParameter(ParamEnum.eCustomParam, 1,  timeoutMs);
    }
    /**
     * Gets all persistant settings (overloaded so timeoutMs is 50 ms).
     *
         * @param allConfigs        Object with all of the persistant settings
     */
    public void getAllConfigs(PigeonIMUConfiguration allConfigs) {
        int timeoutMs = 50;
        getAllConfigs(allConfigs, timeoutMs);
    }   
    /**
     * Configures all persistent settings to defaults.
     *
     * @param timeoutMs
     *              Timeout value in ms. If nonzero, function will wait for
     *              config success and report an error if it times out.
     *              If zero, no blocking or checking is performed.
     *
     * @return Error Code generated by function. 0 indicates no error. 
     */
        public ErrorCode configFactoryDefault(int timeoutMs) {
                return ErrorCode.valueOf(PigeonImuJNI.JNI_ConfigFactoryDefault(getHandle(), timeoutMs));
        }
    /**
     * Configures all persistent settings to defaults (overloaded so timeoutMs is 50 ms).
     *
     * @return Error Code generated by function. 0 indicates no error. 
     */
        public ErrorCode configFactoryDefault() {
                int timeoutMs = 50;
                return configFactoryDefault(timeoutMs);
                
        }
        /**
         * Gets the fault status
         *
         * @param toFill
         *            Container for fault statuses.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode getFaults(PigeonIMU_Faults toFill) {
                int bits = PigeonImuJNI.JNI_GetFaults(getHandle());
                toFill.update(bits);
                return getLastError();
        }
        /**
         * Gets the sticky fault status
         *
         * @param toFill
         *            Container for sticky fault statuses.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode getStickyFaults(PigeonIMU_Faults toFill) {
                int bits = PigeonImuJNI.JNI_GetStickyFaults(getHandle());
                toFill.update(bits);
                return getLastError();
        }

}