tonyp7 / idylio Goto Github PK

#ifndef FPM_FPM_H
#define FPM_FPM_H

#include <stdint.h>

#define DECIMAL_BITS                12u

typedef uint32_t ifloat;

#define int_to_ifloat(n)            ((n) << DECIMAL_BITS)
#define ifloat_to_int(n)            ((n) >> DECIMAL_BITS)
#define float_to_ifloat(n)          ((uint32_t)((n) * (1u << DECIMAL_BITS)))
#define ifloat_to_float(n)          (((float)(n)) / (float)(1u << DECIMAL_BITS))

static inline ifloat mul_ifloat(ifloat a, ifloat b) {
    return (a * b) >> DECIMAL_BITS;
}

struct sin_wave {
    ifloat value;
    ifloat inverse_wave_len;
    uint32_t wave_len;
    uint32_t mod;
    uint16_t bitval;
    uint8_t mul;
};

struct sin_wave *create_sin_wave(uint32_t wave_duration, uint8_t bits);
void delete_sin_wave(struct sin_wave *wave);
void tick_sin_wave(struct sin_wave *wave);

#endif //FPM_FPM_H

#include <stdlib.h>
#include "fpm.h"

static const ifloat ZERO = int_to_ifloat(0u);
static const ifloat ONE = int_to_ifloat(1u);
static const ifloat TWO = int_to_ifloat(2u);
static const ifloat THREE = int_to_ifloat(3u);
static const ifloat FOUR = int_to_ifloat(4u);

// f(x) = x^2 if x <= 1
// f(x) = -(x-2)^2 + 2 if 1 < x <= 3
// f(x) = (x-4)^2 otherwise
static ifloat quadratic_wave(const ifloat v) {
    ifloat x = v;
    if (x <= ONE) {
        return mul_ifloat(x, x);
    }

    if (x <= THREE) {
        x -= TWO;
        x = mul_ifloat(x, x);
        return ZERO - x + TWO;
    }

    x -= FOUR;
    return mul_ifloat(x, x);
}

struct sin_wave *create_sin_wave(const uint32_t wave_duration, uint8_t bits) {
    struct sin_wave* result = malloc(sizeof(struct sin_wave));

    result->wave_len = wave_duration;
    result->value = 0;
    result->inverse_wave_len = float_to_ifloat(1.0f / wave_duration);
    result->mod = 0xFFFFFFFF;
    result->bitval = 0;
    result->mul = (uint8_t) (bits - 1u);

    tick_sin_wave(result);

    return result;
}

void delete_sin_wave(struct sin_wave *wave) {
    free(wave);
}

void tick_sin_wave(struct sin_wave *wave) {
    if (++wave->mod >= wave->wave_len) {
        wave->mod = 0;
    }

    wave->value = mul_ifloat(int_to_ifloat(wave->mod), wave->inverse_wave_len);
    wave->value <<= 2u;
    wave->value = quadratic_wave(wave->value);

    wave->bitval = (uint16_t) ifloat_to_int(wave->value << wave->mul);
}

#include <stdio.h>
#include <math.h>
#include "fpm.h"

int main(int argc, char **argv) {
    struct sin_wave *wave = create_sin_wave(100, 8);
    for (uint32_t i = 0; i < 200; i++) {
        tick_sin_wave(wave);
        printf("%d,%d\n", i, wave->bitval);
    }

    delete_sin_wave(wave);

    return 0;
}