//
// Created by cfif on 05.05.23.
//
#include "MainModesArbiter_Private.h"
#include <stdlib.h>
#include "dig_pot.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 = 130.0f;
const float Rw_typ = 00.0f;   //60.0f   // Типовое сопротивление движка
const float Rw_max = 120.0f;     // Максимальное (для консервативного расчета)

int32_t CalculateSteps(float Rout_required, uint8_t *CountStep100, uint8_t *CountStep10) {
    const float R_ab_100k = 100000.0f;
    const float R_ab_10k  =  10000.0f;
    const float R_w_100k  =     120.0f;
    const float R_w_10k   =     120.0f;
    const float R__const   = R_w_100k + R_w_10k;  // 150 Ом — всегда присутствует

    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__const;
    if (Rout < 0.0f) Rout = 0.0f;

    // Расчёт грубого каскада (без вычитания R_w — он уже в R_const)
    *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.0f) R_tail = 0.0f;

    // Расчёт точного каскада с округлением
    *CountStep10 = (int32_t)(R_tail / Step10 + 0.5f);
    if (*CountStep10 > 255) *CountStep10 = 255;
    if (*CountStep10 < 0)   *CountStep10 = 0;

    return 0;
}
*/


void Mma_Init(
        tMma *env,
        tGpios *gpios,
        tSerialPorts *serialPorts,
        tSerialLinPorts *serialLinPorts,
        tCanPorts *canPorts,
        tSpiPorts *spiPorts,
        tRtcs *rtcs
) {
    env->gpios = gpios;
    env->serialPorts = serialPorts;
    env->serialLinPorts = serialLinPorts;
    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);


    /*
    uint8_t step100, step10;

    float required_resistance = 10860;


    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);


    required_resistance = 0.03f; // DUCT1
    CalculateSteps(required_resistance, &step100, &step10);

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

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



    required_resistance = 3000.03f; // DUCT1
    CalculateSteps(required_resistance, &step100, &step10);

    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);


    required_resistance = 4000.03f; // DUCT2
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_3, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_4, step10);


    required_resistance = 5000.03f; // DUCT3
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_5, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_6, step10);


    required_resistance = 6000.03f; // DUCT4
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_7, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_8, step10);


    required_resistance = 7000.03f; // DUCT5
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_9, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_10, step10);


    required_resistance = 8000.03f; // DUCT6
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_11, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_12, step10);


    required_resistance = 9000.03f; // EVAP TEMP F
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_13, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_14, step10);



    //------------------


    required_resistance = 10000.03f; // DUCT1
    CalculateSteps(required_resistance, &step100, &step10);

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

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


    required_resistance = 11000.03f; // DUCT2
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_3, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_4, step10);


    required_resistance = 12000.03f; // DUCT3
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_5, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_6, step10);


    required_resistance = 13000.03f; // DUCT4
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_7, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_8, step10);


    required_resistance = 14000.03f; // EVAP TEMP R
    CalculateSteps(required_resistance, &step100, &step10);

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

    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_9, step100);
    result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_10, 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);
    }
}