#include <math.h>
#ifndef SINLUT_H
#define SINLUT_H
int factorial(int n);
int t2;
double sineTaylor(double x,int k);
boolean check_time(double t);
double sine,k,p;
unsigned long t;
static unsigned long start_time;
const int out1 = 2;
const int out2 = 3;
const int out3 = 4;
const int out4 = 5;
const int out5 = 6;
const int out6 = 7;

const uint8_t tab[128] = {0,1,2,2,3,4,5,5,6,7,8,8,9,10,10,11,12,13,13,14,15,15,16,17,17,18,18,19,20,20,21,21,22,22,23,23,24,24,25,25,26,26,27,27,27,28,28,28,29,29,29,29,30,30,30,30,30,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,30,30,30,30,30,29,29,29,29,28,28,28,27,27,27,26,26,25,25,24,24,23,23,22,22,21,21,20,20,19,18,18,17,17,16,15,15,14,13,13,12,11,10,10,9,8,8,7,6,5,5,4,3,2,2,1};

//Return sin(x). x is an integer where -128 = -pi and +128 = pi
int sinLUT(int x){
  int out;
  x = x & 0x00ff;  //Truncate bits 8 and up - these just represent multiples of 2pi
  if (x & 0x80)   //Is the angle in the range -pi to 0?
    out = -(int)tab[x&0x7f];  //Yes, lookup the answer and invert it
  else
    out = (int)tab[x];      //No, just lookup the answer
  return out;
}

inline void fastWriteX(byte x){ //Pins 8-13 are PORTB bits 5:0 so just write them
  PORTB = x;
}

inline void fastWriteY(byte x){ //Pins 7-2 are PORTD bits 7:2 so shift data left by two bits
  PORTD = x << 2;
}

#endif
void setup() {
  // put your setup code here, to run once:
  pinMode(out1, OUTPUT);
  pinMode(out2, OUTPUT);
  pinMode(out3, OUTPUT);
  pinMode(out4, OUTPUT);
  pinMode(out5, OUTPUT);
  pinMode(out6, OUTPUT);
  p = 3.141;
  t2 = -128;
  k = 3;
  t = (unsigned long)(1000000/(256*100));//last term is frequency
  start_time = micros();
}

void loop() {
  // put your main code here, to run repeatedly:
   sine = sinLUT(t2);
   t2 += 1;
   if (t2>126) {
    t2 = -128;
   }

   //scale for 6 digital out (+1 and * 1.5 to get 0-3 range)
   sine = 32+sine;
   while (!(check_time(micros()))) {
    continue;//delayMicroseconds(1000000);
   }
   output(sine);
}
boolean check_time(unsigned long f) {
    if (f-start_time >= t) {
    start_time += t;
    return true;
   } else {
    return false;
   }
}
double sineTaylor(double x,int k){
  double y=0;
  for(int i=0;i<k+1;i++){
    y=y+pow(-1,i)*pow(x,(2*i+1))/factorial(2*i+1);
  }
  return y;
}
int factorial(int n){
  int z=1;
  for(int i=1;i<=n;i++){
    z=z*i;
  }
  return z;
}
void output (int i) {
  fastWriteY(i);
  /*
  digitalWrite(out1, i&0x01);
  digitalWrite(out2, i&0x02);
  digitalWrite(out3, i&0x04);
  digitalWrite(out4, i&0x08);
  digitalWrite(out5, i&0x10);
  digitalWrite(out6, i&0x20);
  */
}