/**********************************************************************
 * © 2014 Microchip Technology Inc.
 *
 * SOFTWARE LICENSE AGREEMENT:
 * Microchip Technology Incorporated ("Microchip") retains all ownership and
 * intellectual property rights in the code accompanying this message and in all
 * derivatives hereto.  You may use this code, and any derivatives created by
 * any person or entity by or on your behalf, exclusively with Microchip's
 * proprietary products.  Your acceptance and/or use of this code constitutes
 * agreement to the terms and conditions of this notice.
 *
 * CODE ACCOMPANYING THIS MESSAGE IS SUPPLIED BY MICROCHIP "AS IS".  NO
 * WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE APPLY TO THIS CODE, ITS INTERACTION WITH MICROCHIP'S
 * PRODUCTS, COMBINATION WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION.
 *
 * YOU ACKNOWLEDGE AND AGREE THAT, IN NO EVENT, SHALL MICROCHIP BE LIABLE, WHETHER
 * IN CONTRACT, WARRANTY, TORT (INCLUDING NEGLIGENCE OR BREACH OF STATUTORY DUTY),
 * STRICT LIABILITY, INDEMNITY, CONTRIBUTION, OR OTHERWISE, FOR ANY INDIRECT, SPECIAL,
 * PUNITIVE, EXEMPLARY, INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, FOR COST OR EXPENSE OF
 * ANY KIND WHATSOEVER RELATED TO THE CODE, HOWSOEVER CAUSED, EVEN IF MICROCHIP HAS BEEN
 * ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE.  TO THE FULLEST EXTENT
 * ALLOWABLE BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY RELATED TO
 * THIS CODE, SHALL NOT EXCEED THE PRICE YOU PAID DIRECTLY TO MICROCHIP SPECIFICALLY TO
 * HAVE THIS CODE DEVELOPED.
 *
 * You agree that you are solely responsible for testing the code and
 * determining its suitability.  Microchip has no obligation to modify, test,
 * certify, or support the code.
 *
 ******************************************************************************/
#ifndef smcpos_H
#define smcpos_H

#include "user_parms.h"

typedef struct
{
    int16_t vAlpha; /* Input: Stationary alfa-axis stator voltage */
    int16_t eAlpha; /* Variable: Stationary alfa-axis back EMF */
    int16_t eAlphaFinal; /* Variable: Filtered EMF for Angle calculation */
    int16_t zAlpha; /* Output: Stationary alfa-axis sliding control */
    int16_t gsmopos; /* Parameter: Motor dependent control gain*/
    int16_t estIalpha; /* Variable: Estimated stationary alfa-axis stator current */
    int16_t fsmopos; /* Parameter: Motor dependent plant matrix */
    int16_t vBeta; /* Input: Stationary beta-axis stator voltage */
    int16_t eBeta; /* Variable: Stationary beta-axis back EMF */
    int16_t eBetaFinal; /* Variable: Filtered EMF for Angle calculation */
    int16_t zBeta; /* Output: Stationary beta-axis sliding control */
    int16_t estIbeta; /* Variable: Estimated stationary beta-axis stator current */
    int16_t iAlpha; /* Input: Stationary alfa-axis stator current */
    int16_t iAlphaError; /* Variable: Stationary alfa-axis current error */
    int16_t kSlide; /* Parameter: Sliding control gain */
    int16_t maxSMCError; /* Parameter: Maximum current error for linear SMC */
    int16_t iBeta; /* Input: Stationary beta-axis stator current */
    int16_t iBetaError; /* Variable: Stationary beta-axis current error */
    int16_t kslf; /* Parameter: Sliding control filter gain */
    int16_t kslfFinal; /* Parameter: BEMF Filter for angle calculation */
    int16_t filtOmCoef; /* Parameter: Filter Coef for Omega filtered calc */
    int16_t thetaOffset; /* Output: Offset used to compensate rotor angle */
    int16_t theta; /* Output: Compensated rotor angle */
    int16_t omega; /* Output: Rotor speed */
    int16_t omegaFltred; /* Output: Filtered Rotor speed for speed PI */
} SMC;

typedef SMC *smcHandle;

#define SMC_DEFAULTS {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}

/* Define this in RPMs */

#define SPEED0 MIN_SPEED_IN_RPM
#define SPEED1 (SPEED0 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED2 (SPEED1 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED3 (SPEED2 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED4 (SPEED3 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED5 (SPEED4 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED6 (SPEED5 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED7 (SPEED6 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED8 (SPEED7 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED9 (SPEED8 + (int16_t)((FIELDWEAK_SPEED_RPM - MIN_SPEED_IN_RPM) / 10.0))
#define SPEED10 (FIELDWEAK_SPEED_RPM)

/* Define this in Degrees, from 0 to 360 */

#define THETA_AT_ALL_SPEED 90

#define OMEGA0 (float)(SPEED0 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA1 (float)(SPEED1 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA2 (float)(SPEED2 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA3 (float)(SPEED3 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA4 (float)(SPEED4 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA5 (float)(SPEED5 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA6 (float)(SPEED6 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA7 (float)(SPEED7 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA8 (float)(SPEED8 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA9 (float)(SPEED9 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGA10 (float)(SPEED10 * LOOPTIME_IN_SEC * \
                IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)


#define OMEGANOMINAL	(float)(NOMINAL_SPEED_IN_RPM * LOOPTIME_IN_SEC * \
                		IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)
#define OMEGAFIELDWK	(float)(FIELDWEAK_SPEED_RPM * LOOPTIME_IN_SEC * \
                		IRP_PERCALC * POLE_PAIRS * 2.0 / 60.0)

#define THETA_ALL (uint16_t)(THETA_AT_ALL_SPEED / 180.0 * 32768.0)
#define CONSTANT_PHASE_SHIFT THETA_ALL


#define _PI 3.1416

void SMC_Position_Estimation(smcHandle);
void SMCInit(smcHandle);
void CalcEstI(void);
void CalcIError(void);
void CalcZalpha(void);
void CalcZbeta(void);
void CalcBEMF(void);
void CalcOmegaFltred(void);
int16_t atan2CORDIC(int16_t, int16_t);
int16_t Q15SQRT(int16_t);

extern int16_t prevTheta;
extern int16_t accumTheta;
extern uint16_t accumThetaCnt;
int16_t FracMpy(int16_t mul_1, int16_t mul_2);
int16_t FracDiv(int16_t num_1, int16_t den_1);
void Delay(uint16_t t);
#endif
