#ifndef _FOC_H__
#define _FOC_H__
#include "math/fix_math.h"
#include "foc/core/PI_Controller.h"
#include "foc/lineramp.h"

typedef struct {
	float a;
	float b;
}albt_t;

typedef struct {
	float d;
	float q;
}dq_t;

typedef struct {
	float 	curr_abc[3];
	albt_t  curr_ab;
	float   vol_abc[3]; //相对地电压
	float   mot_velocity;   //from hall or encoder
	float 	mot_angle; //from hall or encoder
	lineramp_t target_id;
	lineramp_t target_iq;
	dq_t    target_vol_dq;
	float 	dc_vol;
	bool    b_openloop;
}foc_in_t;

typedef struct {
	u16   	duty[3];
	u16   	low_duty;
	u16   	mid_duty;
	u8    	sector;
	u8    	sample_phase;
	u16   	sample1;
	u16   	sample2;
	dq_t  	vol_dq;
	dq_t    curr_dq;
	albt_t  vol_albeta;
}foc_out_t;

typedef struct {
	foc_in_t in;
	foc_out_t out;
	float   mot_velocity_filterd; //电机滤波后的转速
	float   mot_vel_radusPers; //电机的电角速度
	float   ts;
	float 	sin;
	float 	cos;
	u32     half_period;
	PI_Controller daxis;
	PI_Controller qaxis;
}foc_t;

void foc_init(foc_t *foc);
void foc_update(foc_t *foc);

void foc_abc_2_dq(float a, float b, float c, float *d, float *q);

static __INLINE void foc_set_target_idq(foc_t *foc, dq_t *idq) {
	line_ramp_set_target(&foc->in.target_id, idq->d);
	line_ramp_set_target(&foc->in.target_iq, idq->q);
}

static __INLINE void clark(float A, float B, float C, albt_t *alpha_beta){
	alpha_beta->a = A;
	alpha_beta->b = ONE_BY_SQRT3 * (B - C);
}

static __INLINE void rev_clark(albt_t *alpha_beta, float *ABC){
	ABC[0] = alpha_beta->a;
	ABC[1] = -alpha_beta->a * 0.5f + alpha_beta->b * SQRT3_BY_2;
	ABC[2] = -alpha_beta->a * 0.5f - alpha_beta->b * SQRT3_BY_2;
}

static __INLINE void rev_park(foc_t *foc, dq_t *dq, albt_t *alpha_beta) {
	float c,s;

	s = foc->sin;
	c = foc->cos;

	alpha_beta->a = dq->d * c - dq->q * s;
	alpha_beta->b = dq->d * s + dq->q * c;
}


static __INLINE void park(foc_t *foc, albt_t *alpha_beta, dq_t *dq) {
	float c,s;

	s = foc->sin;
	c = foc->cos;

	dq->d = alpha_beta->a * c + alpha_beta->b * s;
	dq->q = -alpha_beta->a * s + alpha_beta->b * c;
}

#endif /* _FOC_H__ */