/**
 * Example for a sine/cosine table lookup
 *
 * butchered by true to work in 7 bits (int8_t, two turns), and to fix cos8 function
 * copied / inspired more or less from
 * https://www.atwillys.de/content/cc/sine-lookup-for-embedded-in-c/
 **/

#include "math/sin7.h"

/*
 * The number of bits of our data type: here 8 (sizeof operator returns bytes).
 */
#define INT8_BITS  (8 * sizeof(int8_t))
#define INT8_MAX   ((1<<(INT8_BITS-1))-1)

/*
 * "5 bit" large table = 32 values. The mask: all bit belonging to the table
 * are 1, the all above 0.
 */
#define TABLE_BITS  (5)
#define TABLE_SIZE  (1<<TABLE_BITS)
#define TABLE_MASK  (TABLE_SIZE-1)

/*
 * The lookup table is to 90DEG, the input can be -360 to 360 DEG, where negative
 * values are transformed to positive before further processing. We need two
 * additional bits (*4) to represent 360 DEG:
 */
#define LOOKUP_BITS (TABLE_BITS+2)
#define LOOKUP_MASK ((1<<LOOKUP_BITS)-1)
#define FLIP_BIT    (1<<TABLE_BITS)
#define NEGATE_BIT  (1<<(TABLE_BITS+1))
#define INTERP_BITS (INT8_BITS-1-LOOKUP_BITS)
#define INTERP_MASK ((1<<INTERP_BITS)-1)

/**
 * "5 bit" lookup table for the offsets. These are the sines for exactly
 * at 0deg, 11.25deg, 22.5deg etc. The values are from -1 to 1 in Q8?.
 */
const int8_t code sin90[TABLE_SIZE + 1] = {
	0x00, 0x06, 0x0c, 0x12, 0x18, 0x1e, 0x24, 0x2a,
	0x30, 0x36, 0x3b, 0x41, 0x46, 0x4b, 0x50, 0x55,
	0x59, 0x5e, 0x62, 0x66, 0x69, 0x6c, 0x70, 0x72,
	0x75, 0x77, 0x79, 0x7b, 0x7c, 0x7d, 0x7e, 0x7e,
	0x7f
};

/**
 * Sine calculation using interpolated table lookup.
 * Instead of radiants or degrees we use "turns" here. Means this
 * sine does NOT return one phase for 0 to 2*PI, but for 0 to 1.
 * Input: -1 to 1 as int8  == -128 to 127
 * Output: -1 to 1 as int8 == -128 to 127
 *
 * @param int8_t angle
 * @return int8_t
 */
int8_t sin7(int8_t angle)
{
	int8_t v0, v1;

	if(angle < 0) {
		angle += INT8_MAX;
		angle += 1;
	}

	v0 = (angle >> INTERP_BITS);

	if (v0 & FLIP_BIT) {
		v0 = ~v0;
		v1 = ~angle;
	} else {
		v1 = angle;
	}

	v0 &= TABLE_MASK;

	v1 = sin90[v0] + (int8_t)(((int16_t)(sin90[v0+1]-sin90[v0]) * (v1 & INTERP_MASK)) >> INTERP_BITS);

	if((angle >> INTERP_BITS) & NEGATE_BIT) {
		v1 = -v1;
	}

	return v1;
}

/**
 * Cosine calculation using interpolated table lookup.
 * Instead of radians or degrees we use "turns" here. Means this
 * cosine does NOT return one phase for 0 to 2*PI, but for 0 to 1.
 * Input: -1 to 1 as int8  == -128 to 127
 * Output: -1 to 1 as int8 == -128 to 127
 *
 * @param int8_t angle
 * @return int8_t
 */
int8_t cos7(int8_t angle)
{
	if (angle < 0) {
		angle += INT8_MAX;
		angle += 1;
	}

	return sin7(angle - (INT8_MAX * 3 / 4));
}
