#include "CurrentSense.h"

// get current magnitude
//   - absolute  - if no electrical_angle provided
//   - signed    - if angle provided
float CurrentSense::getDCCurrent(float motor_electrical_angle)
{
    // read current phase currents
    PhaseCurrent_s current = getPhaseCurrents();
    // currnet sign - if motor angle not provided the magnitude is always positive
    float sign = 1;

    // calculate clarke transform
    float i_alpha, i_beta;
    if (!current.c)
    {
        // if only two measured currents
        i_alpha = current.a;
        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
    }
    if (!current.a)
    {
        // if only two measured currents
        float a = -current.c - current.b;
        i_alpha = a;
        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
    }
    if (!current.b)
    {
        // if only two measured currents
        float b = -current.a - current.c;
        i_alpha = current.a;
        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
    }
    else
    {
        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
        float mid = (1.f / 3) * (current.a + current.b + current.c);
        float a = current.a - mid;
        float b = current.b - mid;
        i_alpha = a;
        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
    }

    // if motor angle provided function returns signed value of the current
    // determine the sign of the current
    // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1
    if (motor_electrical_angle)
    {
        float ct;
        float st;
        _sincos(motor_electrical_angle, &st, &ct);
        sign = (i_beta * ct - i_alpha * st) > 0 ? 1 : -1;
    }
    // return current magnitude
    return sign * _sqrt(i_alpha * i_alpha + i_beta * i_beta);
}

// function used with the foc algorihtm
//   calculating DQ currents from phase currents
//   - function calculating park and clarke transform of the phase currents
//   - using getPhaseCurrents internally
DQCurrent_s CurrentSense::getFOCCurrents(float angle_el)
{
    // read current phase currents
    PhaseCurrent_s current = getPhaseCurrents();

    // calculate clarke transform
    float i_alpha, i_beta;
    if (!current.c)
    {
        // if only two measured currents
        i_alpha = current.a;
        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
    }
    if (!current.a)
    {
        // if only two measured currents
        float a = -current.c - current.b;
        i_alpha = a;
        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
    }
    if (!current.b)
    {
        // if only two measured currents
        float b = -current.a - current.c;
        i_alpha = current.a;
        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
    }
    else
    {
        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
        float mid = (1.f / 3) * (current.a + current.b + current.c);
        float a = current.a - mid;
        float b = current.b - mid;
        i_alpha = a;
        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
    }

    // calculate park transform
    float ct;
    float st;
    _sincos(angle_el, &st, &ct);
    DQCurrent_s return_current;
    return_current.d = i_alpha * ct + i_beta * st;
    return_current.q = i_beta * ct - i_alpha * st;
    return return_current;
}

/**
    Driver linking to the current sense
*/
void CurrentSense::linkDriver(FOCDriver *_driver)
{
    driver = _driver;
}

// void CurrentSense::linkDriver(StepperDriver *_driver)
// {
//     driver = _driver;
// }
