/**
 * Example for a sine/cosine table lookup
 * Implementation of sin1() / cos1().
 * We "outsource" this implementation so that the precompiler constants/macros
 * are only defined here.
 *
 * @file sin1.c
 * @author stfwi
 **/
 
#include "sin1.h"
 
/*
 * The number of bits of our data type: here 16 (sizeof operator returns bytes).
 */
#define INT16_BITS  (8 * sizeof(int16_t))
#ifndef INT16_MAX
#define INT16_MAX   ((1<<(INT16_BITS-1))-1)
#endif
 
/*
 * "5 bit" large table = 32 values. The mask: all bit belonging to the table
 * are 1, the all above 0.
 */
#define TABLE_BITS  (7)
#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 (INT16_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 Q15.
 */
static int16_t sin90[TABLE_SIZE+1] = {
  0x0000,0x0192,0x0324,0x04b6,0x0647,0x07d9,0x096a,0x0afb,
  0x0c8b,0x0e1b,0x0fab,0x1139,0x12c7,0x1455,0x15e1,0x176d,
  0x18f8,0x1a82,0x1c0b,0x1d93,0x1f19,0x209f,0x2223,0x23a6,
  0x2527,0x26a7,0x2826,0x29a3,0x2b1e,0x2c98,0x2e10,0x2f86,
  0x30fb,0x326d,0x33de,0x354d,0x36b9,0x3824,0x398c,0x3af2,
  0x3c56,0x3db7,0x3f16,0x4073,0x41cd,0x4325,0x447a,0x45cc,
  0x471c,0x4869,0x49b3,0x4afa,0x4c3f,0x4d80,0x4ebf,0x4ffa,
  0x5133,0x5268,0x539a,0x54c9,0x55f4,0x571d,0x5842,0x5963,
  0x5a81,0x5b9c,0x5cb3,0x5dc6,0x5ed6,0x5fe2,0x60eb,0x61f0,
  0x62f1,0x63ee,0x64e7,0x65dd,0x66ce,0x67bc,0x68a5,0x698b,
  0x6a6c,0x6b4a,0x6c23,0x6cf8,0x6dc9,0x6e95,0x6f5e,0x7022,
  0x70e1,0x719d,0x7254,0x7306,0x73b5,0x745e,0x7503,0x75a4,
  0x7640,0x76d8,0x776b,0x77f9,0x7883,0x7908,0x7989,0x7a04,
  0x7a7c,0x7aee,0x7b5c,0x7bc4,0x7c29,0x7c88,0x7ce2,0x7d38,
  0x7d89,0x7dd5,0x7e1c,0x7e5e,0x7e9c,0x7ed4,0x7f08,0x7f37,
  0x7f61,0x7f86,0x7fa6,0x7fc1,0x7fd7,0x7fe8,0x7ff5,0x7ffc,
  0x7fff
};

/**
 * 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 int16 Q15  == -32768 to 32767.
 * Output: -1 to 1 as int16 Q15 == -32768 to 32767.
 *
 * See the full description at www.AtWillys.de for the detailed
 * explanation.
 *
 * @param int16_t angle Q15
 * @return int16_t Q15
 */
int16_t sin1(int16_t angle)
{
  int16_t v0, v1;
  if(angle < 0) { angle += INT16_MAX; angle += 1; }
  v0 = (angle >> INTERP_BITS);
  if(v0 & FLIP_BIT) { v0 = ~v0; v1 = ~angle; } else { v1 = angle; }
  v0 &= TABLE_MASK;
  v1 = sin90[v0] + (int16_t) (((int32_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 radiants 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 int16 Q15  == -32768 to 32767.
 * Output: -1 to 1 as int16 Q15 == -32768 to 32767.
 *
 * @param int16_t angle Q15
 * @return int16_t Q15
 */
int16_t cos1(int16_t angle)
{
  if(angle < 0) { angle += INT16_MAX; angle += 1; }
  return sin1(angle - (int16_t)(((int32_t)INT16_MAX * 3) / 4));
}