//
// Created by cfif on 05.05.23.
//
#include "MainModesArbiter_Private.h"

// Функция расчета D для Rwa
int32_t calc_D_for_Rwa(float Rwa_desired, float Rab, float Rw) {
    float D_float = 256.0f - ((Rwa_desired - Rw) / Rab) * 256.0f;
    int32_t D = (int32_t) (D_float + 0.5f);  // Округление

    if (D > 255) D = 255;
    if (D < 0) D = 0;

    return D;
}

// Параметры

const float R_const = 163.4f;
const float Rw_typ = 00.0f;   //60.0f   // Типовое сопротивление движка
const float Rw_max = 120.0f;     // Максимальное (для консервативного расчета)

/**
 * Расчет шагов для двухкаскадного цифрового потенциометра
 * Резисторы: 100 кОм (грубый) и 10 кОм (точный)
 * Постоянное сопротивление: 171 Ом
 *
 * @param Rout_required   Требуемое сопротивление в Омах
 * @param CountStep100    Указатель для сохранения шагов грубого резистора (0-255)
 * @param CountStep10     Указатель для сохранения шагов точного резистора (0-255)
 * @return                0 - успех, -1 - ошибка
 */
int32_t CalculateSteps(float Rout_required, int32_t *CountStep100, int32_t *CountStep10) {
    const float R_constL = 0.0f;//150.4f;
    const float R_ab_100k = 100000.0f;
    const float R_ab_10k = 10000.0f;

    if (CountStep100 == NULL || CountStep10 == NULL) {
        return -1;
    }

    const float Step100 = R_ab_100k / 256.0f;
    const float Step10 = R_ab_10k / 256.0f;

    float Rout = Rout_required - R_constL;
    if (Rout < 0) Rout = 0;

    *CountStep100 = (int32_t) (Rout / Step100);
    if (*CountStep100 > 255) *CountStep100 = 255;
    if (*CountStep100 < 0) *CountStep100 = 0;

    float R_tail = Rout - (float) (*CountStep100) * Step100;

    if (R_tail < 0) R_tail = 0;

    *CountStep10 = (int32_t) (R_tail / Step10 + 0.5f);
    if (*CountStep10 > 255) *CountStep10 = 255;
    if (*CountStep10 < 0) *CountStep10 = 0;

    return 0;
}


/**
 * Расчет шагов для двухкаскадного цифрового потенциометра
 * Приоритет заполнения точного резистора (10 кОм)
 * Резисторы: 100 кОм (грубый) и 10 кОм (точный)
 * Постоянное сопротивление: 163.4 Ом
 *
 * @param Rout_required   Требуемое сопротивление в Омах
 * @param CountStep100    Указатель для сохранения шагов грубого резистора (0-255)
 * @param CountStep10     Указатель для сохранения шагов точного резистора (0-255)
 * @return                0 - успех, -1 - ошибка
 */
/**
 * Расчет шагов для двухкаскадного цифрового потенциометра
 * Приоритет заполнения точного резистора (10 кОм)
 * Резисторы: 100 кОм (грубый) и 10 кОм (точный)
 * Постоянное сопротивление: 163.4 Ом
 *
 * @param Rout_required   Требуемое сопротивление в Омах
 * @param CountStep100    Указатель для сохранения шагов грубого резистора (0-255)
 * @param CountStep10     Указатель для сохранения шагов точного резистора (0-255)
 * @return                0 - успех, -1 - ошибка
 */
int32_t CalculateSteps2(float Rout_required, int32_t *CountStep100, int32_t *CountStep10) {
    const float R_constL = 163.4f;
    const float R_ab_100k = 100000.0f;
    const float R_ab_10k = 10000.0f;

    if (CountStep100 == NULL || CountStep10 == NULL) {
        return -1;
    }

    const float Step100 = R_ab_100k / 256.0f;  // ~390.625 Ом на шаг
    const float Step10 = R_ab_10k / 256.0f;    // ~39.0625 Ом на шаг

    float Rout = Rout_required - R_constL;
    if (Rout < 0) Rout = 0;

    // Приоритет 1: Сначала заполняем точный резистор (10 кОм)
    *CountStep10 = (int32_t) (Rout / Step10 + 0.5f);
    if (*CountStep10 > 255) *CountStep10 = 255;
    if (*CountStep10 < 0) *CountStep10 = 0;

    // Вычисляем остаток после установки точного резистора
    float R_remaining = Rout - (float) (*CountStep10) * Step10;
    if (R_remaining < 0) R_remaining = 0;

    // Приоритет 2: Остаток добираем грубым резистором (100 кОм)
    *CountStep100 = (int32_t) (R_remaining / Step100 + 0.5f);
    if (*CountStep100 > 255) *CountStep100 = 255;
    if (*CountStep100 < 0) *CountStep100 = 0;

    // Опционально: корректировка для минимизации общей ошибки
    // Если грубый резистор перекомпенсировал, уменьшаем точный
    float R_actual = R_constL + (float) (*CountStep100) * Step100 + (float) (*CountStep10) * Step10;
    float R_error = R_actual - Rout_required;

    // Если ошибка положительная и большая, пробуем скорректировать
    if (R_error > Step10 && *CountStep10 > 0) {
        // Уменьшаем точный резистор на 1 шаг
        (*CountStep10)--;
        // Пересчитываем грубый резистор
        R_remaining = Rout - (float) (*CountStep10) * Step10;
        if (R_remaining < 0) R_remaining = 0;
        *CountStep100 = (int32_t) (R_remaining / Step100 + 0.5f);
        if (*CountStep100 > 255) *CountStep100 = 255;
    }

    return 0;
}

void Mma_Init(
        tMma *env,
        tGpios *gpios,
        tSerialPorts *serialPorts,
        tCanPorts *canPorts,
        tSpiPorts *spiPorts,
        tRtcs *rtcs
) {
    env->gpios = gpios;
    env->serialPorts = serialPorts;
    env->canPorts = canPorts;
    env->spiPorts = spiPorts;
    env->rtcs = rtcs;

    InitThreadAtrStatic(&env->thread.attr, "Mma", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
    env->thread.id = 0;
}

static _Noreturn void Mma_Thread(tMma *env) {

    // Запуск устройства
    Mma_InitStage(env);

    bool result = false;



// Rwa — желаемое сопротивление между выводами W и A.
// Rab — полное номинальное сопротивление потенциометра (5, 10, 50 или 100 кОм).
// Rw — сопротивление движка (Wiper resistance). Типовое значение — 60 Ом (может варьироваться от 50 до 120 Ом).
// D — десятичное значение кода (от 0 до 255).
/*
    float Rwa = 60.0f;

    float Rwa100 = 21000.0f; // Это сколько сопротивление желаем
    float R100ab = 100000.0f;
    int32_t D100 = (int32_t) (256.0f - (((Rwa100 - Rwa) / R100ab) * 256.0f));

    if (D100 > 255)
        D100 = 255;

    if (D100 < 0)
        D100 = 0;


    float Rwa10 = 3000.0f; // Это сколько сопротивление желаем
    float R10ab = 10000.0f;
    int32_t D10 = (int32_t) (256.0f - (((Rwa10 - Rwa) / R10ab) * 256.0f));

    if (D10 > 255)
        D10 = 255;

    if (D10 < 0)
        D10 = 0;
*/

    int32_t step100, step10;
/*
// Использование
    float R_ab_100k = 100000.0f;
    float R_ab_10k = 10000.0f;
    float Step100 = R_ab_100k / 256.0f; // Шаг 100 кОм резистора
    float Step10 = R_ab_10k / 256.0f; // Шаг 10 кОм резистора

    float Rout = 9000.0f - R_const;
    int32_t CountStep100 = (int32_t) (Rout / Step100); // Целых шагов на резисторе 100 кОм
    float R_tail = Rout - (float) CountStep100 * Step100; // Хвост нужного сопротивления (Ом) для резистора 10 кОм
    int32_t CountStep10 = (int32_t) (R_tail / Step10 + 0.5f); // Шагов на резисторе 10 КОм (+ 0.5f для округления)


    float Rwa_desired_100k = (float)CountStep100 * Step100;
    step100 = calc_D_for_Rwa(Rwa_desired_100k, R_ab_100k, Rw_typ);

    float Rwa_desired_10k = (float)CountStep10 * Step10;
    step10 = calc_D_for_Rwa(Rwa_desired_10k, R_ab_10k, Rw_typ);
*/
/*
    float Rwa_desired2_100k = 000.00f - R_const;
    step100 = calc_D_for_Rwa(Rwa_desired2_100k, R_ab_100k, Rw_typ);

    float Rwa_desired2_10k = 9000.0f - R_const;
    step10 = calc_D_for_Rwa(Rwa_desired2_10k, R_ab_10k, Rw_typ);
*/


    float required_resistance = 10440.0f;
    CalculateSteps(required_resistance, &step100, &step10);

    step100 = 50;
    step10 = 0;

    step100 = 255 - step100;
    step10 = 255 - step10;

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);

    asm("nop");


//    D100 = (int32_t)(255.0f - (R / Step100));
//    D10 = (int32_t)(255.0f - (R / Step10));


//    D100 = 255 - (R / 390.625);
//    int32_t div


//    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, 255-27);
//    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, 255-19);

//    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
//    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);

    uint32_t step = 0;

    for (;;) {

/*
        Rwa_desired_100k += 1000.0f;
        D100 = calc_D_for_Rwa(Rwa_desired_100k, R_ab_100k, Rw_typ);

        if (Rwa_desired_100k > R_ab_100k) {
            Rwa_desired_100k = 0;
        }


        Rwa_desired_10k += 1000.0f;
        D10 = calc_D_for_Rwa(Rwa_desired_10k, R_ab_10k, Rw_typ);

        if (Rwa_desired_10k > R_ab_10k) {
            Rwa_desired_10k = 0;
        }
*/


        // 27
//        result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
        asm("nop");
//        ++step;

//        if (step> 255)
//            step =0;

        // 19
//        result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);
        asm("nop");


        GpioPinSet(&env->gpios->Led.ledR1, true);
        SystemDelayMs(1000);
        GpioPinSet(&env->gpios->Led.ledR1, false);
        SystemDelayMs(1000);

/*
        result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, D100);
        asm("nop");
        result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, D10);
        asm("nop");
        result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_3, D100);
        asm("nop");
        result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_4, D10);
        asm("nop");
*/
        /*


        GpioPinSet(&env->gpios->Led.ledR1, true);
        SystemDelayMs(1000);
        GpioPinSet(&env->gpios->Led.ledR1, false);
        SystemDelayMs(1000);

        data = 0;
        result = setSpiSensor(&env->sensorSPI, data);
*/
        /*
        GpioPinSet(&env->gpios->Led.ledR1, true);
        SystemDelayMs(1000);
        GpioPinSet(&env->gpios->Led.ledR1, false);
        SystemDelayMs(1000);

        uint16_t data = 255;
        bool result = setSpiSensor(&env->sensorSPI, data);

        GpioPinSet(&env->gpios->Led.ledR1, true);
        SystemDelayMs(1000);
        GpioPinSet(&env->gpios->Led.ledR1, false);
        SystemDelayMs(1000);

        data = 128;
        result = setSpiSensor(&env->sensorSPI, data);

        GpioPinSet(&env->gpios->Led.ledR1, true);
        SystemDelayMs(1000);
        GpioPinSet(&env->gpios->Led.ledR1, false);
        SystemDelayMs(1000);

        data = 1;
        result = setSpiSensor(&env->sensorSPI, data);

        GpioPinSet(&env->gpios->Led.ledR1, true);
        SystemDelayMs(1000);
        GpioPinSet(&env->gpios->Led.ledR1, false);
        SystemDelayMs(1000);

        data = 0;
        result = setSpiSensor(&env->sensorSPI, data);
*/
    }
}


void Mma_StartThread(tMma *env) {
    if (!env->thread.id) {
        env->thread.id = osThreadNew((osThreadFunc_t) (Mma_Thread), (void *) (env), &env->thread.attr);
    } else {
        osThreadResume(env->thread.id);
    }
}