#ifndef _angsensor_H
#define _angsensor_H
#include <cstdio>
#ifdef __cplusplus
extern "C" {
#endif
#include "gd32c10x_eval.h"
#include "stdio.h"

u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
						 uint16_t *rx_data);

constexpr u16 VEL_FILTER_SIZE = 4;

class angle_sensor {
  public:
	angle_sensor();
	float read_sensorAngle(void);
	float update(void);
	void reset(void);
	float getAngle(void) const;
	float getRawAngle(void) const;
	float getVelocity(void) const;
	float getRawVelocity(void) const;
	float getcurrent_sensorangle(void) const;

  protected:
	void KTH7823_init(void);
	float m_cumulativeAngle;
	float current_angle_deg;
	float current_angle_rad;
	bool m_firstUpdate;
	float m_lastAngle;
	float m_lastcumulativeAngle;
	float velocity[VEL_FILTER_SIZE] = {0.0f};
};

#ifdef __cplusplus
}
#endif

#endif