#include "s_curve.h"

// s曲线加速度各段参数定义
// 起始速度
#define F0 0.0f

// 加加速度与减减速度
#define FAA 0.0f
#define FRR 0.0f

// 加速段三个时间
#define TAA 0.1f
#define TUA 0.2f
#define TRA 0.1f

// 匀速段
#define TUU 5.0f

// 减速段
#define TAR 0.1f
#define TUR 0.2f
#define TRR 0.1f

// 表格长度
#define S_CURVE_TABLE_LEN 400
int16_t s_curve_table[S_CURVE_TABLE_LEN] = {0};

static int16_t s_curve_func(s_curve_t *s, float32 t, float32 *freq, float32 *acc)
{
    // 辅助常数项
    float32 A, B, C, D, E, F;
    // 表达式中间值
    float32 f3, f4, f5;
    // 加速段，匀速段，减速段时间
    float32 Ta, Tu, Tr;
    // 减加速与加减速段斜率
    float32 fra, far;
    // 起始频率与加加速频率，减减速频率
    float32 f0, faa, frr;
    float32 taa, tua, tra, tuu, tar, tur, trr;

    // 获取参数
    faa = s->faa;
    frr = s->frr;

    taa = s->taa;
    tua = s->tua;
    tra = s->tra;
    tuu = s->tuu;
    tar = s->tar;
    tur = s->tur;
    trr = s->trr;

    f0 = s->f0;

    fra = faa * taa / tra;
    far = frr * trr / tar;

    Ta = taa + tua + tra;
    Tu = tuu;
    Tr = tar + tur + trr;

    A = f0;
    B = f0 - 0.5 * faa * taa * taa;
    C = f0 + 0.5 * faa * taa * taa + faa * taa * tua + 0.5 * fra * (taa + tua) * (taa + tua) - fra * Ta * (taa + tua);

    // f1 = f0 + 0.5 * faa * taa * taa;
    // f2 = f0 + 0.5 * faa * taa * taa + faa * taa * tua;
    f3 = 0.5 * fra * Ta * Ta + C;

    D = f3 - 0.5 * far * (Ta + Tu) * (Ta + Tu);
    f4 = -far * 0.5 * (Ta + Tu + tar) * (Ta + Tu + tar) + far * (Ta + Tu) * (Ta + Tu + tar) + D;

    E = f4 + far * tar * (Ta + Tu + tar);
    f5 = -far * tar * (Ta + Tu + Tr - trr) + E;

    F = f5 + frr * (Ta + Tu + Tr) * (Ta + Tu + Tr - trr) - 0.5 * frr * (Ta + Tu + Tr - trr) * (Ta + Tu + Tr - trr);

    // 如果时间点在全行程规定的时间段内
    if ((t >= 0) && (t <= Ta + Tu + Tr))
    {
        // 加加速段
        if ((t >= 0) && (t <= taa))
        {
            *freq = 0.5 * faa * t * t + A;
            *acc = faa * t;
        }
        // 匀加速段
        else if ((t >= taa) && (t <= taa + tua))
        {
            *freq = faa * taa * t + B;
            *acc = faa * taa;
        }
        // 加减速段
        else if ((t >= taa + tua) && (t <= taa + tua + tra))
        {
            *freq = -0.5 * fra * t * t + fra * Ta * t + C;
            *acc = -fra * t + fra * Ta;
        }
        // 匀速段
        else if ((t >= Ta) && (t <= Ta + tuu))
        {
            *freq = f3;
            *acc = 0;
        }
        // 加减速段
        else if ((t >= Ta + Tu) && (t <= Ta + Tu + tar))
        {
            *freq = -0.5 * far * t * t + far * (Ta + Tu) * t + D;
            *acc = -far * t + far * (Ta + Tu);
        }
        // 匀减速
        else if ((t >= Ta + Tu + tar) && (t <= Ta + Tu + tar + tur))
        {
            *freq = -far * tar * t + E;
            *acc = -far * tar;
        }
        // 减减速
        else if ((t >= Ta + Tu + Tr - trr) && (t <= Ta + Tu + Tr))
        {
            *freq = 0.5 * frr * t * t - frr * (Ta + Tu + Tr) * t + F;
            *acc = frr * t - frr * (Ta + Tu + Tr);
        }
    }
    else
    {
        return -1;
    }

    return 0;
}

// S型曲线初始化
void s_curve_table_gen(uint16_t tmin, uint16_t tmax)
{
    uint16_t i, tint;
    float32 ti;
    float32 freq;
    float32 acc;
    float32 fi;
    s_curve_t s;
    osel_memset((uint8_t *)&s, 0, sizeof(s_curve_t));

    s.f0 = F0;
    s.taa = TAA;
    s.tua = TUA;
    s.tra = TRA;
    s.tuu = TUU;
    s.tar = TAR;
    s.tur = TUR;
    s.trr = TRR;

    // 根据约束条件求出加加速段与减减速段斜率
    s.faa = 2.0 / (s.taa * (s.taa + s.tra + 2 * s.tua));
    s.frr = 2.0 / (s.trr * (s.tar + s.trr + 2 * s.tur));

    for (i = 0; i < S_CURVE_TABLE_LEN; i++)
    {
        // 求出每个时间点对应的频率以及加速度
        fi = i * (TAA + TUA + TRA + TUU + TAR + TUR + TRR) / S_CURVE_TABLE_LEN;
        s_curve_func(&s, fi, &freq, &acc);

        // 根据最大与最小装载值确定定时器实际值
        ti = tmax - (tmax - tmin) * freq;
        // 转换为整数值
        tint = (uint16_t)ti;

        // 存入s曲线表
        s_curve_table[i] = tint;
    }
}