// Пульт тележки: стик (АЦП PA0/PA1) -> SBUS на USART1 TX (PA9).
// Протокол: инвертированный SBUS, 100000 бод 8E2, 25 байт каждые 50 мс
// (см. docs/protocol.md). USB CDC (Serial) — отладочный вывод.
#include <Arduino.h>

// ---- калибровка стика (АЦП 12 бит, замерено 2026-08-20) ----
static const int X_FWD = 650, X_MID = 2014, X_BACK = 3378;   // PA0 (плечи равны: 2014-650 = 3378-2014 = 1364)
static const int Y_RIGHT = 479, Y_MID = 1981, Y_LEFT = 3586; // PA1
static const int DEADZONE = 15; // отсечка дребезга вокруг нейтрали

// ---- SBUS-значения старого пульта ----
static const uint16_t SBUS_MID = 1024;
static const uint16_t CH2_MIN = 433, CH2_MAX = 1643;  // назад..вперёд
static const uint16_t CH4_MIN = 446, CH4_MAX = 1654;  // влево..вправо

// ---- expo: 0 = линейно, 1 = максимально мягкая нейтраль ----
static const float EXPO_K = 0.0f;
// ---- общий масштаб выхода: 1.0 = диапазон старого пульта ----
static const float RANGE_SCALE = 0.75f;

// симметричные плечи: вперёд и назад дают одинаковый максимум
static const uint16_t CH2_SPAN = 591; // min(1643-1024, 1024-433)
static const uint16_t CH4_SPAN = 578; // min(1654-1024, 1024-446)

// ---- профиль газа/поворота ----
static const float MOVE_START = 0.15f; // старт движения, доля хода стика
static const float POWER_FWD  = 0.60f; // потолок «вперёд»
static const float POWER_BACK = 0.50f; // потолок «назад»
static const float POWER_TURN = 0.70f; // потолок поворота
static const uint16_t CH2_DB = 221;    // мёртвая зона приёмника тележки (ЗАМЕРИТЬ по монитору)
static const uint16_t CH4_DB = 221;

static const uint32_t FRAME_PERIOD_MS = 50;
static const uint32_t PIN_X = PA0;
static const uint32_t PIN_Y = PA1;

static UART_HandleTypeDef s_sbusUart;

// USART1 TX = PA9 (AF7), 100000 бод, 8E2, TX инвертирован
static void sbusUartInit() {
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_USART1_CLK_ENABLE();

    GPIO_InitTypeDef gpio = {};
    gpio.Pin = GPIO_PIN_9;
    gpio.Mode = GPIO_MODE_AF_PP;
    gpio.Pull = GPIO_NOPULL;
    gpio.Speed = GPIO_SPEED_FREQ_LOW;
    gpio.Alternate = GPIO_AF7_USART1;
    HAL_GPIO_Init(GPIOA, &gpio);

    s_sbusUart.Instance = USART1;
    s_sbusUart.Init.BaudRate = 100000;
    s_sbusUart.Init.WordLength = UART_WORDLENGTH_9B; // 8 данных + чётность
    s_sbusUart.Init.StopBits = UART_STOPBITS_2;
    s_sbusUart.Init.Parity = UART_PARITY_EVEN;
    s_sbusUart.Init.Mode = UART_MODE_TX;
    s_sbusUart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    s_sbusUart.Init.OverSampling = UART_OVERSAMPLING_16;
    s_sbusUart.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_TXINVERT_INIT;
    s_sbusUart.AdvancedInit.TxPinLevelInvert = UART_ADVFEATURE_TXINV_ENABLE;
    HAL_UART_Init(&s_sbusUart);
}

// нормализация одной оси в [-1..1] с мёртвой зоной и асимметричными плечами
static float axisNorm(int adc, int lowEnd, int mid, int highEnd) {
    float x;
    if (adc < mid - DEADZONE) {
        x = (float)(mid - adc) / (float)(mid - lowEnd); // к lowEnd -> +1
    } else if (adc > mid + DEADZONE) {
        x = -(float)(adc - mid) / (float)(highEnd - mid); // к highEnd -> -1
    } else {
        return 0.0f;
    }
    return constrain(x, -1.0f, 1.0f);
}

// expo-кривая: гасит чувствительность у нейтрали, сохраняет края
static float expo(float x) {
    return EXPO_K * x * x * x + (1.0f - EXPO_K) * x;
}

static uint16_t toSbus(float x, uint16_t span) {
    return (uint16_t)(SBUS_MID + lroundf(x * span));
}

// стик [-1..1] -> SBUS-значение канала: ниже MOVE_START — нейтраль, выше —
// линейно от порога срабатывания приёмника (db) до POWER_FRAC*span на полном стике
static uint16_t driveToSbus(float x, uint16_t span, uint16_t db,
                            float powerPos, float powerNeg) {
    float mag = fabsf(x);
    if (mag <= MOVE_START)
        return SBUS_MID;
    float outMax = (x > 0.0f ? powerPos : powerNeg) * span;
    float t = (mag - MOVE_START) / (1.0f - MOVE_START);
    long delta = lroundf(db + t * (outMax - db));
    return (x > 0.0f) ? (uint16_t)(SBUS_MID + delta)
                      : (uint16_t)(SBUS_MID - delta);
}

static void sbusPack(uint8_t out[25], const uint16_t ch[16]) {
    out[0] = 0x0F;
    memset(out + 1, 0, 22);
    uint32_t bitpos = 0;
    for (int n = 0; n < 16; n++) {
        for (int b = 0; b < 11; b++) {
            if (ch[n] & (1u << b))
                out[1 + (bitpos >> 3)] |= 1u << (bitpos & 7);
            bitpos++;
        }
    }
    out[23] = 0x00; // флаги
    out[24] = 0x00; // футер
}

static int readAvg(uint32_t pin) {
    uint32_t acc = 0;
    for (int i = 0; i < 16; i++)
        acc += analogRead(pin);
    return acc / 16;
}

void setup() {
    Serial.begin(115200);
    analogReadResolution(12);
    pinMode(PIN_X, INPUT_ANALOG);
    pinMode(PIN_Y, INPUT_ANALOG);
    sbusUartInit();
}

void loop() {
    static uint32_t next = 0;
    uint32_t now = millis();
    if (now < next)
        return;
    next = now + FRAME_PERIOD_MS;

    int adcX = readAvg(PIN_X);
    int adcY = readAvg(PIN_Y);
    // вперёд = adcX к X_FWD (вниз) -> +1; вправо = adcY к Y_RIGHT -> +1
    float fwd = axisNorm(adcX, X_FWD, X_MID, X_BACK);
    float right = axisNorm(adcY, Y_RIGHT, Y_MID, Y_LEFT);

    uint16_t ch[16];
    for (int i = 0; i < 16; i++)
        ch[i] = SBUS_MID;
    ch[1] = driveToSbus(fwd, CH2_SPAN, CH2_DB, POWER_FWD, POWER_BACK);  // канал 2 — газ
    ch[3] = driveToSbus(right, CH4_SPAN, CH4_DB, POWER_TURN, POWER_TURN); // канал 4 — поворот

    uint8_t frame[25];
    sbusPack(frame, ch);
    HAL_UART_Transmit(&s_sbusUart, frame, sizeof(frame), 20);

    Serial.print("adc=");
    Serial.print(adcX);
    Serial.print(",");
    Serial.print(adcY);
    Serial.print(" fwd=");
    Serial.print(fwd, 3);
    Serial.print(" right=");
    Serial.print(right, 3);
    Serial.print(" ch2=");
    Serial.print(ch[1]);
    Serial.print(" ch4=");
    Serial.println(ch[3]);
}