/*
 *  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.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 BasePigeon {
        private long m_handle;
        private BasePigeonSimCollection m_simCollection;

        private int m_deviceNumber = 0;
        
        /**
         * Create a Pigeon object that communicates with Pigeon on CAN Bus.
         *
         * @param deviceNumber
         *            CAN Device Id of Pigeon [0,62]
         * @param version Version of the PigeonIMU
         * @param canbus Name of the CANbus; can be a SocketCAN interface (on Linux),
         *               or a CANivore device name or serial number
         */
        public BasePigeon(int deviceNumber, String version, String canbus) {
                m_handle = PigeonImuJNI.JNI_new_PigeonImu(deviceNumber, version, canbus);
                m_deviceNumber = deviceNumber;
                m_simCollection = new BasePigeonSimCollection(this, false);
        }

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

        protected BasePigeon(long handle) {
                m_handle = handle;
                m_simCollection = new BasePigeonSimCollection(this, false);
        }

        /**
         * Destructor for Pigeon
         * @return Error Code generated by function. 0 indicates no error.
         */
    public ErrorCode DestroyObject() {
        return ErrorCode.valueOf(PigeonImuJNI.JNI_destroy_PigeonImu(m_handle));
    }

    //public static void DestroyAllBasePigeons() {
    //    PigeonImuJNI.JNI_destroy_AllBasePigeons();
    //}

        /**
         * @return object that can set simulation inputs.
         */
        public BasePigeonSimCollection getSimCollection(){ return m_simCollection;}

        /**
         * Sets the Yaw register to the specified value.
         *
         * @param angleDeg  New yaw in degrees [+/- 368,640 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 setYaw(double angleDeg, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetYaw(m_handle, angleDeg, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the Yaw register to the specified value.
         *
         * @param angleDeg  New yaw in degrees [+/- 368,640 degrees]
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setYaw(double angleDeg) {
                int timeoutMs = 0;
                return setYaw( angleDeg,  timeoutMs);
        }
        /**
         * Atomically add to the Yaw register.
         *
         * @param angleDeg  Degrees to add to the Yaw 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 addYaw(double angleDeg, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_AddYaw(m_handle, angleDeg, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Atomically add to the Yaw register.
         *
         * @param angleDeg  Degrees to add to the Yaw register.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode addYaw(double angleDeg) {
                int timeoutMs = 0;
                return addYaw( angleDeg,  timeoutMs);   
        }
        /**
         * Sets the Yaw 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 setYawToCompass(int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetYawToCompass(m_handle, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the Yaw register to match the current compass value.
         *
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setYawToCompass() {
                int timeoutMs = 0;
                return setYawToCompass(timeoutMs);
        }
        /**
         * Sets the AccumZAngle.
         *
         * @param angleDeg  Degrees to set AccumZAngle 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 setAccumZAngle(double angleDeg, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetAccumZAngle(m_handle, angleDeg, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the AccumZAngle.
         *
         * @param angleDeg  Degrees to set AccumZAngle to.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setAccumZAngle(double angleDeg) {
                int timeoutMs = 0;
                return setAccumZAngle(angleDeg, timeoutMs);
        }

        // ----------------------- General Error status -----------------------//
        /**
         * Call GetLastError() generated by this object.
         * Not all functions return an error code but can
         * potentially report errors.
         *
         * This function can be used to retrieve those error codes.
         *
         * @return The last ErrorCode generated.
         */
        public ErrorCode getLastError() {
                int retval = PigeonImuJNI.JNI_GetLastError(m_handle);
                return ErrorCode.valueOf(retval);
        }

        // ----------------------- Strongly typed Signal decoders
        // -----------------------//
        /**
         * Get 6d Quaternion data.
         *
         * @param wxyz Array to fill with quaternion data w[0], x[1], y[2], z[3]
         * @return The last ErrorCode generated.
         */
        public ErrorCode get6dQuaternion(double[] wxyz) {
                int retval = PigeonImuJNI.JNI_Get6dQuaternion(m_handle, wxyz);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Get Yaw, Pitch, and Roll data.
         *
         * @param ypr_deg Array to fill with yaw[0], pitch[1], and roll[2] data.
         *                                      Yaw is within [-368,640, +368,640] degrees.
         *                                      Pitch is within [-90,+90] degrees.
         *                                      Roll is within [-90,+90] degrees.
         * @return The last ErrorCode generated.
         */
        public ErrorCode getYawPitchRoll(double[] ypr_deg) {
                int retval = PigeonImuJNI.JNI_GetYawPitchRoll(m_handle, ypr_deg);
                return ErrorCode.valueOf(retval);
        }

        /**
         * Get the yaw from the Pigeon
         * @return Yaw
         */
        public double getYaw() {
                return PigeonImuJNI.JNI_GetYaw(m_handle);
        }
        /**
         * Get the pitch from the Pigeon
         * @return Pitch
         */
        public double getPitch() {
                return PigeonImuJNI.JNI_GetPitch(m_handle);
        }
        /**
         * Get the roll from the Pigeon
         * @return Roll
         */
        public double getRoll() {
                return PigeonImuJNI.JNI_GetRoll(m_handle);
        }
        /**
         * Get AccumGyro data.
         * AccumGyro is the integrated gyro value on each axis.
         *
         * @param xyz_deg Array to fill with x[0], y[1], and z[2] AccumGyro data
         * @return The last ErrorCode generated.
         */
        public ErrorCode getAccumGyro(double[] xyz_deg) {
                int retval = PigeonImuJNI.JNI_GetAccumGyro(m_handle, xyz_deg);
                return ErrorCode.valueOf(retval);
        }

        /**
         * Get the absolute compass heading.
         * @return compass heading [0,360) degrees.
         */
        public double getAbsoluteCompassHeading() {
                double retval = PigeonImuJNI.JNI_GetAbsoluteCompassHeading(m_handle);
                return retval;
        }

        /**
         * Get the continuous compass heading.
         * @return continuous compass heading [-23040, 23040) degrees. Use
         *         SetCompassHeading to modify the wrap-around portion.
         */
        public double getCompassHeading() {
                double retval = PigeonImuJNI.JNI_GetCompassHeading(m_handle);
                return retval;
        }

        /**
         * Gets the compass' measured magnetic field strength.
         * @return field strength in Microteslas (uT).
         */
        public double getCompassFieldStrength() {
                double retval = PigeonImuJNI.JNI_GetCompassFieldStrength(m_handle);
                return retval;
        }
        /**
         * Gets the temperature of the pigeon.
         *
         * @return Temperature in ('C)
         */
        public double getTemp() {
                double retval = PigeonImuJNI.JNI_GetTemp(m_handle);
                return retval;
        }

        /**
         * Gets the current Pigeon uptime.
         *
         * @return How long has Pigeon been running in whole seconds. Value caps at
         *         255.
         */
        public int getUpTime() {
                int retval = PigeonImuJNI.JNI_GetUpTime(m_handle);
                return retval;
        }
        /**
         * Get Raw Magnetometer data.
         *
         * @param rm_xyz Array to fill with x[0], y[1], and z[2] data
         *                              Number is equal to 0.6 microTeslas per unit.
         * @return The last ErrorCode generated.
         */
        public ErrorCode getRawMagnetometer(short[] rm_xyz) {
                int retval = PigeonImuJNI.JNI_GetRawMagnetometer(m_handle, rm_xyz);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Get Biased Magnetometer data.
         *
         * @param bm_xyz Array to fill with x[0], y[1], and z[2] data
         *                              Number is equal to 0.6 microTeslas per unit.
         * @return The last ErrorCode generated.
         */
        public ErrorCode getBiasedMagnetometer(short[] bm_xyz) {
                int retval = PigeonImuJNI.JNI_GetBiasedMagnetometer(m_handle, bm_xyz);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Get Biased Accelerometer data.
         *
         * @param ba_xyz Array to fill with x[0], y[1], and z[2] data.
         *                      These are in fixed point notation Q2.14.  eg. 16384 = 1G
         * @return The last ErrorCode generated.
         */
        public ErrorCode getBiasedAccelerometer(short[] ba_xyz) {
                int retval = PigeonImuJNI.JNI_GetBiasedAccelerometer(m_handle, ba_xyz);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Get Raw Gyro data.
         *
         * @param xyz_dps Array to fill with x[0], y[1], and z[2] data in degrees per second.
         * @return The last ErrorCode generated.
         */
        public ErrorCode getRawGyro(double[] xyz_dps) {
                int retval = PigeonImuJNI.JNI_GetRawGyro(m_handle, xyz_dps);
                return ErrorCode.valueOf(retval);
        }

        /**
         * @return number of times Pigeon Reset
         */
        public int getResetCount() {
                int k = PigeonImuJNI.JNI_GetResetCount(m_handle);
                return k;
        }
        /**
         * @return Reset flags for Pigeon
         */
        public int getResetFlags() {
                int k = PigeonImuJNI.JNI_GetResetFlags(m_handle);
                return k;
        }
        /**
         * Gets the firmware version of the device.
         *
         * @return param holds the firmware version of the device. Device must be powered
         * cycled at least once.
         */
        public int getFirmwareVersion() {
                int k = PigeonImuJNI.JNI_GetFirmwareVersion(m_handle);
                return k;
        }

        /**
         * @return true iff a reset has occurred since last call.
         */
        public boolean hasResetOccurred() {
                boolean k = PigeonImuJNI.JNI_HasResetOccurred(m_handle);
                return k;
        }

        /**
         * Sets the value of a custom parameter. This is for arbitrary use.
     *
     * Sometimes it is necessary to save calibration/declination/offset
     * information in the device. Particularly if the
     * device is part of a subsystem that can be replaced.
         *
         * @param newValue
         *            Value for custom parameter.
         * @param paramIndex
         *            Index of custom parameter. [0-1]
         * @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 configSetCustomParam(int newValue, int paramIndex, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_ConfigSetCustomParam(m_handle, newValue, paramIndex, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the value of a custom parameter. This is for arbitrary use.
     *
     * Sometimes it is necessary to save calibration/declination/offset
     * information in the device. Particularly if the
     * device is part of a subsystem that can be replaced.
         *
         * @param newValue
         *            Value for custom parameter.
         * @param paramIndex
         *            Index of custom parameter. [0-1]
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode configSetCustomParam(int newValue, int paramIndex ) {
                int timeoutMs = 0;
                return configSetCustomParam( newValue,  paramIndex, timeoutMs);
        }

        /**
         * Gets the value of a custom parameter. This is for arbitrary use.
     *
     * Sometimes it is necessary to save calibration/declination/offset
     * information in the device. Particularly if the
     * device is part of a subsystem that can be replaced.
         *
         * @param paramIndex
         *            Index of custom parameter. [0-1]
         * @param timoutMs
         *            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 Value of the custom param.
         */
        public int configGetCustomParam(int paramIndex, int timoutMs) {
                int retval = PigeonImuJNI.JNI_ConfigGetCustomParam(m_handle, paramIndex, timoutMs);
                return retval;
        }
        /**
         * Gets the value of a custom parameter. This is for arbitrary use.
     *
     * Sometimes it is necessary to save calibration/declination/offset
     * information in the device. Particularly if the
     * device is part of a subsystem that can be replaced.
         *
         * @param paramIndex
         *            Index of custom parameter. [0-1]
         * @return Value of the custom param.
         */
        public int configGetCustomParam(int paramIndex) {
                int timeoutMs = 0;
                return configGetCustomParam(paramIndex,timeoutMs);
        }

        /**
         * Sets a parameter. Generally this is not used.
     * This can be utilized in
     * - Using new features without updating API installation.
     * - Errata workarounds to circumvent API implementation.
     * - Allows for rapid testing / unit testing of firmware.
         *
         * @param param
         *            Parameter enumeration.
         * @param value
         *            Value of parameter.
         * @param subValue
         *            Subvalue for parameter. Maximum value of 255.
         * @param ordinal
         *            Ordinal of parameter.
         * @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 configSetParameter(ParamEnum param, double value, int subValue, int ordinal, int timeoutMs) {
                return configSetParameter(param.value, value, subValue, ordinal, timeoutMs);
        }
        /**
         * Sets a parameter. Generally this is not used.
     * This can be utilized in
     * - Using new features without updating API installation.
     * - Errata workarounds to circumvent API implementation.
     * - Allows for rapid testing / unit testing of firmware.
         *
         * @param param
         *            Parameter enumeration.
         * @param value
         *            Value of parameter.
         * @param subValue
         *            Subvalue for parameter. Maximum value of 255.
         * @param ordinal
         *            Ordinal of parameter.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode configSetParameter(ParamEnum param, double value, int subValue, int ordinal) {
                int timeoutMs = 0;
                return configSetParameter(param,  value,  subValue,  ordinal, timeoutMs);
        }
        /**
         * Sets a parameter. Generally this is not used.
     * This can be utilized in
     * - Using new features without updating API installation.
     * - Errata workarounds to circumvent API implementation.
     * - Allows for rapid testing / unit testing of firmware.
         *
         * @param param
         *            Parameter enumeration.
         * @param value
         *            Value of parameter.
         * @param subValue
         *            Subvalue for parameter. Maximum value of 255.
         * @param ordinal
         *            Ordinal of parameter.
         * @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 configSetParameter(int param, double value, int subValue, int ordinal, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_ConfigSetParameter(m_handle, param, value, subValue, ordinal,
                                timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets a parameter. Generally this is not used.
     * This can be utilized in
     * - Using new features without updating API installation.
     * - Errata workarounds to circumvent API implementation.
     * - Allows for rapid testing / unit testing of firmware.
         *
         * @param param
         *            Parameter enumeration.
         * @param value
         *            Value of parameter.
         * @param subValue
         *            Subvalue for parameter. Maximum value of 255.
         * @param ordinal
         *            Ordinal of parameter.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode configSetParameter(int param, double value, int subValue, int ordinal) {
                int timeoutMs = 0;
                return  configSetParameter( param,  value,  subValue,  ordinal, timeoutMs);
        }
        /**
         * Gets a parameter. Generally this is not used.
     * This can be utilized in
     * - Using new features without updating API installation.
     * - Errata workarounds to circumvent API implementation.
     * - Allows for rapid testing / unit testing of firmware.
         *
         * @param param
         *            Parameter enumeration.
         * @param ordinal
         *            Ordinal of parameter.
         * @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 Value of parameter.
         */
        public double configGetParameter(ParamEnum param, int ordinal, int timeoutMs) {
                return configGetParameter(param.value, ordinal, timeoutMs);
        }
        /**
         * Gets a parameter. Generally this is not used.
     * This can be utilized in
     * - Using new features without updating API installation.
     * - Errata workarounds to circumvent API implementation.
     * - Allows for rapid testing / unit testing of firmware.
         *
         * @param param
         *            Parameter enumeration.
         * @param ordinal
         *            Ordinal of parameter.
         * @return Value of parameter.
         */
        public double configGetParameter(ParamEnum param, int ordinal ) {
                int timeoutMs = 0;
                return configGetParameter(param,  ordinal, timeoutMs);
        }
        /**
         * Gets a parameter.
         *
         * @param param
         *            Parameter enumeration.
         * @param ordinal
         *            Ordinal of parameter.
         * @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 Value of parameter.
         */
        public double configGetParameter(int param, int ordinal, int timeoutMs) {
                return PigeonImuJNI.JNI_ConfigGetParameter(m_handle, param, ordinal, timeoutMs);
        }
        /**
         * Gets a parameter.
         *
         * @param param
         *            Parameter enumeration.
         * @param ordinal
         *            Ordinal of parameter.
         * @return Value of parameter.
         */
        public double configGetParameter(int param, int ordinal) {
                int timeoutMs = 0;
                return configGetParameter( param,  ordinal, timeoutMs);
        }
        /**
         * Sets the period of the given status frame.
         *
         * @param statusFrame
         *            Frame whose period is to be changed.
         * @param periodMs
         *            Period in ms for the given frame.
         * @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 setStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, int periodMs, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetStatusFramePeriod(m_handle, statusFrame.value, periodMs, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the period of the given status frame.
         *
         * @param statusFrame
         *            Frame whose period is to be changed.
         * @param periodMs
         *            Period in ms for the given frame.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, int periodMs) {
                int timeoutMs = 0;
                return setStatusFramePeriod( statusFrame,  periodMs, timeoutMs);
        }
        /**
         * Sets the period of the given status frame.
         *
         * @param statusFrame
         *            Frame whose period is to be changed.
         * @param periodMs
         *            Period in ms for the given frame.
         * @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 setStatusFramePeriod(int statusFrame, int periodMs, int timeoutMs) {
                int retval = PigeonImuJNI.JNI_SetStatusFramePeriod(m_handle, statusFrame, periodMs, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the period of the given status frame.
         *
         * @param statusFrame
         *            Frame whose period is to be changed.
         * @param periodMs
         *            Period in ms for the given frame.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setStatusFramePeriod(int statusFrame, int periodMs) {
                int timeoutMs = 0;
                return setStatusFramePeriod( statusFrame,  periodMs, timeoutMs);
        }

        /**
         * Gets the period of the given status frame.
         *
         * @param frame
         *            Frame to get the period of.
         * @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 Period of the given status frame.
         */
        public int getStatusFramePeriod(PigeonIMU_StatusFrame frame, int timeoutMs) {
                return PigeonImuJNI.JNI_GetStatusFramePeriod(m_handle, frame.value, timeoutMs);
        }
        /**
         * Gets the period of the given status frame.
         *
         * @param frame
         *            Frame to get the period of.
         * @return Period of the given status frame.
         */
        public int getStatusFramePeriod(PigeonIMU_StatusFrame frame) {
                int timeoutMs = 0;
                return getStatusFramePeriod(frame, timeoutMs);
        }
        /**
         * Sets the period of the given control frame.
         *
         * @param frame
         *            Frame whose period is to be changed.
         * @param periodMs
         *            Period in ms for the given frame.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setControlFramePeriod(PigeonIMU_ControlFrame frame, int periodMs) {
                int retval = PigeonImuJNI.JNI_SetControlFramePeriod(m_handle, frame.value, periodMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Sets the period of the given control frame.
         *
         * @param frame
         *            Frame whose period is to be changed.
         * @param periodMs
         *            Period in ms for the given frame.
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode setControlFramePeriod(int frame, int periodMs) {
                int retval = PigeonImuJNI.JNI_SetControlFramePeriod(m_handle, frame, periodMs);
                return ErrorCode.valueOf(retval);
        }

        // ------ Faults ----------//
        /**
         * Clears the Sticky Faults
         *
         * @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 clearStickyFaults(int timeoutMs) {
                int retval = PigeonImuJNI.JNI_ClearStickyFaults(m_handle, timeoutMs);
                return ErrorCode.valueOf(retval);
        }
        /**
         * Clears the Sticky Faults
         *
         * @return Error Code generated by function. 0 indicates no error.
         */
        public ErrorCode clearStickyFaults() {
                int timeoutMs = 0;
                return clearStickyFaults(timeoutMs);
        }

        /**
         * @return The Device Number
         */
        public int getDeviceID(){
                return m_deviceNumber;
        }
    /**
     * 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(m_handle, 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);
                
        }

        public long getHandle() { return m_handle; }

}