//
// 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 Rw_typ = 60.0f;      // Типовое сопротивление движка
const float Rw_max = 120.0f;     // Максимальное (для консервативного расчета)

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;
*/

// Использование
    float Rwa_desired_100k = 1000.0f;
    float R_ab_100k = 100000.0f;
    int32_t D100 = calc_D_for_Rwa(Rwa_desired_100k, R_ab_100k, Rw_typ);

    float Rwa_desired_10k = 0.0f;
    float R_ab_10k = 10000.0f;
    int32_t D10 = calc_D_for_Rwa(Rwa_desired_10k, R_ab_10k, Rw_typ);

    uint32_t  step = 0;

    for (;;) {

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


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

        if (Rwa_desired_100k > 100000) {
            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 > 10000) {
            Rwa_desired_10k = 0;
        }




        result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_3, 255);
        asm("nop");
        ++step;

        if (step> 255)
            step =0;


        result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_4, D10);
        asm("nop");

/*
        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);
    }
}