/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            mathtool.c
 Partner Filename:    mathtool.h
 Description:         The math function
 Complier:            IAR Embedded Workbench for ARM 7.80, IAR Systems.
 CPU TYPE :           GD32F3x0
*************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************
*************************************************************************
Revising History (ECL of this file):

************************************************************************/

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
#ifndef _MATHTOOL_C_
#define _MATHTOOL_C_
#endif

/************************************************************************
 Included File:
*************************************************************************/
#include "syspar.h"
#include "user.h"

/************************************************************************
 Constant Table:
*************************************************************************/
static const SWORD mth_swSinTab[SINTABSIZE] = {
    0,      //	0
    804,    //	1
    1608,   //	2
    2411,   //	3
    3212,   //	4
    4011,   //	5
    4808,   //	6
    5602,   //	7
    6393,   //	8
    7180,   //	9
    7962,   //	10
    8740,   //	11
    9512,   //	12
    10279,  //	13
    11039,  //	14
    11793,  //	15
    12540,  //	16
    13279,  //	17
    14010,  //	18
    14733,  //	19
    15447,  //	20
    16151,  //	21
    16846,  //	22
    17531,  //	23
    18205,  //	24
    18868,  //	25
    19520,  //	26
    20160,  //	27
    20788,  //	28
    21403,  //	29
    22006,  //	30
    22595,  //	31
    23170,  //	32
    23732,  //	33
    24279,  //	34
    24812,  //	35
    25330,  //	36
    25833,  //	37
    26320,  //	38
    26791,  //	39
    27246,  //	40
    27684,  //	41
    28106,  //	42
    28511,  //	43
    28899,  //	44
    29269,  //	45
    29622,  //	46
    29957,  //	47
    30274,  //	48
    30572,  //	49
    30853,  //	50
    31114,  //	51
    31357,  //	52
    31581,  //	53
    31786,  //	54
    31972,  //	55
    32138,  //	56
    32286,  //	57
    32413,  //	58
    32522,  //	59
    32610,  //	60
    32679,  //	61
    32729,  //	62
    32758,  //	63
    32767,  //	64
    32758,  //	65
    32729,  //	66
    32679,  //	67
    32610,  //	68
    32522,  //	69
    32413,  //	70
    32286,  //	71
    32138,  //	72
    31972,  //	73
    31786,  //	74
    31581,  //	75
    31357,  //	76
    31114,  //	77
    30853,  //	78
    30572,  //	79
    30274,  //	80
    29957,  //	81
    29622,  //	82
    29269,  //	83
    28899,  //	84
    28511,  //	85
    28106,  //	86
    27684,  //	87
    27246,  //	88
    26791,  //	89
    26320,  //	90
    25833,  //	91
    25330,  //	92
    24812,  //	93
    24279,  //	94
    23732,  //	95
    23170,  //	96
    22595,  //	97
    22006,  //	98
    21403,  //	99
    20788,  //	100
    20160,  //	101
    19520,  //	102
    18868,  //	103
    18205,  //	104
    17531,  //	105
    16846,  //	106
    16151,  //	107
    15447,  //	108
    14733,  //	109
    14010,  //	110
    13279,  //	111
    12540,  //	112
    11793,  //	113
    11039,  //	114
    10279,  //	115
    9512,   //	116
    8740,   //	117
    7962,   //	118
    7180,   //	119
    6393,   //	120
    5602,   //	121
    4808,   //	122
    4011,   //	123
    3212,   //	124
    2411,   //	125
    1608,   //	126
    804,    //	127
    0,      //	128
    -804,   //	129
    -1608,  //	130
    -2411,  //	131
    -3212,  //	132
    -4011,  //	133
    -4808,  //	134
    -5602,  //	135
    -6393,  //	136
    -7180,  //	137
    -7962,  //	138
    -8740,  //	139
    -9512,  //	140
    -10279, //	141
    -11039, //	142
    -11793, //	143
    -12540, //	144
    -13279, //	145
    -14010, //	146
    -14733, //	147
    -15447, //	148
    -16151, //	149
    -16846, //	150
    -17531, //	151
    -18205, //	152
    -18868, //	153
    -19520, //	154
    -20160, //	155
    -20788, //	156
    -21403, //	157
    -22006, //	158
    -22595, //	159
    -23170, //	160
    -23732, //	161
    -24279, //	162
    -24812, //	163
    -25330, //	164
    -25833, //	165
    -26320, //	166
    -26791, //	167
    -27246, //	168
    -27684, //	169
    -28106, //	170
    -28511, //	171
    -28899, //	172
    -29269, //	173
    -29622, //	174
    -29957, //	175
    -30274, //	176
    -30572, //	177
    -30853, //	178
    -31114, //	179
    -31357, //	180
    -31581, //	181
    -31786, //	182
    -31972, //	183
    -32138, //	184
    -32286, //	185
    -32413, //	186
    -32522, //	187
    -32610, //	188
    -32679, //	189
    -32729, //	190
    -32758, //	191
    -32768, //	192
    -32758, //	193
    -32729, //	194
    -32679, //	195
    -32610, //	196
    -32522, //	197
    -32413, //	198
    -32286, //	199
    -32138, //	200
    -31972, //	201
    -31786, //	202
    -31581, //	203
    -31357, //	204
    -31114, //	205
    -30853, //	206
    -30572, //	207
    -30274, //	208
    -29957, //	209
    -29622, //	210
    -29269, //	211
    -28899, //	212
    -28511, //	213
    -28106, //	214
    -27684, //	215
    -27246, //	216
    -26791, //	217
    -26320, //	218
    -25833, //	219
    -25330, //	220
    -24812, //	221
    -24279, //	222
    -23732, //	223
    -23170, //	224
    -22595, //	225
    -22006, //	226
    -21403, //	227
    -20788, //	228
    -20160, //	229
    -19520, //	230
    -18868, //	231
    -18205, //	232
    -17531, //	233
    -16846, //	234
    -16151, //	235
    -15447, //	236
    -14733, //	237
    -14010, //	238
    -13279, //	239
    -12540, //	240
    -11793, //	241
    -11039, //	242
    -10279, //	243
    -9512,  //	244
    -8740,  //	245
    -7962,  //	246
    -7180,  //	247
    -6393,  //	248
    -5602,  //	249
    -4808,  //	250
    -4011,  //	251
    -3212,  //	252
    -2411,  //	253
    -1608,  //	254
    -804,   //	255
    0       //	256
};
static const UWORD mth_uwArctanTab[ARCTANTABSIZE] = {
    8192, // 0
    4836, // 1
    2555, // 2
    1297, // 3
    651,  // 4
    326,  // 5
    163,  // 6
    81,   // 7
    41,   // 8
    20,   // 9
    10,   // 10
    5,    // 11
    3,    // 12
    1,    // 13
    1,    // 14
    0     // 15
};
/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: math_voSinCos;
 Description: SIN & COS table.
 Call by: ...;
 Input Variables: Angle
 Output/Return Variables: SIN, COS
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void mth_voSinCos(UWORD angle, SINCOS *v)
{
    SWORD mth_swThsEntPu, mth_swNxtEntPu, mth_swDltAngPu;
    UWORD mth_uwIpValPu;
    //=======================================================================
    //    Calculate Cos(theta_p)
    //=======================================================================
    mth_swThsEntPu = mth_swSinTab[((angle + 8192) & 0x7fff) >> 7]; // High 8 bit, 32767 to 255      Q15-Q7=Q8
    mth_swNxtEntPu = mth_swSinTab[(((angle + 8192) & 0x7fff) >> 7) + 1];
    mth_swDltAngPu = mth_swNxtEntPu - mth_swThsEntPu;
    mth_uwIpValPu = (angle & 0x007f) * 256; // Low 7 bit, 127 to 32512
    v->swCosPu = ((SLONG)mth_swDltAngPu * mth_uwIpValPu >> 15) + mth_swThsEntPu;
    //=======================================================================
    //    Calculate Sin(theta_p)
    //=======================================================================
    mth_swThsEntPu = mth_swSinTab[(angle & 0x7fff) >> 7]; // High 8 bit, 32767 to 255      Q15-Q7=Q8
    mth_swNxtEntPu = mth_swSinTab[((angle & 0x7fff) >> 7) + 1];
    mth_swDltAngPu = mth_swNxtEntPu - mth_swThsEntPu;
    mth_uwIpValPu = (angle & 0x007f) * 256; // Low 7 bit, 127 to 32512
    v->swSinPu = ((SLONG)mth_swDltAngPu * mth_uwIpValPu >> 15) + mth_swThsEntPu;
}

/***************************************************************
 Function: Lpf;
 Description: LowPass filter cof calculation;
 Call by: repeated functions;
 Input Variables: ulLpfTime_us, uwExeFreq(Hz),uwLPFCof
 Output/Return Variables: SLONG *slpLpfOut
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void mth_voLPFilterCoef(ULONG ulLpfTime_us, ULONG ulExeFreq, UWORD *uwLPFCof)
{
    // y(n) = y(n-1) + T/(T+tao)*[x(n) - y(n-1)] = T/(T+tao)*x(n) + tao/(T+tao)*y(n-1)
    // uwLPFCof = T/(T+tao)  //Q15

    // ulExeFreq range(50Hz,50KHz)
    ULONG ulTCtrl_us; // control time: us

    if (ulExeFreq < 1)
    {
        ulExeFreq = 1;
    }
    ulTCtrl_us = 1000000 / ulExeFreq; // Unit:1us

    if ((ulTCtrl_us < 1) && (ulLpfTime_us < 1))
    {
        ulLpfTime_us = 1;
    }
    *uwLPFCof = (ulTCtrl_us << 15) / (ulTCtrl_us + ulLpfTime_us); // Q15
}
/***************************************************************
 Function: Lpf;
 Description: LowPass filter;
 Call by: repeated functions;
 Input Variables: swLpfIn,uwLPFCof
 Output/Return Variables: SLONG *slpLpfOut
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void mth_voLPFilter(SWORD swLpfIn, LPF_OUT *out)
{
    //    swLpfIn(Q15);
    //    ulLpfTime_us:Unit:1us (Range0.000ms~20000.000ms)
    //    uwExeFreq:Unit:Hz    the frequence of call
    //    slLpfOut

    UWORD       uwKx;
    SLONG_UNION sluTmp;
    SLONG       slTmp1;

    uwKx = out->uwKx; // Q15

    sluTmp.sl = out->slY.sl;

    slTmp1 = sluTmp.sl + (uwKx * swLpfIn << 1) - (uwKx * sluTmp.sw.hi << 1) - (uwKx * sluTmp.sw.low >> 15);

    out->slY.sl = slTmp1; // Q31
}
/***************************************************************
 Function: mth_slSqrt;
 Description: sqrt;
 Call by: repeated functions;
 Input Variables:
 Output/Return Variables:
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
SLONG mth_slSqrt(SLONG slInput)
{
    SLONG slRemainder;
    SLONG slRoot;
    SWORD swN;

    slRoot = 0;
    if (slInput < 0)
    {
        slInput = 0;
    }
    if ((slInput - 2147352578) > 0) // slInput>(32767*32767*2)
    {
        slRoot = 46339;
        return slRoot;
    }
    if (slInput > 0)
    {
        for (swN = 15; swN >= 0; swN--)
        {
            slRemainder = slInput - (SLONG)slRoot * slRoot;

            if (slRemainder > 0)
            {
                slRoot = slRoot + (1 << swN);
            }
            else
            {
                slRoot = slRoot - (1 << swN);
                if (slRoot <= 0)
                {
                    slRoot = 0;
                }
            }
        }
    }
    else
    {
        slRoot = 0;
    }

    return slRoot;
}
/***************************************************************
 Function: mth_slRamp;
 Description: ramp;
 Call by: repeated functions;
 Input Variables:
 Output/Return Variables:
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
SLONG mth_slRamp(SLONG slInput, SLONG slDelta, SLONG slOutputLimit)
{
    SLONG slOutput;

    slOutput = slInput + slDelta;
    if (slOutput > slOutputLimit)
    {
        slOutput = slOutputLimit;
    }
    return slOutput;
}
/***************************************************************
 Function: mth_voArctan;
 Description: arctan;
 Call by: repeated functions;
 Input Variables:
 Output/Return Variables:
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
SWORD mth_voArctan(ARCTAN_IN *in)
{
    UWORD uwQuadrant, uwStep;
    SLONG slX, slY, slCurAngle, slNewX;
    UWORD uwEstThetaPu;
    slCurAngle = 0;

    slX = in->swAlphaPu;
    slY = in->swBetaPu;

    if ((slX == 0) || (slY == 0))
    {
        if (slX == 0)
        {
            if (slY > 0)
            {
                uwEstThetaPu = 8192; // Q15
            }
            else if (slY < 0)
            {
                uwEstThetaPu = 24576; // Q15
            }
            else
            {
                uwEstThetaPu = 0;
            }
        }
        if (slY == 0)
        {
            if (slX > 0)
            {
                uwEstThetaPu = 0; // Q15
            }
            else if (slX < 0)
            {
                uwEstThetaPu = 16384; // Q15
            }
        }
    }
    else
    {
        if ((slX > 0) && (slY > 0)) // Quadrant 0, 0 < angle < 90
        {
            uwQuadrant = 0;
        }
        else if ((slX < 0) && (slY > 0)) // Quadrant 1, 90 < angle < 180
        {
            uwQuadrant = 1;
            slX = -slX;
        }
        else if ((slX < 0) && (slY < 0)) // Quadrant 2, 180 < angle < 270
        {
            uwQuadrant = 2;
            slX = -slX;
            slY = -slY;
        }
        else if ((slX > 0) && (slY < 0)) // Quadrant 3, 270 < angle < 360
        {
            uwQuadrant = 3;
            slY = -slY;
        }

        for (uwStep = 0; uwStep < (ARCTANTABSIZE - 1); uwStep++)
        {
            if (slY > 0)
            {
                slNewX = slX + (slY >> uwStep);
                slY = slY - (slX >> uwStep);
                slX = slNewX;
                slCurAngle += mth_uwArctanTab[uwStep];
            }
            else if (slY < 0)
            {
                slNewX = slX - (slY >> uwStep);
                slY = slY + (slX >> uwStep);
                slX = slNewX;
                slCurAngle -= mth_uwArctanTab[uwStep];
            }
            else
            {
                slCurAngle = slCurAngle;
            }
        }

        switch (uwQuadrant)
        {
        case 0:
            slCurAngle = slCurAngle;
            break;
        case 1:
            slCurAngle = 32767 - slCurAngle;
            break;
        case 2:
            slCurAngle = 32767 + slCurAngle;
            break;
        case 3:
            slCurAngle = 65534 - slCurAngle;
            break;
        }

        slCurAngle = slCurAngle >> 1;

        if (slCurAngle > 32767)
        {
            slCurAngle = 32767;
        }
        else if (slCurAngle < 0)
        {
            slCurAngle = slCurAngle + 32768;
        }

        uwEstThetaPu = (UWORD)slCurAngle;
    }
    return uwEstThetaPu;
}
/************************************************************************
 Local Functions: N/A
*************************************************************************/

/*************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co., Ltd.
 All rights reserved.
*************************************************************************/
#ifdef _MATHTOOL_C_
#undef _MATHTOOL_C_
#endif
/*************************************************************************
 End of this File (EOF):
 Do not put anything after this part!
*************************************************************************/