#include "ZDistA_wrapper.h"
#include "ZDistA_komp.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>

// Dodatkowe includy potrzebne do kompilacji
#include "analog_in.h"

// Funkcja pomocnicza do odczytu stanu binary_io
static inline int get_binary_io_val(struct binary_io *b) {
    if (!b || !b->ptr) return 0;
    return (*b->ptr & b->bit_mask) != 0;
}

// Definicja "uchwytu". Ta struktura przechowuje wszystko, co jest potrzebne do działania algorytmu.
// Jest niewidoczna dla Pythona.
struct ZDistA_handle {
    // Główna struktura logiczna z oryginalnego kodu
    struct ZDistA_komp_logic logic;

    // Pamięć na dane wejściowe (fazory) - wskaźniki w 'logic' będą wskazywać tutaj
    float u_re[3], u_im[3];
    float i_re[3], i_im[3];
    float u12_re, u12_im, u23_re, u23_im, u31_re, u31_im;
    float u_mag[3], i_mag[3], u12_mag, u23_mag, u31_mag;
    
    // Składowe symetryczne
    float su1_re, su1_im, su1_mag;
    float su0_re, su0_im, su0_mag;
    float su2_re, su2_im, su2_mag;
    float si1_re, si1_im, si1_mag;
    float si0_re, si0_im, si0_mag;
    float si2_re, si2_im, si2_mag;

    // Dummy analog_in_params, ponieważ nie mamy pełnego środowiska
    struct analog_in_params param_i;
    struct analog_in_params param_u;

    // Struktura z pustymi wskaźnikami, bo nie jest używana w tym trybie
    struct ZDistA_komp_args args;
};

// Funkcja pomocnicza do ustawiania wskaźników w strukturze 'logic'
static void setup_pointers(ZDistA_handle* h) {
    struct ZDistA_komp_logic* log = &h->logic;

    // Ustawienie wskaźników na dane wejściowe
    log->U1_orta = &h->u_re[0]; log->U1_ortb = &h->u_im[0]; log->U1 = &h->u_mag[0];
    log->U2_orta = &h->u_re[1]; log->U2_ortb = &h->u_im[1]; log->U2 = &h->u_mag[1];
    log->U3_orta = &h->u_re[2]; log->U3_ortb = &h->u_im[2]; log->U3 = &h->u_mag[2];
    
    log->I1_orta = &h->i_re[0]; log->I1_ortb = &h->i_im[0]; log->I1 = &h->i_mag[0];
    log->I2_orta = &h->i_re[1]; log->I2_ortb = &h->i_im[1]; log->I2 = &h->i_mag[1];
    log->I3_orta = &h->i_re[2]; log->I3_ortb = &h->i_im[2]; log->I3 = &h->i_mag[2];

    // Napięcia międzyfazowe
    log->U12_orta = &h->u12_re; log->U12_ortb = &h->u12_im; log->U12 = &h->u12_mag;
    log->U23_orta = &h->u23_re; log->U23_ortb = &h->u23_im; log->U23 = &h->u23_mag;
    log->U31_orta = &h->u31_re; log->U31_ortb = &h->u31_im; log->U31 = &h->u31_mag;

    // Składowe symetryczne
    log->sU1_orta = &h->su1_re; log->sU1_ortb = &h->su1_im; log->sU1 = &h->su1_mag;
    log->sU0_orta = &h->su0_re; log->sU0_ortb = &h->su0_im; log->sU0 = &h->su0_mag;
    log->sU2_orta = &h->su2_re; log->sU2_ortb = &h->su2_im; log->sU2 = &h->su2_mag;
    
    log->sI1_orta = &h->si1_re; log->sI1_ortb = &h->si1_im; log->sI1 = &h->si1_mag;
    log->sI0_orta = &h->si0_re; log->sI0_ortb = &h->si0_im; log->sI0 = &h->si0_mag;
    log->sI2_orta = &h->si2_re; log->sI2_ortb = &h->si2_im; log->sI2 = &h->si2_mag;

    // "Dummy" wskaźniki, aby uniknąć błędów
    log->param_I = &h->param_i;
    log->param_U = &h->param_u;
    
    // Inne wskaźniki, które muszą być zainicjowane
    log->dw.log_ptr = log;
    log->nast_.wyjscie.Zdist_dw = &log->dw;
    log->nast_.wyjscie.on = &log->nast_.on_;
}

DLL_EXPORT ZDistA_handle* ZDistA_init(double z1_r, double z1_x, double line_angle_deg, int kierunek) {
    ZDistA_handle* h = (ZDistA_handle*)malloc(sizeof(ZDistA_handle));
    if (!h) return NULL;

    memset(h, 0, sizeof(ZDistA_handle));
    setup_pointers(h);

    struct Nastawy_przeliczone_ZDistA_komp* nast = &h->logic.nast_;
    nast->on_ = 1; // Włączamy logikę
    
    // Ustawiamy parametry przekładni (hardkodowane, jak w tester.py)
    h->param_u.znam_wtor = 1.0; // Wartości znormalizowane
    h->param_i.znam_wtor = 1.0;
    nast->wyjscie.przekladnia = 1.0;

    // Ustawienia charakterystyk - wzorowane na logice z distance_algorithm_zimba.py
    nast->tanfi1 = tan(line_angle_deg * (3.14159265 / 180.0));
    nast->tanfi2 = tan(85.0 * (3.14159265 / 180.0)); // Typowa wartość

    // Strefa 1
    nast->typ[1] = 0; // Poligonalna
    nast->kierunek[1] = kierunek;
    nast->n_pol[1][0].Rr = z1_r;      // LE
    nast->n_pol[1][0].Xr = z1_x;      // LE
    nast->n_pol[1][1].Rr = z1_r;      // LL
    nast->n_pol[1][1].Xr = z1_x;      // LL

    // Strefy 2-5 z rozsądnymi wartościami domyślnymi
    for (int i = 2; i < 6; ++i) {
        nast->typ[i] = 0;
        nast->kierunek[i] = kierunek;
        nast->n_pol[i][0].Rr = z1_r * (1.0 + (i-1)*0.5);
        nast->n_pol[i][0].Xr = z1_x * (1.0 + (i-1)*0.5);
        nast->n_pol[i][1].Rr = z1_r * (1.0 + (i-1)*0.5);
        nast->n_pol[i][1].Xr = z1_x * (1.0 + (i-1)*0.5);
    }
    
    // Inne ważne parametry
    nast->Igr = 0.1 * 0.1; // Próg prądowy
    nast->kp = 1.05; // Współczynnik powrotu
    nast->kpk = 1.05 * 1.05;
	nast->Igrp = nast->Igr * nast->kpk;

    // Przeliczenie wartości powrotnych i tangensów
    for(int i = 0; i<6; ++i) {
        for(int j=0; j<2; ++j) {
            nast->n_pol[i][j].Rp = nast->n_pol[i][j].Rr / nast->kp;
            nast->n_pol[i][j].Xp = nast->n_pol[i][j].Xr / nast->kp;
            nast->n_pol[i][j].Rrtanfi1 = nast->n_pol[i][j].Rr * nast->tanfi1;
            nast->n_pol[i][j].Rptanfi1 = nast->n_pol[i][j].Rp * nast->tanfi1;
        }
    }
    nast->Xr1f = nast->n_pol[1][0].Xr * (1 + nast->tanfi2 / nast->tanfi1);
	nast->Xr1fp = nast->Xr1f / nast->kp;

    return h;
}

DLL_EXPORT int ZDistA_step(
    ZDistA_handle* handle,
    const float u_re[3], const float u_im[3],
    const float i_re[3], const float i_im[3]
) {
    if (!handle) return 0;
    
    // 1. Skopiuj dane wejściowe do wewnętrznych buforów uchwytu
    for (int i = 0; i < 3; ++i) {
        handle->u_re[i] = u_re[i]; handle->u_im[i] = u_im[i];
        handle->i_re[i] = i_re[i]; handle->i_im[i] = i_im[i];
        handle->u_mag[i] = sqrtf(u_re[i]*u_re[i] + u_im[i]*u_im[i]);
        handle->i_mag[i] = sqrtf(i_re[i]*i_re[i] + i_im[i]*i_im[i]);
    }

    // 2. Oblicz wartości, które w oryginale pochodziły z innych modułów
    // Napięcia międzyfazowe
    handle->u12_re = u_re[0] - u_re[1]; handle->u12_im = u_im[0] - u_im[1];
    handle->u23_re = u_re[1] - u_re[2]; handle->u23_im = u_im[1] - u_im[2];
    handle->u31_re = u_re[2] - u_re[0]; handle->u31_im = u_im[2] - u_im[0];
    handle->u12_mag = sqrtf(handle->u12_re*handle->u12_re + handle->u12_im*handle->u12_im);
    handle->u23_mag = sqrtf(handle->u23_re*handle->u23_re + handle->u23_im*handle->u23_im);
    handle->u31_mag = sqrtf(handle->u31_re*handle->u31_re + handle->u31_im*handle->u31_im);

    // Składowe symetryczne (stałe 'a' i 'a2' dla uproszczenia)
    const float a_re = -0.5, a_im = 0.8660254; // sqrt(3)/2
    
    // I0 = (I1+I2+I3)/3
    handle->si0_re = (i_re[0] + i_re[1] + i_re[2]) / 3.0f;
    handle->si0_im = (i_im[0] + i_im[1] + i_im[2]) / 3.0f;
    
    // I1_zg = (I1 + a*I2 + a2*I3)/3
    handle->si1_re = (i_re[0] + (i_re[1]*a_re - i_im[1]*a_im) + (i_re[2]*a_re - i_im[2]*-a_im)) / 3.0f;
    handle->si1_im = (i_im[0] + (i_re[1]*a_im + i_im[1]*a_re) + (i_re[2]*-a_im + i_im[2]*a_re)) / 3.0f;
    
    // I2_pr = (I1 + a2*I2 + a*I3)/3
    handle->si2_re = (i_re[0] + (i_re[1]*a_re - i_im[1]*-a_im) + (i_re[2]*a_re - i_im[2]*a_im)) / 3.0f;
    handle->si2_im = (i_im[0] + (i_re[1]*-a_im + i_im[1]*a_re) + (i_re[2]*a_im + i_im[2]*a_re)) / 3.0f;

    // To samo dla napięć
    handle->su0_re = (u_re[0] + u_re[1] + u_re[2]) / 3.0f;
    handle->su0_im = (u_im[0] + u_im[1] + u_im[2]) / 3.0f;
    handle->su1_re = (u_re[0] + (u_re[1]*a_re - u_im[1]*a_im) + (u_re[2]*a_re - u_im[2]*-a_im)) / 3.0f;
    handle->su1_im = (u_im[0] + (u_re[1]*a_im + u_im[1]*a_re) + (u_re[2]*-a_im + u_im[2]*a_re)) / 3.0f;
    handle->su2_re = (u_re[0] + (u_re[1]*a_re - u_im[1]*-a_im) + (u_re[2]*a_re - u_im[2]*a_im)) / 3.0f;
    handle->su2_im = (u_im[0] + (u_re[1]*-a_im + u_im[1]*a_re) + (u_re[2]*a_im + u_im[2]*a_re)) / 3.0f;

    // Magnitudy składowych symetrycznych
    handle->si0_mag = sqrtf(handle->si0_re*handle->si0_re + handle->si0_im*handle->si0_im);
    handle->si1_mag = sqrtf(handle->si1_re*handle->si1_re + handle->si1_im*handle->si1_im);
    handle->si2_mag = sqrtf(handle->si2_re*handle->si2_re + handle->si2_im*handle->si2_im);
    handle->su0_mag = sqrtf(handle->su0_re*handle->su0_re + handle->su0_im*handle->su0_im);
    handle->su1_mag = sqrtf(handle->su1_re*handle->su1_re + handle->su1_im*handle->su1_im);
    handle->su2_mag = sqrtf(handle->su2_re*handle->su2_re + handle->su2_im*handle->su2_im);

    // 3. Wywołaj główną funkcję logiki
    ZDistA_komp(NULL, &handle->logic);

    // 4. Sprawdź, czy którekolwiek z wyjść jest aktywne
    struct ZDistA_komp_logic* log = &handle->logic;
    if (get_binary_io_val(&log->P1_L1E) || get_binary_io_val(&log->P1_L2E) || get_binary_io_val(&log->P1_L3E) ||
        get_binary_io_val(&log->P1_L1L2) || get_binary_io_val(&log->P1_L2L3) || get_binary_io_val(&log->P1_L3L1)) {
        return 1;
    }

    return 0;
}

DLL_EXPORT void ZDistA_cleanup(ZDistA_handle* handle) {
    if (handle) {
        free(handle);
    }
}