// Using a stepper to simulate a brushed moter with encoder
// https://wokwi.com/projects/408602086380711937

// This code uses a a stepper as its own encoder and 
// tries to 


const uint16_t StepsPerRevolution = 200;

// connection Constants

// As in StepperDemo for Motor 1 on AVR
constexpr byte dirPinStepper  =  8;
constexpr byte enablePinStepper = 12;
constexpr byte stepPinStepper =  9;  // OC1A in case of AVR
constexpr byte PotPin = A0;


// This constant sets the motor speed. The value is in nanoSeconds per pulse,
// as we are using a quadrature encoder there are 4 pulses per encoder slot.
// Example; 1 motor rev per second = 144 pulses /sec.
// nS per pulse = 1 billion / 144 = 6944444
//#define  TARGET_COUNT_PERIOD  3472222    // 2 RPS
//#define  TARGET_COUNT_PERIOD  6944444    // 1 RPS
//#define  TARGET_COUNT_PERIOD  13888889   // 0.5 RPS
const long EncCountPerRev = 200 * 4;
const float RPS = 0.25;
// 800 pulses/sec
// ns between counts
const uint32_t CPS = EncCountPerRev * RPS;
uint32_t TARGET_COUNT_PERIOD = 1000000000 / (EncCountPerRev*RPS / 4); //ns

unsigned long milliRPS = 0; // pot-controlled milli-rev/sec


// Roman Black DCmotor Control Vars
long mpos, rpos; // byte-sized motor phase and reference phase
volatile uint32_t bres; // Bresenham accumulator
long mlag; // phase difference
#define PWMREG OCR0B // OC0B pin5 PWM register
const long MOTOR_PROP_GAIN = 1; // PWMcount/phaseError
// save values
uint8_t myPWM;

uint32_t nowUs, nowMs;


void doProcess() {
  const uint32_t interval = 1000;
  static uint32_t last = -interval;
  static uint32_t oldMilliRPS = 9999;
  if (nowMs - last >= interval) {
    last += interval;
    if (milliRPS != oldMilliRPS) {
      oldMilliRPS = milliRPS;
      uint32_t myTargetCountPeriod = 0;
      if ( milliRPS == 0 ) {
        myTargetCountPeriod = -1;
      }
      else {
        // myTargetCountPeriod = (1000000000UL / (EncCountPerRev *milliRPS/4/1000)
        myTargetCountPeriod = 1000000000UL / EncCountPerRev * 1000 / milliRPS * 4;

      }
      noInterrupts();
      TARGET_COUNT_PERIOD = myTargetCountPeriod;
      interrupts();
    }
  }
}

void setup() {
  Serial.begin(115200);
  Serial.print("RPS:");
  Serial.print(RPS);
  Serial.print(" CPS:");
  Serial.print(CPS);
  Serial.print(" nsPerCount:");
  Serial.print(TARGET_COUNT_PERIOD);

  Serial.println();

  pinMode(5, OUTPUT);
  pinMode(dirPinStepper, OUTPUT);
  pinMode(stepPinStepper, OUTPUT);
  pinMode(enablePinStepper, OUTPUT);
  digitalWrite(dirPinStepper, HIGH);
  digitalWrite(enablePinStepper, LOW);

  setupAdafruitMultitaskingInterrupt();

}

void loop() {
  nowUs = micros();
  nowMs = millis();
  doInputs();
  doProcess();
  report();
  //  moveMotor();
  moveMotorPWM(stepPinStepper, PWMREG);
}

void setupAdafruitMultitaskingInterrupt() {
  // Timer0 is already used for millis() - we'll just interrupt somewhere
  // in the middle and call the "Compare A" function below
  OCR0A = 0xAF;  // OC0A is the pin6 PWM pin.
  TIMSK0 |= _BV(OCIE0A);
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// poll the system to read the encoder, update the plan, and calculate the pwm
//
// Copied from https://www.romanblack.com/onesec/DCmotor_xtal.htm
// Slightly modified to use Arduino registers & interrupt scheme
// Would work well with Adafruit's timer0 multitasking per
// https://learn.adafruit.com/multi-tasking-the-arduino-part-2/timers
//
//
//void interrupt()
ISR(TIMER0_COMPA_vect)  // works with TIMSK0:OCIE0A
{
  static byte enc_last;
  byte enc_new;
  //-------------------------------------------------------
  // This is TMR0 int, prescaled at 2:1 so we get here every 512 instructions.
  // This int does the entire closed loop speed control;
  //  1. updates encoder to see if motor has moved, records it position
  //  2. updates reference freq generator, records its position
  //  3. compares the two, sets PWM if motor lags behind reference
  //  4. limit both counts, so they never roll, but still retain all error
  //-------------------------------------------------------
  // clear int flag straight away to give max safe time
  //  INTCON.T0IF = 0;

  //  1. updates encoder to see if motor has moved, records it position
  // poll quadrature encoder on Uno pins 2&3
  // 4count/pulse
  enc_new = (PIND & 0b00001100) >> 2; // get the 2 encoder bits
  if (enc_new != enc_last)
  {
    if ((enc_new & 0b1) != enc_last >> 1) mpos++; // record new motor position
    else                          mpos--;
    enc_last = enc_new;
  }

  //  2. updates reference freq generator, records its position
  bres += 1024000;                 // add nS per interrupt period (512 insts * 200nS)
  if (bres >= TARGET_COUNT_PERIOD) // if reached a new reference step
  {
    bres -= TARGET_COUNT_PERIOD;
    rpos++;                       // record new xtal-locked reference position
  }

  //  3. compares the two, set PWM% if motor lags behind reference
  if (mpos < rpos)  // if motor lagging behind
  {
    mlag = (rpos - mpos);                            // find how far it's lagging behind
    if (mlag >= (255 / MOTOR_PROP_GAIN)) PWMREG = 255; // full power if is too far behind
    else PWMREG = (mlag * MOTOR_PROP_GAIN);          // else adjust PWM if slightly behind
  }
  else            // else if motor is fast, cut all power!
  {
    PWMREG = 0;
  }

  //  4. limit both counts, so they never roll, but still retain all error
  if (false && (rpos > 250 || mpos > 250))
  {
    rpos -= 50;
    mpos -= 50;
  }

}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


void moveMotorPWM(const int stepPin, const int pwm) {
  // Full scale 120RPM = 2RPS, 200step/rev
  // at 255 PWM 120 PRM, 2RPS, 400 SPS, 1e6/400= 2500us/step
  // or 1e9/400ns/step
  // at 1PWM 1e9 /(400 *PWM/255) = 637500000ns/step
  // = 1e9/(FullScaleStepsPerSec/255*PWM
  const uint32_t FullSpeedSPS = 200UL * 100 / 60 ; // steps/rev * rev/min/(60s/min)
  const uint32_t interval = 1000;
  static uint32_t last = -interval;
  static uint32_t stepNs = 0; // ns between steps
  static long motBres = 0;
  static int lastPWM = 0;
  if (nowUs - last >= interval) {
    last += interval;
    if (pwm > 0) { // skip if stopped
      if (pwm != lastPWM) { // new speed?
        lastPWM = pwm;
        stepNs = 1000000000UL / FullSpeedSPS * 255 / pwm;
      }
      motBres += interval * 1000; // ns
      //      long stepNs = 1000000UL / long(200 * RPS); // 0.25 RPS
      if (motBres > stepNs) {
        motBres -= stepNs;
        digitalWrite(9, HIGH);
        // delayMicroseconds(900);
        digitalWrite(9, LOW);
      }
    }
  }
}


void moveMotor() {
  const uint32_t interval = 1000;
  static uint32_t last = -interval;
  static long motBres = 0;
  //if(digitalRead(9)) digitalWrite(9,LOW);
  if (nowUs - last >= interval) {
    last += interval;
    motBres += interval; // us
    noInterrupts();
    myPWM = PWMREG;
    interrupts();
    //long myThreshold = (1024UL - myPWM * 4)*10;
    long myThreshold = 1000000UL / long(200 * RPS); // 0.25 RPS
    if (motBres > myThreshold) {
      motBres -= myThreshold;
      digitalWrite(9, HIGH);
      // delayMicroseconds(900);
      digitalWrite(9, LOW);
    }
  }
}




void report() {
  const uint32_t interval = 1000;
  static uint32_t last = -interval;
  if (nowMs - last >= interval) {
    last += interval;
    noInterrupts();
    uint32_t myBres = bres;
    long myMlag = mlag;
    myPWM = PWMREG;
    interrupts();
    Serial.print(nowMs);
    Serial.print(" bres:");
    Serial.print(myBres);
    Serial.print(" mpos:");
    Serial.print(mpos);
    Serial.print(" rpos:");
    Serial.print(rpos);
    Serial.print(" mlag:");
    Serial.print(mlag);
    Serial.print(" PWM:");
    Serial.print(myPWM);
    Serial.println();
  }
}

void doInputs() {
  const uint32_t interval = 100;
  static uint32_t last = -interval;
  if (nowMs - last >= interval) {
    last += interval;
    int newPot = analogRead(A0);
    static int lastPot = -1;
    if (newPot != lastPot) {
      lastPot = newPot;
      milliRPS = map(newPot, 0, 1023, 0, 2000);
      Serial.print("New Speed: ");
      Serial.print(milliRPS);
      Serial.println("mRPS");
    }
  }
}