#define PIN_RIGHT  PORTD5  // D5  - pin D09, OC1A, Right side MOSFETS
#define PIN_LEFT   PORTD6  // D6 - pin D10, 0C1B, Left side MOSFETS
#define PIN_UPDATE PORTD4  // D4 - duty cycle detector updating
#define PIN_ADC    A0

#define UPDATE bitSet(PIND, PIN_UPDATE)
#define TABLE_LEN 40                                   // sine Table 
#define WF_AMPLITUDE 254                               // sine table maximum value

volatile uint8_t waveForm[TABLE_LEN];

void getWaveForm() {// generate waveform table
  double phase;
  const double wfa = double(WF_AMPLITUDE);
  for (int i = 0; i < TABLE_LEN / 2; i++) {
    phase = ((double(i) + 0.5) * HALF_PI) / (TABLE_LEN / 2);
    waveForm[i] = uint8_t(sin(phase) * wfa + 0.5);
    waveForm[TABLE_LEN - 1 - i] = waveForm[i];
  }
}

void setup() {
  Serial.begin(115200);        // opens serial port, sets data rate to 115200 bps
  bitClear(PORTD, PIN_RIGHT);  // set right pin output to 0V
  bitClear(PORTD, PIN_LEFT);   // set left pin output to 0V
  bitClear(PORTD, PIN_UPDATE); // set update pin output to 0V
  bitSet(DDRD, PIN_RIGHT);     // Set right pin direction as output
  bitSet(DDRD, PIN_LEFT);      // Set left pin direction as output
  bitSet(DDRD, PIN_UPDATE);    // Set update pin direction as output
  getWaveForm();
}

void loop() {
  static uint8_t i;
  static uint8_t side;
  if (i == TABLE_LEN) {
    i = 0;
    side = 1 - side;
  }
  //Serial.print(i);
  //Serial.print("\t");
  //Serial.print(side);
  if (side) {
    analogWrite(5, waveForm[i]);
    digitalWrite(6, 0);
    //Serial.print("\t");
    //Serial.println(waveForm[i]);
  } else {
    analogWrite(6, waveForm[i]);
    digitalWrite(5, 0);
    //Serial.print("\t");
    //Serial.println(waveForm[i]);
  }
  delay(10);
  UPDATE;
  
  Serial.println(analogRead(PIN_ADC));
  i++;
}