/*
 *  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
 */

#pragma once

#include <string>
#include "ctre/phoenix/CANBusAddressable.h"
#include "ctre/phoenix/CustomParamConfiguration.h"
#include "ctre/phoenix/paramEnum.h"
#include "ctre/phoenix/ErrorCode.h"
#include "ctre/phoenix/sensors/PigeonIMU_ControlFrame.h"
#include "ctre/phoenix/sensors/PigeonIMU_Faults.h"
#include "ctre/phoenix/sensors/PigeonIMU_StatusFrame.h"
#include "ctre/phoenix/sensors/PigeonIMU_StickyFaults.h"
#include "ctre/phoenix/sensors/BasePigeonSimCollection.h"

/* forward prototype */
namespace ctre {
namespace phoenix {
namespace motorcontrol {
namespace can {
class TalonSRX;
}
}
}
}

namespace ctre {
namespace phoenix {
/** sensors namespace */
namespace sensors {

/**
 * Configurables available to Pigeon
 */
struct BasePigeonConfiguration : CustomParamConfiguration{
	BasePigeonConfiguration() {}

    /**
     * @return String representation of configs
     */
	std::string toString() {
		return toString("");
	}

    /**
     * @param prependString
     *              String to prepend to configs
     * @return String representation of configs
     */
    std::string toString(std::string prependString) {
        std::string retstr = CustomParamConfiguration::toString(prependString);

        return retstr;
    }
};// struct BasePigeon

/**
 * Util class to help with Pigeon configurations
 */
struct BasePigeonConfigUtils {
private:
	static BasePigeonConfiguration _default;
public:
	/**
	 * Determine if specified value is different from default
	 * @param settings settings to compare against
	 * @return if specified value is different from default
	 * @{
	 */
	static bool CustomParam0Different (const BasePigeonConfiguration & settings) { return (!(settings.customParam0 == _default.customParam0)) || !settings.enableOptimizations; }
	static bool CustomParam1Different (const BasePigeonConfiguration & settings) { return (!(settings.customParam1 == _default.customParam1)) || !settings.enableOptimizations; }
	/** @} */
};

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

	/**
	 * Create a Pigeon object that communicates with Pigeon on CAN Bus.
	 *
	 * @param deviceNumber
	 *            CAN Device Id of Pigeon [0,62]
	 * @param canbus Name of the CANbus; can be a SocketCAN interface (on Linux),
	 *               or a CANivore device name or serial number
	 */
	BasePigeon(int deviceNumber, std::string const &version, std::string const &canbus = "");

    ~BasePigeon();

	/**
	 * Destructs all pigeon objects
	 */
    static void DestroyAllBasePigeons();

	/**
	 * Sets the Yaw register to the specified value.
	 *
	 * @param angleDeg  Degree of Yaw  [+/- 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.
	 */
	int SetYaw(double angleDeg, int timeoutMs = 0);
	/**
	 * 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.
	 */
	int AddYaw(double angleDeg, int timeoutMs = 0);
	/**
	 * 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.
	 */
	int SetYawToCompass(int timeoutMs = 0);

	/**
	 * 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.
	 */
	int SetAccumZAngle(double angleDeg, int timeoutMs = 0);
	/**
	 * 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.
	 */
	ErrorCode GetLastError() const;
	/**
	 * 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.
	 */
	ErrorCode Get6dQuaternion(double wxyz[4]) const;
	/**
	 * Get Yaw, Pitch, and Roll data.
	 *
	 * @param ypr 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.
	 */
	ErrorCode GetYawPitchRoll(double ypr[3]) const;
	/**
	 * Get the yaw from the Pigeon
	 * @return Yaw
	 */
	double GetYaw() const;
	/**
	 * Get the pitch from the Pigeon
	 * @return Pitch
	 */
	double GetPitch() const;
	/**
	 * Get the roll from the Pigeon
	 * @return Roll
	 */
	double GetRoll() const;
	/**
	 * 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.
	 */
	int GetAccumGyro(double xyz_deg[3]) const;
	/**
	 * Get the absolute compass heading.
	 * @return compass heading [0,360) degrees.
	 */
	double GetAbsoluteCompassHeading() const;
	/**
	 * Get the continuous compass heading.
	 * @return continuous compass heading [-23040, 23040) degrees. Use
	 *         SetCompassHeading to modify the wrap-around portion.
	 */
	double GetCompassHeading() const;
	/**
	 * Gets the compass' measured magnetic field strength.
	 * @return field strength in Microteslas (uT).
	 */
	double GetCompassFieldStrength() const;
	/**
	 * Gets the temperature of the pigeon.
	 *
	 * @return Temperature in ('C)
	 */
	double GetTemp() const;
	/**
	 * Gets the current Pigeon uptime.
	 *
	 * @return How long has Pigeon been running in whole seconds. Value caps at
	 *         255.
	 */
	uint32_t GetUpTime() const;
	/**
	 * 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.
	 */
	int GetRawMagnetometer(int16_t rm_xyz[3]) const;

	/**
	 * 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.
	 */
	int GetBiasedMagnetometer(int16_t bm_xyz[3]) const;
	/**
	 * 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.
	 */
	int GetBiasedAccelerometer(int16_t ba_xyz[3]) const;
	/**
	 * 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.
	 */
	int GetRawGyro(double xyz_dps[3]) const;
	/**
	 * @return number of times Pigeon Reset
	 */
	uint32_t GetResetCount() const;
	/**
	 * @return Reset flags for Pigeon
	 */
	uint32_t GetResetFlags() const;
	/**
	 * @return firmware version of Pigeon
	 */
	uint32_t GetFirmVers() const;

	/**
	 * @return true iff a reset has occurred since last call.
	 */
	bool HasResetOccurred() const;

	/**
	 * 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.
	 */
	ErrorCode ConfigSetCustomParam(int newValue, int paramIndex, int timeoutMs = 0);
	/**
	 * 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 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 the custom param.
	 */
	int ConfigGetCustomParam(int paramIndex, int timeoutMs = 0);
	/**
	 * 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.
	 */
	ErrorCode ConfigSetParameter(ParamEnum param, double value,
			uint8_t subValue, int ordinal, int timeoutMs = 0);
	/**
	 * 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.
	 */
	double ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal, int timeoutMs = 0);
    /**
	 * Gets a parameter by passing an int by reference
	 * 
	 * @param param
	 * 			  Parameter enumeration
	 * @param valueToSend
	 * 			  Value to send to parameter
	 * @param valueReceived
	 * 			  Reference to integer to receive
	 * @param subValue
	 * 			  SubValue of parameter
	 * @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.
	 */
    ErrorCode ConfigGetParameter(ParamEnum param, int32_t valueToSend,
            int32_t & valueReceived, uint8_t & subValue, int32_t ordinal,
            int32_t 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.
	 */
	ErrorCode SetStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, uint8_t periodMs,
			int timeoutMs = 0);

	/**
	 * 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.
	 */
	int GetStatusFramePeriod(PigeonIMU_StatusFrame frame,
			int timeoutMs = 0) ;
	/**
	 * 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.
	 */
	ErrorCode SetControlFramePeriod(PigeonIMU_ControlFrame frame,
			int periodMs);
	/**
	 * Gets the firmware version of the device.
	 *
	 * @return param holds the firmware version of the device. Device must be powered
	 * cycled at least once.
	 */
	int GetFirmwareVersion() ;
	/**
	 * Clears the Sticky Faults
	 *
	 * @return Error Code generated by function. 0 indicates no error.
	 */
	ErrorCode ClearStickyFaults(int timeoutMs = 0);
	
	/**
	 * @return Pigeon resource handle.
	 */
	void* GetLowLevelHandle() const {
		return _handle;
	}
	
	//------ All Configs ----------//
    /**
     * 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. 
     */
    virtual ctre::phoenix::ErrorCode ConfigAllSettings(const BasePigeonConfiguration &allConfigs, int timeoutMs = 50);
    /**
     * 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.
     */
    virtual void GetAllConfigs(BasePigeonConfiguration &allConfigs, int timeoutMs = 50);
    /**
     * 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. 
     */
    virtual ErrorCode ConfigFactoryDefault(int timeoutMs = 50);

	/**
	 * @return object that can set simulation inputs.
	 */
	virtual BasePigeonSimCollection& GetSimCollection();
private:
	/** firmware state reported over CAN */
	enum MotionDriverState {
		Init0 = 0,
		WaitForPowerOff = 1,
		ConfigAg = 2,
		SelfTestAg = 3,
		StartDMP = 4,
		ConfigCompass_0 = 5,
		ConfigCompass_1 = 6,
		ConfigCompass_2 = 7,
		ConfigCompass_3 = 8,
		ConfigCompass_4 = 9,
		ConfigCompass_5 = 10,
		SelfTestCompass = 11,
		WaitForGyroStable = 12,
		AdditionalAccelAdjust = 13,
		Idle = 14,
		Calibration = 15,
		LedInstrum = 16,
		Error = 31,
	};
	/** sub command for the various Set param enums */
	enum TareType {
		SetValue = 0x00, AddOffset = 0x01, MatchCompass = 0x02, SetOffset = 0xFF,
	};
	/** data storage for reset signals */
	struct ResetStats {
		int32_t resetCount;
		int32_t resetFlags;
		int32_t firmVers;
		bool hasReset;
	};
	ResetStats _resetStats = { 0, 0, 0, false };

	/** Portion of the arbID for all status and control frames. */
	void* _handle;
	uint32_t _deviceNumber;
	uint32_t _usageHist = 0;
	uint64_t _cache;
	uint32_t _len;
	BasePigeonSimCollection* _simCollection;

	/** overall threshold for when frame data is too old */
	const uint32_t EXPECTED_RESPONSE_TIMEOUT_MS = (200);

	int PrivateSetParameter(ParamEnum paramEnum, TareType tareType,
			double angleDeg, int timeoutMs = 0);

	double GetTemp(const uint64_t & statusFrame);
	
protected:
	BasePigeon(ctre::phoenix::motorcontrol::can::TalonSRX& talonSrx);

};// class BasePigeon
} // namespace signals
} // namespace phoenix
} // namespace ctre
