/*
 * detect_phase_break.c
 *
 *  Created on: 10 дек. 2020 г.
 *      Author: star
 */

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

#include <detect_phase_break.h>


#define CONST_IQ_2PI  105414357 // 360 градусов
#define CONST_IQ_PI2   26353589  // 90 градусов


static void clear_alg_vars(BREAK_PHASE_I *v);
static int calc_direction(BREAK_PHASE_I *v);
static int calc_error_if_break(BREAK_PHASE_I *v, int num, int field_direction);

//Функция возвращает номер канада, по определился обрыв фазы
// 0 - нет обрыва
// 1- фаза U
// 2- фаза V
// 3- фаза W
int calc_break_I_phase(BREAK_PHASE_I *v) {

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

    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 clear_alg_vars(BREAK_PHASE_I *v) {
    int i = 0;
    for (i = 0; i < 3; i++) {
        v->latch_break_start[i] = 0;
        v->latched_teta[i] = 0;
    }
}

int calc_direction(BREAK_PHASE_I *v) {
    int direction = 1;
    if (v->teta > v->prev_teta) {
        if (v->teta - v->prev_teta < CONST_IQ_PI2) { direction = 1;}
        else { direction = 0;}
    } else {
        if (v->prev_teta - v->teta < CONST_IQ_2PI) { direction = 0;}
        else { direction = 1;}
    }
    v->prev_teta = v->teta;
    return direction;
}

int calc_error_if_break(BREAK_PHASE_I *v, int num, int field_direction) {
    int err = 0;
    if (v->latch_break_start[num] == 0) {
        v->latch_break_start[num] = 1;
        v->latched_teta[num] = v->teta;
    } else {
        if (field_direction == 0) {
            if (v->latched_teta[num] > v->teta) {
                err = v->latched_teta[num] - v->teta > CONST_IQ_PI2 ? num + 1 : 0;
            } else {
                err = v->teta - v->latched_teta[num] < CONST_IQ_PI2 - CONST_IQ_2PI ? num + 1 : 0;
            }
        } else {
            if (v->latched_teta[num] < v->teta) {
                err = v->teta - v->latched_teta[num] > CONST_IQ_PI2 ? num + 1 : 0;
            } else {
                err = v->latched_teta[num] - v->teta < CONST_IQ_PI2 - CONST_IQ_2PI ? num + 1 : 0;
            }
        }
    }
    return err;
}