File content as of revision 0:08606a13a816:

#include "MATH1.h"
#define pi  3.14159265359
double mean(double *d, int len)
{
    double mean = 0.0;
    double sum = 0.0;

    for(int i = 0 ; i<len ; i++) {
        sum += d[i];
    }

    mean = sum / len ;
    return mean;
}

double MAXof3(double one, double two, double three)
{
    double MAXMAX = one;
    double yoyoyo[2]= {two, three};
    for(int i = 0; i<2 ; i++) {
        if(yoyoyo[i]>MAXMAX) {
            MAXMAX = yoyoyo[i];
        }
    }
    return MAXMAX;
}

///square///
double MATH::sqr(double value)
{
    double sqrvalue = value * value;
    return sqrvalue;
}


///line voltage RMS///
double MATH::LVRMS(double* d1, double* d2,  uint16_t len)
{
    double sum = 0.0;
    for (int i = 0; i < len; i++) {
        sum += sqr((d1[i]-d2[i]));
    }
    double RMS = sqrt(sum/len);
    return RMS;
}

///RMS///
double MATH::RMS(double* d, uint16_t len)
{
    double sum = 0.0;
    for (int i = 0; i < len; i++) {
        sum += d[i]*d[i];
    }
    double rms = sqrt(sum/len)*1.0;
    return rms;
}


///unbalance rate///
double MATH::UR(double d1, double d2, double d3)
{
    double abc_avg = (d1+d2+d3) / 3.0;
    double x[3] = {fabs(d1 - abc_avg), fabs(d2 - abc_avg), fabs(d3 - abc_avg)};
    double dmax = 0.0;
    for(int i=0 ; i<3 ; i++) {
        if(x[i]>dmax) {
            dmax=x[i];
        }
    }
    double URv = dmax / abc_avg * 100.0;
    return URv;
}


///angle(rad)///
double MATH::angle(double d1, double d2)
{

    double sqrt_2 = sqrt(2.0);
    if (d1 > 0.0 ) {
        if(d2 > 0.0) {
            double rad = fabs(atan(d2 / d1));
            return (0.0 + rad);
        } else {
            double rad = fabs(atan(d2 / d1));
            return (0.0 - rad);
        }
    } else {
        if(d2 > 0.0) {
            double rad = fabs(atan(d2 / d1));
            return (pi - rad);
        } else {
            double rad = fabs(atan(d2 / d1));
            return (pi + rad);
        }
    }
}

///Unbalance Factor///
double MATH::UF(double rms_a, double rms_b, double rms_c, double angle_a, double angle_b, double angle_c)
{
    double a = 120.0 * pi / 180 ;      /// degree ->  rad
    double ang_v1_1 = angle_b + a*2.0 ;
    double ang_v1_2 = angle_c + a;
    double ang_v2_1 = angle_b + a;
    double ang_v2_2 = angle_c + a*2.0;

    double v1 = sqrt(sqr((rms_a * cos(angle_a)) + (rms_b * cos(ang_v1_1)) + (rms_c * cos(ang_v1_2)))
                     + sqr((rms_a * sin(angle_a)) + (rms_b * sin(ang_v1_1)) + (rms_c * sin(ang_v1_2))));
    double v2 = sqrt(sqr((rms_a * cos(angle_a)) + (rms_b * cos(ang_v2_1)) + (rms_c * cos(ang_v2_2)))
                     + sqr((rms_a * sin(angle_a)) + (rms_b * sin(ang_v2_1)) + (rms_c * sin(ang_v2_2))));

    return (v2/v1)*100.0 ;
}

///DFT///
double MATH::doDFT(double *input, double *output, int N)
{
    for (int i=0 ; i<N ; i++) {
        double re = 0;
        double im = 0;
        for(int j=0 ; j<N ; j++) {
            re += input[j]*cos(2*pi*i*j/N);
            im += input[j]*(-sin(2*pi*i*j/N));
        }
        output[i] = sqrt(sqr(re) + sqr(im))/N;
    }
    return *output;

}


void MATH::integal(double *in, double *out, int len, int sps)
{

    double t = 1.0 /sps ;
    double  mean;
    double sum = 0.0;
    out[0] = 0 ;
    double v = 0;

    for(int i = 1 ; i < len ; i++) {
        v += ((in[i - 1] + in[i]) * t / 2.0);       ///   m/s --> *1000 mm/s --> *1000 *1000 um/s
        out[i] = v;
    }

    mean = mean(out, len);

    for(int i = 0 ; i<len ; i++) {
        out[i] = ( out[i] - mean ) * 1000;
    }


}

void MATH::detrend(double *y, int len, int sps)
{
    int i;
    double t = 1.0 /sps ;

    double sum_k1 = 0.0, sum_k2 = 0.0, xbar = 0.0, ybar = 0.0;

    double *x, *d1, *d2;
    x = (double*) malloc(sizeof(double) * len);
    d1 = (double*) malloc(sizeof(double) * len);
    d2 = (double*) malloc(sizeof(double) * len);

    for(i=0 ; i<len ; i++) {
        x[i] = (i+1)*t;
    }

    xbar = mean(x, len);
    ybar = mean(y, len);

    for(i=0 ; i<len ; i++) {
        d1[i] = x[i]-xbar;
        d2[i] = y[i]-ybar;
        sum_k1 += d1[i]*d2[i];
        sum_k2 += d1[i]*d1[i];
    }
    
    double A,B;
    A = sum_k1/sum_k2;
    B = ybar-A*xbar;

    double * trend;
    trend = (double*) malloc(sizeof(double) * len);

    for(i=0 ; i<len ; i++) {
        trend[i]=A*x[i] + B;
        y[i]=y[i]-trend[i];
    }

}


double MATH::Peak2Peak(double *in, int len)
{
    int i;
    double outpp;

    double MAX = in[0];
    for(i = 1 ; i < len ; i++) {
        MAX = (in[i] > MAX ? in[i]:MAX ) ;
    }

    double min = in[0];
    for(i = 1 ; i < len ; i++) {
        min = (in[i] < min ? in[i]:min ) ;
    }

    outpp = MAX - min;

    return outpp;
}


double MATH::PeakValue(double *x , double *y, double *z, int len)
{
    double xmax = fabs(x[0]), ymax = fabs(y[0]), zmax = fabs(z[0]);
    for(int i =1; i<len ; i++) {
        if(fabs(x[i]) > xmax) {
            xmax = fabs(x[i]);
        }
        if(fabs(y[i]) > ymax) {
            ymax = fabs(y[i]);
        }
        if(fabs(z[i]) > zmax) {
            zmax = fabs(z[i]);
        }
    }
    double MAX = ymax;
    double xzmax[2]= {xmax, zmax};
    for(int i=0 ; i<2 ; i++) {
        if(xzmax[i] > MAX) {
            MAX = xzmax[i];
        }
    }

    return MAX ;
}


double MATH::UBValue(double *maxi, double *x , double *y, double *z, int len)
{
    double xmin = fabs(x[0]) , ymin = fabs(y[0]) , zmin = fabs(z[0]);
    for(int i = 1 ; i<len ; i++) {
        if(fabs(x[i]) < xmin) {
            xmin = fabs(x[i]);
        }
        if(fabs(y[i]) < ymin) {
            ymin = fabs(y[i]);
        }
        if(fabs(z[i]) < zmin) {
            zmin = fabs(z[i]);
        }
    }
    double MIN = zmin;
    double yxmin[2]= {ymin, xmin};
    for(int i=0 ; i<2 ; i++) {
        if(yxmin[i] < MIN) {
            MIN = yxmin[i];
        }
    }

    double UB = *maxi + 0.5 *( *maxi - MIN ) / ( len - 1 );
    return UB;
}


double MATH::THD(double *ass_1, double *ass_2, double *ass_3, int pu)
{
    double WTF, WTFa, WTFb, WTFc;
    double you_1, you_2, you_3, fu_1, fu_2, fu_3;

    switch(pu) {
        case 10000 :
            you_1  = (sqr(ass_1[120]) + sqr(ass_1[180]) + sqr(ass_1[240]) + sqr(ass_1[300]) + sqr(ass_1[360]) + sqr(ass_1[420]) + sqr(ass_1[480]) + sqr(ass_1[540]) + sqr(ass_1[600]) + sqr(ass_1[660]));
            fu_1 = sqrt(you_1);
            WTFa  = (fu_1 / ass_1[60])*100;

            you_2  = (sqr(ass_2[120]) + sqr(ass_2[180]) + sqr(ass_2[240]) + sqr(ass_2[300]) + sqr(ass_2[360]) + sqr(ass_2[420]) + sqr(ass_2[480]) + sqr(ass_2[540]) + sqr(ass_2[600]) + sqr(ass_2[660]));
            fu_2 = sqrt(you_2);
            WTFb  = (fu_2 / ass_2[60])*100;

            you_3  = (sqr(ass_3[120]) + sqr(ass_3[180]) + sqr(ass_3[240]) + sqr(ass_3[300]) + sqr(ass_3[360]) + sqr(ass_3[420]) + sqr(ass_3[480]) + sqr(ass_3[540]) + sqr(ass_3[600]) + sqr(ass_3[660]));
            fu_3 = sqrt(you_3);
            WTFc  = (fu_3 / ass_3[60])*100;

            WTF = MAXof3(WTFa, WTFb, WTFc);
            break;
        case 8192 :

            double opps_2, opps_3, opps_4, opps_5, opps_6, opps_7, opps_8, opps_9, opps_10, opps_11;

            opps_2 = (ass_1[49] > ass_1[50] ? ass_1[49] :ass_1[50]);
            opps_4 = (ass_1[98] > ass_1[99] ? ass_1[98] :ass_1[99]);
            opps_6 = (ass_1[147] > ass_1[148] ? ass_1[147] :ass_1[148]);
            opps_7 = (ass_1[172] > ass_1[173] ? ass_1[172] :ass_1[173]);
            opps_9 = (ass_1[221] > ass_1[222] ? ass_1[221] :ass_1[222]);
            opps_11 = (ass_1[270] > ass_1[271] ? ass_1[270] :ass_1[271]);
            opps_3 = MAXof3(ass_1[73], ass_1[74], ass_1[75]);
            opps_5 = MAXof3(ass_1[122], ass_1[123], ass_1[124]);
            opps_8 = MAXof3(ass_1[196], ass_1[197], ass_1[198]);
            opps_10 = MAXof3(ass_1[245], ass_1[246], ass_1[247]);

            you_1  = (sqr(opps_2) + sqr(opps_3) + sqr(opps_4) + sqr(opps_5) + sqr(opps_6) + sqr(opps_7) + sqr(opps_8) + sqr(opps_9) + sqr(opps_10) + sqr(opps_11));
            fu_1 = sqrt(you_1);
            WTFa  = (fu_1 / ass_1[24])*100;

            opps_2 = (ass_2[49] > ass_2[50] ? ass_2[49] :ass_2[50]);
            opps_4 = (ass_2[98] > ass_2[99] ? ass_2[98] :ass_2[99]);
            opps_6 = (ass_2[147] > ass_2[148] ? ass_2[147] :ass_2[148]);
            opps_7 = (ass_2[172] > ass_2[173] ? ass_2[172] :ass_2[173]);
            opps_9 = (ass_2[221] > ass_2[222] ? ass_2[221] :ass_2[222]);
            opps_11 = (ass_2[270] > ass_2[271] ? ass_2[270] :ass_2[271]);
            opps_3 = MAXof3(ass_2[73], ass_2[74], ass_2[75]);
            opps_5 = MAXof3(ass_2[122], ass_2[123], ass_2[124]);
            opps_8 = MAXof3(ass_2[196], ass_2[197], ass_2[198]);
            opps_10 = MAXof3(ass_2[245], ass_2[246], ass_2[247]);

            you_2  = (sqr(opps_2) + sqr(opps_3) + sqr(opps_4) + sqr(opps_5) + sqr(opps_6) + sqr(opps_7) + sqr(opps_8) + sqr(opps_9) + sqr(opps_10) + sqr(opps_11));
            fu_2 = sqrt(you_2);
            WTFb  = (fu_2 / ass_2[24])*100;

            opps_2 = (ass_3[49] > ass_3[50] ? ass_3[49] :ass_3[50]);
            opps_4 = (ass_3[98] > ass_3[99] ? ass_3[98] :ass_3[99]);
            opps_6 = (ass_3[147] > ass_3[148] ? ass_3[147] :ass_3[148]);
            opps_7 = (ass_3[172] > ass_3[173] ? ass_3[172] :ass_3[173]);
            opps_9 = (ass_3[221] > ass_3[222] ? ass_3[221] :ass_3[222]);
            opps_11 = (ass_3[270] > ass_3[271] ? ass_3[270] :ass_3[271]);
            opps_3 = MAXof3(ass_3[73], ass_3[74], ass_3[75]);
            opps_5 = MAXof3(ass_3[122], ass_3[123], ass_3[124]);
            opps_8 = MAXof3(ass_3[196], ass_3[197], ass_3[198]);
            opps_10 = MAXof3(ass_3[245], ass_3[246], ass_3[247]);

            you_3  = (sqr(opps_2) + sqr(opps_3) + sqr(opps_4) + sqr(opps_5) + sqr(opps_6) + sqr(opps_7) + sqr(opps_8) + sqr(opps_9) + sqr(opps_10) + sqr(opps_11));
            fu_3 = sqrt(you_3);
            WTFc  = (fu_3 / ass_3[24])*100;

            WTF = MAXof3(WTFa, WTFb, WTFc);
            break;
    }

    return WTF;
}


double MATH::HD(double *ck_1, double *ck_2, double *ck_3, int pu)
{
    double blow, blow1, blow2, blow3;
    double su_3, su_5, su_7, su_9, su_11;

    switch(pu) {

        case 10000:

            su_3 = (ck_1[180] / ck_1[60])*100;
            su_5 = (ck_1[300] / ck_1[60])*100;
            su_7 = (ck_1[420] / ck_1[60])*100;
            su_9 = (ck_1[540] / ck_1[60])*100;
            su_11 = (ck_1[660] / ck_1[60])*100;

            blow1 = su_3;
            double job[4]= {su_5, su_7, su_9, su_11};
            for(int i = 0 ; i<4 ; i++) {
                if(job[i] > blow1) {
                    blow1 = job[i];
                }
            }

            su_3 = (ck_2[180] / ck_2[60])*100;
            su_5 = (ck_2[300] / ck_2[60])*100;
            su_7 = (ck_2[420] / ck_2[60])*100;
            su_9 = (ck_2[540] / ck_2[60])*100;
            su_11 = (ck_2[660] / ck_2[60])*100;

            blow2 = su_3;
            double job2[4]= {su_5, su_7, su_9, su_11};
            for(int i = 0 ; i<4 ; i++) {
                if(job2[i] > blow2) {
                    blow2 = job2[i];
                }
            }

            su_3 = (ck_3[180] / ck_3[60])*100;
            su_5 = (ck_3[300] / ck_3[60])*100;
            su_7 = (ck_3[420] / ck_3[60])*100;
            su_9 = (ck_3[540] / ck_3[60])*100;
            su_11 = (ck_3[660] / ck_3[60])*100;

            blow3 = su_3;
            double job3[4]= {su_5, su_7, su_9, su_11};
            for(int i = 0 ; i<4 ; i++) {
                if(job3[i] > blow3) {
                    blow3 = job3[i];
                }
            }

            blow = MAXof3(blow1, blow2, blow3);

            break;

        case 8192 :

            double opps_3, opps_5, opps_7, opps_9, opps_11;
            opps_3 = MAXof3(ck_1[73], ck_1[74], ck_1[75]);
            opps_5 = MAXof3(ck_1[122], ck_1[123], ck_1[124]);
            opps_7 = (ck_1[172] > ck_1[173] ? ck_1[172] :ck_1[173]);
            opps_9 = (ck_1[221] > ck_1[222] ? ck_1[221] :ck_1[222]);
            opps_11 = (ck_1[270] > ck_1[271] ? ck_1[270] :ck_1[271]);


            su_3 = (opps_3 / ck_1[24])*100;
            su_5 = (opps_5 / ck_1[24])*100;
            su_7 = (opps_7 / ck_1[24])*100;
            su_9 = (opps_9 / ck_1[24])*100;
            su_11 = (opps_11 / ck_1[24])*100;

            blow1 = su_3;
            double job4[4]= {su_5, su_7, su_9, su_11};
            for(int i = 0 ; i<4 ; i++) {
                if(job4[i] > blow1) {
                    blow1 = job4[i];
                }
            }


            opps_3 = MAXof3(ck_2[73], ck_2[74], ck_2[75]);
            opps_5 = MAXof3(ck_2[122], ck_2[123], ck_2[124]);
            opps_7 = (ck_2[172] > ck_2[173] ? ck_2[172] :ck_2[173]);
            opps_9 = (ck_2[221] > ck_2[222] ? ck_2[221] :ck_2[222]);
            opps_11 = (ck_2[270] > ck_2[271] ? ck_2[270] :ck_2[271]);

            su_3 = (opps_3 / ck_2[24])*100;
            su_5 = (opps_5 / ck_2[24])*100;
            su_7 = (opps_7 / ck_2[24])*100;
            su_9 = (opps_9 / ck_2[24])*100;
            su_11 = (opps_11 / ck_2[24])*100;

            blow2 = su_3;
            double job5[4]= {su_5, su_7, su_9, su_11};

            for(int i = 0 ; i<4 ; i++) {
                if(job5[i] > blow2) {
                    blow2 = job5[i];
                }
            }


            opps_3 = MAXof3(ck_3[73], ck_3[74], ck_3[75]);
            opps_5 = MAXof3(ck_3[122], ck_3[123], ck_3[124]);
            opps_7 = (ck_3[172] > ck_3[173] ? ck_3[172] :ck_3[173]);
            opps_9 = (ck_3[221] > ck_3[222] ? ck_3[221] :ck_3[222]);
            opps_11 = (ck_3[270] > ck_3[271] ? ck_3[270] :ck_3[271]);

            su_3 = (opps_3 / ck_3[24])*100;
            su_5 = (opps_5 / ck_3[24])*100;
            su_7 = (opps_7 / ck_3[24])*100;
            su_9 = (opps_9 / ck_3[24])*100;
            su_11 = (opps_11 / ck_3[24])*100;

            blow3 = su_3;
            double job6[4]= {su_5, su_7, su_9, su_11};

            for(int i = 0 ; i<4 ; i++) {
                if(job6[i] > blow3) {
                    blow3 = job6[i];
                }
            }

            blow = MAXof3(blow1, blow2, blow3);

            break;
    }

    return blow;

}