#include "utils.h"
#include <Arduino.h>

#define function auto
#define let auto

namespace esp32 {
namespace utils {

// 三次贝塞尔曲线计算
function cubicBezier(float t, float p0, float p1, float p2, float p3) -> float {
    let u = 1.0f - t;
    return (u * u * u * p0) + (3.0f * u * u * t * p1) +
           (3.0f * u * t * t * p2) + (t * t * t * p3);
}

function solveCubicBezierX(float xTarget,
                           float x1,
                           float x2,
                           float epsilon) -> float {
    let t = xTarget;
    let x = cubicBezier(t, 0.0f, x1, x2, 1.0f);
    int iteration = 0;

    while (abs(x - xTarget) > epsilon && iteration < 100) {
        let d = 3.0f * (1.0f - t) * (1.0f - t) * (x1 - 0.0f) +
                6.0f * (1.0f - t) * t * (x2 - x1) + 3.0f * t * t * (1.0f - x2);
        if (d == 0.0f)
            break;
        t -= (x - xTarget) / d;
        x = cubicBezier(t, 0.0f, x1, x2, 1.0f);
        iteration++;
    }

    return t;
}

function linear2CubicBezier(float source,
                            float x1,
                            float y1,
                            float x2,
                            float y2) -> float {
    if (source < 0.0f || source > 1.0f) {
        return source;  // 返回原值而不是抛出异常
    }

    let t = solveCubicBezierX(source, x1, x2);
    let y = cubicBezier(t, 0.0f, y1, y2, 1.0f);

    return y;
}

function rgbTo565(uint32_t rgbColor) -> uint16_t {
    uint8_t r = (rgbColor >> 16) & 0xFF;
    uint8_t g = (rgbColor >> 8) & 0xFF;
    uint8_t b = rgbColor & 0xFF;

    // 交换 r 和 b
    uint8_t tmp = r;
    r = b;
    b = tmp;

    uint16_t r5 = (r * 31 / 255) << 11;
    uint16_t g6 = (g * 63 / 255) << 5;
    uint16_t b5 = (b * 31 / 255);

    return r5 | g6 | b5;
}

}  // namespace utils
}  // namespace esp32