/// \file midi2stepper.h
/// \brief header file for Midi2Stepper class
/// \author GrumpyDeveloper https://contentnation.net/en/grumpydevelop
/// \license MIT

#include "midi2stepper.h"
#include "pins_arduino.h"

Midi2Stepper m2s;

Midi2Stepper::Midi2Stepper() {
  m_enable = 0;
  m_counterA = 0;
  m_counterB = 0;
  m_counterC = 0;
  m_counterD = 0;
  m_counterE = 0;
  m_counterF = 0;
  m_counterG = 0;
  m_counterH = 0;
  m_cEndA = 0;
  m_cEndB = 0;
  m_cEndC = 0;
  m_cEndD = 0;
  m_cEndE = 0;
  m_cEndF = 0;
  m_cEndG = 0;
  m_cEndH = 0;
}

void Midi2Stepper::begin() {
  // set stepper pins to 0 and enable output
  for (uint8_t i = 0; i < 8; ++i) {
      digitalWrite(i+5, false);
      pinMode(i+5, OUTPUT);
  }
  // set /enable pin to 1 (off)
  digitalWrite(13, true);
  pinMode(13, OUTPUT);
  
  cli(); // disable interrupts
  TCCR1A = 0;    // Set the CTC mode
  TCCR1B = 0;
  TCNT1 = 0;
  OCR1A = 335;  // 333 = (16*10^6) / (480000*1) - 1 (must be <65536), adjusted to actual measured frequency
  TCCR1B |= (1 << WGM12);
  sei();  // enable interrupt
}

void Midi2Stepper::power(bool status) {
  cli();
  TCNT1  = 0;
  if (status) { // turn on
    // Set CS10 bit for 1 prescaler
    TCCR1B |= (1 << CS10);
    // enable timer compare interrupt
    TIMSK1 |= (1 << OCIE1A);
  } else {
    // Clear CS10 bit for 1 prescaler
    TCCR1B &= ~(1 << CS10);
    // enable timer compare interrupt
    TIMSK1 &= ~(1 << OCIE1A);
    digitalWrite(5, false);
    digitalWrite(6, false);
    digitalWrite(7, false);
    digitalWrite(8, false);
    digitalWrite(9, false);
    digitalWrite(10, false);
    digitalWrite(11, false);
    digitalWrite(12, false);
  }
  sei();
  digitalWrite(13, !status);
}

ISR(TIMER1_COMPA_vect) {  // timer1 interrupt 40kHz
  m2s.tick();
}

void Midi2Stepper::tick() {
  bool bmod = false; // was port b modified
  bool dmod = false; // was port d modified
  // create local copies to speed things up
  uint8_t portb = PORTB;
  uint8_t portd = PORTD;
  uint8_t enable = m_enable;

  // for each 
  if (enable & 1) {
    ++m_counterA;
    if (m_counterA > m_cEndA) {
      portd ^= (1 << PD5);  // toggle D5
      dmod = true;
      m_counterA = 0;
    }
  }
  if (enable & 2) {
    ++m_counterB;
    if (m_counterB > m_cEndB) {
      portd ^= (1 << PD6);  // toggle D6
      dmod = true;
      m_counterB = 0;
    }
  }
  if (enable & 4) {
    ++m_counterC;
    if (m_counterC > m_cEndC) {
      portd ^= (1 << PD7);  // toggle D7
      dmod = true;
      m_counterC = 0;
    }
  }
  if (enable & 8) {
    ++m_counterD;
    if (m_counterD > m_cEndD) {
      portb ^= (1 << PB0);  // toggle D8
      bmod = true;
      m_counterD = 0;
    }
  }
  if (enable & 16) {
    ++m_counterE;
    if (m_counterE > m_cEndE) {
      portb ^= (1 << PB1);  // toggle D9
      bmod = true;
      m_counterE = 0;
    }
  }
  if (enable & 32) {
    ++m_counterF;
    if (m_counterF > m_cEndF) {
      portb ^= (1 << PB2);  // toggle D10
      bmod = true;
      m_counterF = 0;
    }
  }
  if (enable & 64) {
    ++m_counterG;
    if (m_counterG > m_cEndG) {
      portb ^= (1 << PB3);  // toggle D11
      bmod = true;
      m_counterG = 0;
    }
  }
  if (enable & 128) {
    ++m_counterH;
    if (m_counterH > m_cEndH) {
      portb ^= (1 << PB4);  // toggle D12
      bmod = true;
      m_counterH = 0;
    }
  }

  if (bmod)
    PORTB = portb;
  if (dmod)
    PORTD = portd;
}

void Midi2Stepper::setChannel(uint8_t channelNum, uint16_t freq) {
  uint16_t v = 0xFFFF;
  if (freq > 0) {                        // was a frequency set?
    v = (((uint16_t)24000) / freq) - 1;
    // channel is turned on later
  } else {
    m_enable &= ~(1 << channelNum);      // turn off specified channel
    digitalWrite(5 + channelNum, false); // set output pin to 0
  }
  switch (channelNum) {
    case 0:
     m_cEndA = v;
     break;
    case 1:
     m_cEndB = v;
     break;
    case 2:
     m_cEndC = v;
     break;
    case 3:
     m_cEndD = v;
     break;
    case 4:
     m_cEndE = v;
     break;
    case 5:
     m_cEndF = v;
     break;
    case 6:
     m_cEndG = v;
     break;
    case 7:
     m_cEndH = v;
     break;
  }
  if (freq > 0) {                        // was a frequency set?
    m_enable |= 1 << channelNum;         // turn on specified channel
  }

}