/*
 * detect_phase_break.c
 *
 *  Created on: 10 äåê. 2020 ã.
 *      Author: star
 */

#include "IQmathLib.h"

//#include "DSP281x_Examples.h"   // DSP281x Examples Include File
//#include "DSP281x_Device.h"     // DSP281x Headerfile Include File

#include "detect_phase_break2.h"


#define CONST_IQ_2PI  	105414357 // 2*pi 360 ãðàäóñîâ
#define CONST_IQ_3_2PI   79060767 // 4/3 pi 270 ãðàäóñîâ
#define CONST_IQ_PI2   	 26353589  // 90 ãðàäóñîâ



void check_brocken_phase(BREAK2_PHASE *v)
{
    int i;
    int ph_a=0, ph_b=0, ph_c=0;
    _iq plus_a, max_a;

    if (    (v->iqCh[0] >= v->iqCh[1] && v->iqCh[0] < v->iqCh[2])
         || (v->iqCh[0] >= v->iqCh[2] && v->iqCh[0] < v->iqCh[1]) )
        ph_a = 1;
    else
        ph_a = -1;

    if (    (v->iqCh[1] >= v->iqCh[0] && v->iqCh[1] < v->iqCh[2])
         || (v->iqCh[1] >= v->iqCh[2] && v->iqCh[1] < v->iqCh[0]) )
        ph_b = 1;
    else
        ph_b = -1;

    if (   (v->iqCh[2] >= v->iqCh[0] && v->iqCh[2] < v->iqCh[1])
        || (v->iqCh[2] >= v->iqCh[1] && v->iqCh[2] < v->iqCh[0]) )
        ph_c = 1;
    else
        ph_c = -1;


 //   plus_a = _IQ(360.0/v->config.freq_pwm * v->freq_signal);
	plus_a = _IQmpy(v->config.calc_const, v->freq_signal);
	

    v->sum_brocken_out[0] += plus_a*ph_a;
    v->sum_brocken_out[1] += plus_a*ph_b;
    v->sum_brocken_out[2] += plus_a*ph_c;

	v->plus_a = plus_a;

    for (i=0;i<3;i++)
    {
        if (v->sum_brocken_out[i]>=CONST_IQ_2PI) v->sum_brocken_out[i] = CONST_IQ_2PI;
        if (v->sum_brocken_out[i]<=0) v->sum_brocken_out[i] = 0;

        if (v->sum_brocken_out[i]>CONST_IQ_3_2PI)
            v->return_brocken_code |= (1<<i);
    }

	max_a = 0;
	if (v->sum_brocken_out[0]>=max_a) max_a = v->sum_brocken_out[0];
	if (v->sum_brocken_out[1]>=max_a) max_a = v->sum_brocken_out[1];
	if (v->sum_brocken_out[2]>=max_a) max_a = v->sum_brocken_out[2];
	v->sum_brocken_out[3] = max_a; // íàøëè ìàêñèìóì

}



void check_i_out_brocken(float freq)
{



}





//Ôóíêöèß âîçâðàùàåò íîìåð êàíàäà, ïî îïðåäåëèëñß îáðûâ ôàçû
// 0 - íåò îáðûâà
// 1- ôàçà U
// 2- ôàçà V
// 3- ôàçà W
int calc_break2_phase(BREAK2_PHASE *v) {

//    int field_direction = 1; //1 - forward, 0 - reverse
    int err = 0;

    if (v->freq_signal==0)
    {
        v->sum_brocken_out[0] = 0;
        v->sum_brocken_out[1] = 0;
        v->sum_brocken_out[2] = 0;
		v->sum_brocken_out[3] = 0;
        v->brocken_i_out  = 0;
    }
    else
    {
        if (_IQabs(v->iqCh[0])>v->config.minimal_level
                || _IQabs(v->iqCh[1])>v->config.minimal_level
                || _IQabs(v->iqCh[2])>v->config.minimal_level )
        {
            check_brocken_phase(v);
        }
        else
        {

            v->iqCh[0] = 0;
            v->iqCh[1] = 0;
            v->iqCh[2] = 0;

            check_brocken_phase(v);

        }
    }

//    if (brocken_i_out & 0x1)
//        error.power_errors.bit.phase_a_brocken |= 1;
//    if (brocken_i_out & 0x2)
//        error.power_errors.bit.phase_b_brocken |= 1;
//    if (brocken_i_out & 0x4)
//        error.power_errors.bit.phase_c_brocken |= 1;
//
//    if(is_errors()) set_err_state();


//
//    if (v->teta > CONST_IQ_2PI) {
//        v->teta = CONST_IQ_2PI;
//    }
//    if(v->teta < 0) {
//        v->teta = 0;
//    }
//    field_direction = calc_direction(v);
//    if (v->iqImod < v->config.iqLevelZero) {
//        clear_alg_vars(v);
//        return 0;
//    }
//
//    if (_IQabs(v->iqIu) < v->config.iqLevelZero &&
//            _IQabs(v->iqIv + v->iqIw) < v->config.iqLevelZero &&
//            _IQabs(v->iqIv) > v->config.iqLevelZero && _IQabs(v->iqIw) > v->config.iqLevelZero) {
//        err = calc_error_if_break(v, 0, field_direction);
//    } else {
//        v->latch_break_start[0] = 0;
//    }
//    if (_IQabs(v->iqIv) < v->config.iqLevelZero &&
//            _IQabs(v->iqIu + v->iqIw) < v->config.iqLevelZero &&
//            _IQabs(v->iqIu) > v->config.iqLevelZero && _IQabs(v->iqIw) > v->config.iqLevelZero) {
//        err = calc_error_if_break(v, 1, field_direction);
//    } else {
//        v->latch_break_start[1] = 0;
//    }
//    if (_IQabs(v->iqIw) < v->config.iqLevelZero &&
//            _IQabs(v->iqIv + v->iqIu) < v->config.iqLevelZero &&
//            _IQabs(v->iqIv) > v->config.iqLevelZero && _IQabs(v->iqIu) > v->config.iqLevelZero) {
//        err = calc_error_if_break(v, 2, field_direction);
//    } else {
//        v->latch_break_start[2] = 0;
//    }

    return err;
}




void init_break2_phase(BREAK2_PHASE *v)
{
	v->config.iq_freq = _IQ(v->config.freq_pwm / v->config.norma_freq);
	v->config.calc_const = 	_IQdiv(CONST_IQ_2PI, v->config.iq_freq);
	v->return_brocken_code = 0;


}

void clear_break2_phase(BREAK2_PHASE *v)
{

            v->iqCh[0] = 0;
            v->iqCh[1] = 0;
            v->iqCh[2] = 0;
        	v->sum_brocken_out[0] = 0;
        	v->sum_brocken_out[1] = 0;
        	v->sum_brocken_out[2] = 0;
			v->sum_brocken_out[3] = 0;
        	v->brocken_i_out  = 0;

			v->return_brocken_code = 0;
}