/*
 * madgwick.hpp
 *
 *  Created on: Jun 20, 2024
 *      Author: Dario
 */

#ifndef STA_MATHS_ATTITUDE_INTEGRATE_HPP
#define STA_MATHS_ATTITUDE_INTEGRATE_HPP

#include <sta/math/quaternion.hpp>

#include <cstdint>

namespace sta
{
	namespace math
	{
		class MadgwickFilter {
		public:
			/**
			 * @brief Construct a new Madgwick Filter object
			 * 
			 * @param n 
			 * @param alpha 
			 */
			MadgwickFilter(Quaternion state, Quaternion direction, uint32_t n, float alpha);

			/**
			 * @brief Predicts the next step using rotation rate ingegration.
			 * 
			 * @param dt The time that passed since the last predict.
			 * @param ox Rotation rate around the x axis in rad/s.
			 * @param oy Rotation rate around the y axis in rad/s.
			 * @param oz Rotation rate around the z axis in rad/s.
			 */
			void predict(float dt, float ox, float oy, float oz);

			void correct(float dx, float dy, float dz);
		private:
			Quaternion objective(Quaternion q, Quaternion s);

			Quaternion state_;
			Quaternion direction_;
			uint32_t n_;
			float alpha_;
		};
	}  // namespace math
}  // namespace sta



#endif /* STA_MATHS_ATTITUDE_INTEGRATE_HPP */