/****************************************************************************
 * include/dsp.h
 *
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.  The
 * ASF licenses this file to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance with the
 * License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
 * License for the specific language governing permissions and limitations
 * under the License.
 *
 ****************************************************************************/

#ifndef __INCLUDE_DSP_H
#define __INCLUDE_DSP_H

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <nuttx/compiler.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include <stdbool.h>
#include <math.h>

#include <assert.h>

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

/* Disable DEBUGASSERT macro if LIBDSP debug is not enabled */

#ifdef CONFIG_LIBDSP_DEBUG
#  ifndef CONFIG_DEBUG_ASSERTIONS
#    warning "Need CONFIG_DEBUG_ASSERTIONS to work properly"
#  endif
#else
#  undef DEBUGASSERT
#  define DEBUGASSERT(x)
#endif

#ifndef CONFIG_LIBDSP_PRECISION
#  define CONFIG_LIBDSP_PRECISION 0
#endif

/* Phase rotation direction */

#define DIR_CW   (1.0f)
#define DIR_CCW  (-1.0f)

/* Some math constants ******************************************************/

#define SQRT3_BY_TWO_F     (0.866025f)
#define SQRT3_BY_THREE_F   (0.57735f)
#define ONE_BY_SQRT3_F     (0.57735f)
#define TWO_BY_SQRT3_F     (1.15470f)

/* Some lib constants *******************************************************/

/* Motor electrical angle is in range 0.0 to 2*PI */

#define MOTOR_ANGLE_E_MAX    (2.0f*M_PI_F)
#define MOTOR_ANGLE_E_MIN    (0.0f)
#define MOTOR_ANGLE_E_RANGE  (MOTOR_ANGLE_E_MAX - MOTOR_ANGLE_E_MIN)

/* Motor mechanical angle is in range 0.0 to 2*PI */

#define MOTOR_ANGLE_M_MAX    (2.0f*M_PI_F)
#define MOTOR_ANGLE_M_MIN    (0.0f)
#define MOTOR_ANGLE_M_RANGE  (MOTOR_ANGLE_M_MAX - MOTOR_ANGLE_M_MIN)

/* Some useful macros *******************************************************/

/****************************************************************************
 * Name: LP_FILTER
 *
 * Description:
 *   Simple single-pole digital low pass filter:
 *     Y(n) = (1-beta)*Y(n-1) + beta*X(n) = (beta * (Y(n-1) - X(n)))
 *
 *     filter - (0.0 - 1.0) where 1.0 gives unfiltered values
 *     filter = T * (2*PI) * f_c
 *
 *     phase shift = -arctan(f_in/f_c)
 *
 *     T    - period at which the digital filter is being calculated
 *     f_in - input frequency of the filter
 *     f_c  - cutoff frequency of the filter
 *
 * REFERENCE: https://www.embeddedrelated.com/showarticle/779.php
 *
 ****************************************************************************/

#define LP_FILTER(val, sample, filter) val -= (filter * (val - sample))

/****************************************************************************
 * Name: SVM3_BASE_VOLTAGE_GET
 *
 * Description:
 *  Get maximum voltage for SVM3 without overmodulation
 *
 *  Notes:
 *   max possible phase voltage for 3-phase power inwerter:
 *     Vd = (2/3)*Vdc
 *   max phase reference voltage according to SVM modulation diagram:
 *     Vrefmax = Vd * cos(30*) = SQRT3_BY_2 * Vd
 *   which give us:
 *     Vrefmax = SQRT3_BY_3 * Vdc
 *
 *   Vdc - bus voltage
 *
 ****************************************************************************/

#define SVM3_BASE_VOLTAGE_GET(vbus) (vbus * SQRT3_BY_THREE_F)

/****************************************************************************
 * Public Types
 ****************************************************************************/

/* This structure represents phase angle.
 * Besides angle value it also stores sine and cosine values for given angle.
 */

struct phase_angle_s
{
  float   angle;               /* Phase angle in radians <0, 2PI> */
  float   sin;                 /* Phase angle sine */
  float   cos;                 /* Phase angle cosine */
};

typedef struct phase_angle_s phase_angle_t;

/* This structure stores motor angles and corresponding sin and cos values
 *
 * th_el = th_m * pole_pairs
 * th_m = th_el/pole_pairs
 *
 * where:
 *   th_el      - motor electrical angle
 *   th_m       - motor mechanical angle
 *   pole_pairs - motor pole pairs
 *
 *  NOTE: pole_pairs = poles_total/2
 */

struct motor_angle_s
{
  phase_angle_t angle_el;      /* Electrical angle */
  float         anglem;        /* Mechanical angle in radians <0, 2PI> */
  float         one_by_p;      /* Aux variable */
  uint8_t       p;             /* Number of the motor pole pairs */
  int8_t        i;             /* Pole counter */
};

/* Float number saturaton */

struct float_sat_s
{
  float min;                    /* Lower limit */
  float max;                    /* Upper limit */
};

typedef struct float_sat_s float_sat_t;

/* PI/PID controller state structure */

struct pid_controller_s
{
  float       out;              /* Controller output */
  float_sat_t sat;              /* Output saturation */
  float       err;              /* Current error value */
  float       err_prev;         /* Previous error value */
  float       KP;               /* Proportional coefficient */
  float       KI;               /* Integral coefficient */
  float       KD;               /* Derivative coefficient */
  float       part[3];          /* 0 - proporitonal part
                                 * 1 - integral part
                                 * 2 - derivative part
                                 */
};

typedef struct pid_controller_s pid_controller_t;

/* This structure represents the ABC frame (3 phase vector) */

struct abc_frame_s
{
  float a;                     /* A component */
  float b;                     /* B component */
  float c;                     /* C component */
};

typedef struct abc_frame_s abc_frame_t;

/* This structure represents the alpha-beta frame (2 phase vector) */

struct ab_frame_s
{
  float a;                     /* Alpha component */
  float b;                     /* Beta component */
};

typedef struct ab_frame_s ab_frame_t;

/* This structure represent the direct-quadrature frame */

struct dq_frame_s
{
  float d;                     /* Driect component */
  float q;                     /* Quadrature component */
};

typedef struct dq_frame_s dq_frame_t;

/* Space Vector Modulation data for 3-phase system */

struct svm3_state_s
{
  uint8_t     sector;          /* Current space vector sector */
  float       d_u;             /* Duty cycle for phase U */
  float       d_v;             /* Duty cycle for phase V */
  float       d_w;             /* Duty cycle for phase W */
  float       d_max;           /* Duty cycle max */
  float       d_min;           /* Duty cycle min */
};

/* Motor open-loop control data */

struct openloop_data_s
{
  float max;           /* Open-loop max speed */
  float angle;         /* Open-loop current angle normalized to <0.0, 2PI> */
  float per;           /* Open-loop control execution period */
};

/* Common motor observer structure */

struct motor_observer_s
{
  float angle;               /* Estimated observer angle */
  float speed;               /* Estimated observer speed */
  float per;                 /* Observer execution period */

  float angle_err;           /* Observer angle error.
                              * This can be used to gradually eliminate
                              * error between openloop angle and observer
                              * angle
                              */

  /* There are different types of motor observers which different
   * sets of private data.
   */

  void *so;                  /* Speed estimation observer data */
  void *ao;                  /* Angle estimation observer data */
};

/* Speed observer division method data */

struct motor_sobserver_div_s
{
  float angle_diff;             /* Mechanical angle difference */
  float angle_acc;              /* Accumulated mechanical angle */
  float angle_prev;             /* Previous mechanical angle */
  float one_by_dt;              /* Frequency of observer execution */
  float cntr;                   /* Sample counter */
  float samples;                /* Number of samples for observer */
  float filter;                 /* Low-pass filter for final omega */
};

/* Speed observer PLL method data */
#if 0
struct motor_sobserver_pll_s
{
  /* TODO */
};
#endif

/* Motor Sliding Mode Observer private data */

struct motor_observer_smo_s
{
  float k_slide;        /* Bang-bang controller gain */
  float err_max;        /* Linear mode threshold */
  float F;              /* Current observer F gain (1-Ts*R/L) */
  float G;              /* Current observer G gain (Ts/L) */
  float emf_lp_filter1; /* Adaptive first low pass EMF filter */
  float emf_lp_filter2; /* Adaptive second low pass EMF filter */
  ab_frame_t emf;       /* Estimated back-EMF */
  ab_frame_t emf_f;     /* Fitlered estimated back-EMF */
  ab_frame_t z;         /* Correction factor */
  ab_frame_t i_est;     /* Estimated idq current */
  ab_frame_t v_err;     /* v_err = v_ab - emf */
  ab_frame_t i_err;     /* i_err = i_est - i_dq */
  ab_frame_t sign;      /* Bang-bang controller sign */
};

/* Motor physical parameters.
 * This data structure was designed to work with BLDC/PMSM motors,
 * but probably can be used to describe different types of motors.
 */

struct motor_phy_params_s
{
  uint8_t p;                   /* Number of the motor pole pairs */

  float   res;                 /* Phase-to-neutral temperature compensated
                                * resistance
                                */
  float   res_base;            /* Phase-to-neutral base resistance */
  float   res_alpha;           /* Temperature coefficient of resistance */
  float   res_temp_ref;        /* Reference temperature of alpha */
  float   ind;                 /* Average phase-to-neutral inductance */
  float   one_by_ind;          /* Inverse phase-to-neutral inductance */
};

/* Field oriented control (FOC) data
 * REVISIT:
 */

struct foc_data_s
{
  abc_frame_t      v_abc;    /* Voltage in ABC frame */
  ab_frame_t       v_ab;     /* Voltage in alpha-beta frame */
  dq_frame_t       v_dq;     /* Voltage in dq frame */
  ab_frame_t       v_ab_mod; /* Modulation voltage normalized to
                              * magnitude (0.0, 1.0)
                              */

  abc_frame_t      i_abc;    /* Current in ABC frame */
  ab_frame_t       i_ab;     /* Current in apha-beta frame */
  dq_frame_t       i_dq;     /* Current in dq frame */
  dq_frame_t       i_dq_err; /* DQ current error */

  dq_frame_t       i_dq_ref; /* Current dq reference frame */
  pid_controller_t id_pid;   /* Current d-axis component PI controller */
  pid_controller_t iq_pid;   /* Current q-axis component PI controller */

  float vdq_mag_max;         /* Maximum dq voltage magnitude */
  float vab_mod_scale;       /* Voltage alpha-beta modulation scale */
};

/****************************************************************************
 * Public Functions Prototypes
 ****************************************************************************/

#undef EXTERN
#if defined(__cplusplus)
#define EXTERN extern "C"
extern "C"
{
#else
#define EXTERN extern
#endif

/* Math functions */

float fast_sin(float angle);
float fast_sin2(float angle);
float fast_cos(float angle);
float fast_cos2(float angle);
float fast_atan2(float y, float x);

void f_saturate(FAR float *val, float min, float max);

float vector2d_mag(float x, float y);
void vector2d_saturate(FAR float *x, FAR float *y, float max);

void dq_saturate(FAR dq_frame_t *dq, float max);
float dq_mag(FAR dq_frame_t *dq);

/* PID controller functions */

void pid_controller_init(FAR pid_controller_t *pid,
                         float KP, float KI, float KD);
void pi_controller_init(FAR pid_controller_t *pid,
                        float KP, float KI);
void pid_saturation_set(FAR pid_controller_t *pid, float min, float max);
void pi_saturation_set(FAR pid_controller_t *pid, float min, float max);
void pid_integral_reset(FAR pid_controller_t *pid);
void pi_integral_reset(FAR pid_controller_t *pid);
float pi_controller(FAR pid_controller_t *pid, float err);
float pid_controller(FAR pid_controller_t *pid, float err);

/* Transformation functions */

void clarke_transform(FAR abc_frame_t *abc, FAR ab_frame_t *ab);
void inv_clarke_transform(FAR ab_frame_t *ab, FAR abc_frame_t *abc);
void park_transform(FAR phase_angle_t *angle, FAR ab_frame_t *ab,
                    FAR dq_frame_t *dq);
void inv_park_transform(FAR phase_angle_t *angle, FAR dq_frame_t *dq,
                        FAR ab_frame_t *ab);

/* Phase angle related functions */

void angle_norm(FAR float *angle, float per, float bottom, float top);
void angle_norm_2pi(FAR float *angle, float bottom, float top);
void phase_angle_update(FAR struct phase_angle_s *angle, float val);

/* 3-phase system space vector modulation */

void svm3_init(FAR struct svm3_state_s *s, float min, float max);
void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *ab);
void svm3_current_correct(FAR struct svm3_state_s *s,
                          int32_t *c0, int32_t *c1, int32_t *c2);

/* Field Oriented control */

void foc_vbase_update(FAR struct foc_data_s *foc, float vbase);
void foc_idq_ref_set(FAR struct foc_data_s *data, float d, float q);

void foc_init(FAR struct foc_data_s *data,
              float id_kp, float id_ki, float iq_kp, float iq_ki);
void foc_process(FAR struct foc_data_s *foc,
                 FAR abc_frame_t *i_abc,
                 FAR phase_angle_t *angle);

/* BLDC/PMSM motor observers */

void motor_observer_init(FAR struct motor_observer_s *observer,
                         FAR void *ao, FAR void *so, float per);
float motor_observer_speed_get(FAR struct motor_observer_s *o);
float motor_observer_angle_get(FAR struct motor_observer_s *o);

void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
                             float kslide, float err_max);
void motor_observer_smo(FAR struct motor_observer_s *o,
                        FAR ab_frame_t *i_ab, FAR ab_frame_t *v_ab,
                        FAR struct motor_phy_params_s *phy, float dir);

void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
                              uint8_t samples, float filer, float per);
void motor_sobserver_div(FAR struct motor_observer_s *o,
                         float angle, float dir);

/* Motor openloop control */

void motor_openloop_init(FAR struct openloop_data_s *op,
                         float max, float per);
void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir);
float motor_openloop_angle_get(FAR struct openloop_data_s *op);

/* Motor angle */

void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p);
void motor_angle_e_update(FAR struct motor_angle_s *angle,
                          float angle_new, float dir);
void motor_angle_m_update(FAR struct motor_angle_s *angle,
                          float angle_new, float dir);
float motor_angle_m_get(FAR struct motor_angle_s *angle);
float motor_angle_e_get(FAR struct motor_angle_s *angle);

/* Motor physical parameters functions */

void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
                            float res, float ind);
void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy,
                               float res_alpha, float res_temp_ref);

#undef EXTERN
#if defined(__cplusplus)
}
#endif

#endif /* __INCLUDE_DSP_H */