#include "avr/pgmspace.h"

// table of 256 sine values / one sine period / stored in flash memory
PROGMEM  prog_uchar sine16[]  = {127, 176, 217, 245, 255, 245, 217, 176, 128, 79, 38, 10, 0, 10, 38, 79, 127 };

PROGMEM prog_uchar arctan256[] = {
 0, 1, 1, 2, 3, 3, 4, 4, 5, 6, 6, 7, 8, 8, 9, 10, 10, 11, 11, 12, 13, 13, 14, 15, 15, 16, 16, 17, 18, 18, 19, 20,
 20, 21, 22, 22, 23, 23, 24, 25, 25, 26, 27, 27, 28, 28, 29, 30, 30, 31, 31, 32, 33, 33, 34, 34, 35, 36, 36, 37,
 38, 38, 39, 39, 40, 41, 41, 42, 42, 43, 44, 44, 45, 45, 46, 46, 47, 48, 48, 49, 49, 50, 51, 51, 52, 52, 53, 53,
 54, 55, 55, 56, 56, 57, 57, 58, 58, 59, 60, 60, 61, 61, 62, 62, 63, 63, 64, 65, 65, 66, 66, 67, 67, 68, 68, 69,
 69, 70, 70, 71, 71, 72, 72, 73, 74, 74, 75, 75, 76, 76, 77, 77, 78, 78, 79, 79, 80, 80, 81, 81, 82, 82, 83, 83,
 84, 84, 84, 85, 85, 86, 86, 87, 87, 88, 88, 89, 89, 90, 90, 91, 91, 91, 92, 92, 93, 93, 94, 94, 95, 95, 96, 96,
 96, 97, 97, 98, 98, 99, 99, 99, 100, 100, 101, 101, 102, 102, 102, 103, 103, 104, 104, 104, 105, 105, 106, 106,
 106, 107, 107, 108, 108, 108, 109, 109, 110, 110, 110, 111, 111, 112, 112, 112, 113, 113, 113, 114, 114, 115, 115,
 115, 116, 116, 116, 117, 117, 118, 118, 118, 119, 119, 119, 120, 120, 120, 121, 121, 121, 122, 122, 122, 123, 123,
 123, 124, 124, 124, 125, 125, 125, 126, 126, 126, 127, 127, 127, 128, 128, 128
};

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))

volatile byte V[6]; // Analogue values
byte Z[6]; // Zero-offset values for each channel
volatile boolean F[6]; // "You have new Volts" flag.

unsigned int A; // Angle

long Accum; // For zero-measurement and buffering

volatile byte index; // index to current mux channel

byte oldindex = 0; // Only do arctan if there is new data
// variables used inside interrupt service declared as voilatile
volatile byte icnt;              // var inside interrupt

void setup()
{
  DDRB = B00101111;      // PORTB as output. except pin12
  DDRD = B11111100;      // PortD as output, except serial IO
  Serial.begin(1000000);        // connect to the serial port

  //First populate the zero-volt value for each channel
  analogReference(EXTERNAL);
  for (int i = 0 ; i <= 5 ; i++) { // iterate through channels
    Accum = 0;
    for (int j = 1 ; j<=1000 ; j++) { // 1000 samples should do
      Accum = Accum + analogRead(i);
    }
    Z[i] = Accum / 4000; // 10-to-8 bitshift and 1000
  }

  Setup_timer2();

  // disable interrupts to avoid timing distortion
  cbi (TIMSK0,TOIE0);              // disable Timer0 !!! delay() is now not available

  SetupADC();

}
void loop()
{
  sbi (TIMSK2,TOIE2);              // enable Timer2 Interrupt
  while(1){
      // 6000 rpm is a max change of 102.4 counts per cycle.
      // any bigger changes will be rejected as glitches
      byte B;
          if (F[0] || F[3]){
            F[0] = false;
            F[3] = false;
            A = arctan(V[0] - Z[0], V[3] - Z[3]);
            B = (A & B00011111);
            Serial.write(B00000000 | B);
            B = ((A >> 5) & B00011111);
            Serial.write(B10000000 | B);
          }
          if (F[1] || F[4]){
            F[1] = false;
            F[4] = false;
            A = arctan(V[1] - Z[1], V[4] - Z[4]);
            B = (A & B00011111);
            Serial.write(B00100000 | B);
            B = ((A >> 5) & B00011111);
            Serial.write(B10100000 | B);
          }
          if (F[2] || F[5]){
            F[2] = false;
            F[5] = false;
            A = arctan(V[2] - Z[2], V[5] - Z[5]);
            B = (A & B00011111);
            Serial.write(B01000000 | B);
            B = ((A >> 5) & B00011111);
            Serial.write(B11000000 | B);
          }
  }
}
//******************************************************************
// timer2 setup
// set prscaler to 1, PWM mode to phase correct PWM,  16000000/510 = 31372.55 Hz clock
void Setup_timer2() {

  sbi (TCCR2B, CS20); // Timer2 Clock Prescaler to : 1
  cbi (TCCR2B, CS21);
  cbi (TCCR2B, CS22);

  // Timer2 PWM Mode set to Phase Correct PWM
  cbi (TCCR2A, COM2A0);  // clear Compare Match
  sbi (TCCR2A, COM2A1);

  sbi (TCCR2A, WGM20);  // Mode 1  / Phase Correct PWM
  cbi (TCCR2A, WGM21);
  cbi (TCCR2B, WGM22);
}

//Setup ADC
void SetupADC() {
 // cbi(ADMUX, REFS1);
 //cbi(ADMUX, REFS0); // External Aref
 analogReference(EXTERNAL);
 sbi(ADMUX, ADLAR); // Left-aligned for 8-bit data
 cbi(ADMUX, MUX3);
 cbi(ADMUX, MUX2);
 cbi(ADMUX, MUX1);
 cbi(ADMUX, MUX0); // Set the Mux to zero to begin

 sbi(ADCSRA, ADEN); // Enable the ADC
 cbi(ADCSRA, ADSC); // Don't start conversion yet
 cbi(ADCSRA, ADATE); // No auto-trigger
 cbi(ADCSRA, ADIF); // Not sure if that is possible or wise
 sbi(ADCSRA, ADIE); // Conversion-complete Interrupt to process data
 sbi(ADCSRA, ADPS2);
 cbi(ADCSRA, ADPS1);
 cbi(ADCSRA, ADPS0); // ADC Clock prescalar = 16
}

//******************************************************************
// Timer2 Interrupt Service at 31372,550 KHz = 32uSec
// this is the timebase REFCLOCK for the DDS generator
// FOUT = (M (REFCLK)) / (2 exp 32)
// runtime : 8 microseconds ( inclusive push and pop)
ISR(TIMER2_OVF_vect) {

  icnt= 15 & (++icnt);     // use upper 8 bits for phase accu as frequency information
                         // read value fron ROM sine table and send to PWM DAC
  OCR2A=pgm_read_byte_near(sine16 + icnt);

  if (icnt == 5 || icnt==7 || icnt == 14) {
    sbi(ADCSRA, ADSC);  // Start Conversion
  }
}

// ADC Conversion Complete Interrupt
ISR(ADC_vect) {
 V[index] = ADCH; //get the data
 F[index] = true;
 if (++index > 5) index = 0; // increment the ADC channel number with wrap
 ADMUX = (ADMUX & B11110000) | index; // set the ACDC channel

}

// Lookup-Table based Arctan. Returns an angle from 0-1023 "Binary Degrees"

unsigned int arctan(int V1, int V2) {
  //Handle the quadrants explicitly
  if (V1 < 0)
  {
    if (V2 < 0)
    {
      if (V1 < V2) // 180-225
      {
        return 512 + pgm_read_byte_near(arctan256 + 256 * V2 / V1);
      }
      else // 225-270
      {
        return 768 - pgm_read_byte_near(arctan256 + 256 * V1 / V2);
      }
    }
    else // V2 => 0
    {
      if (-V1 < V2) // 90-135
      {
        return 256 + pgm_read_byte_near(arctan256 - 256 * V1 / V2);
      }
      else // 135-180
      {
        return 512 - pgm_read_byte_near(arctan256 - 256 * V2 / V1);
      }
    }
  }
  else // V1 => 0
  {
    if (V2 < 0)
    {
      if (V1 < -V2) // 270-315
      {
        return 768 + pgm_read_byte_near(arctan256 - 256 * V1 / V2);
      }
      else // 315-360
      {
         return 1023 - pgm_read_byte_near(arctan256 - 256 * V2 / V1);
      }
    }
    else // V2 => 0
    {
      if (V1 < V2) // 45-90
      {
        return 256 - pgm_read_byte_near(arctan256 + 256 * V1 / V2);
      }
      else // 0-45
      {
        return pgm_read_byte_near(arctan256 + 256 * V2 / V1);
      }
    }
  }
}