// math_util.c
//
// Copyright (C) 2020 Dan Rodrigues <danrr.gh.oss@gmail.com>
//
// SPDX-License-Identifier: MIT

#include "math_util.h"

#include "sin_table.h"

// sin() / cos()

static const uint16_t SIN_HALF_PERIOD = SIN_PERIOD / 2;
static const uint16_t SIN_QUARTER_PERIOD = SIN_PERIOD / 4;

int16_t cos(uint16_t angle) {
    return sin(angle + SIN_QUARTER_PERIOD);
}

int16_t sin(uint16_t angle) {
    angle &= (SIN_PERIOD - 1);

    // special cases to avoid "flat spots" in the sin wave
    if (angle == SIN_QUARTER_PERIOD) {
        return SIN_MAX;
    } else if (angle == (3 * SIN_QUARTER_PERIOD)) {
        return -SIN_MAX;
    }

    uint16_t index = angle;
    index = (angle & SIN_QUARTER_PERIOD ? -index : index);
    index &= (SIN_QUARTER_PERIOD - 1);

    int16_t sin = sin_table[index];

    return (angle & SIN_HALF_PERIOD ? -sin : sin);
}