/*
* very simple dc motor model
* a low pass filter
* https://en.wikipedia.org/wiki/Low-pass_filter
* No back EMF etc just pure lp filter
*
*now with Q encoding
* May 2025 Henrik Schiøler Jens Dalsgaard Nielsen , AAU   vrs 0.4
* JDN: simple DC MOTOR simull 1. ord lp
* JDN: JDND&HS May 2025
* JDN: dcmotorhs05
*/

/* ENCODER
* QUADPIN1
* QUADPIN2
*  DIRPIN gives direction of rotation
* ISRPIN goes low and high hevery time rotation has increased/decreased at lesat  radPrDecoderStep 
*/

/*
*   INPUT
*   1) uncomment testSquare in loop
*   or 
*   2) use serial interface
*  tx  '['<floatval>']'   like  [1.23]    
*  this value updates in0  (like test Square)
*/

/*
* OUTPUT
*  1) DIRPIN(See nr below)  gives direction of rotation
*  2) ISR PIN (See nr below) do a high->low->high   falling edge interrupt
*     
*/


// interface
// DIRPIN LOW RR  HIGH RL
//ISRPIN FALLING INTERRUPT FOR EVERY NEW encoder step
// ERRPIN  error detected

const int QUADPIN1 = 8;
const int QUADPIN2 = 9;
const int LOOPTIMEPIN = 10;

bool QUADPIN1_state = false;
bool QUADPIN2_state = false;
bool shift = false;

int TimeCnt = 0;


// SYS DFT //

const float fS = 5000.0;  // samplings freq [Hz]

// DC motor /lp filter
#define PI 3.141592653

float calcFilterCoef(float fS, float fLP) {  //  https://en.wikipedia.org/wiki/Low-pass_filter
  float ts, rc, a;
  ts = 1.0 / fS;
  rc = fLP / (2 * PI);
  a = (2 * PI * (1.0 / fS) * fLP) / ((2 * PI * (1 / fS)) + 1.0);
  return a;
}


const float fFlt = 0.05;                          // corner freq for first order lowpass model [Hz]
const float aWeight = calcFilterCoef(fS, fFlt);  // weightfactor to simulate 1 ord LP filter
                                                 // https://en.wikipedia.org/wiki/Low-pass_filter
// +- max input and output limits
const float vInMax = 2.0;   // volt
const float vOutMax = 2.0;  // volt

// low pass fhttps://en.wikipedia.org/wiki/Low-pass_filterilter vars

// DC MOTOR no 1
float out0 = 0.0;      // output from LP /dc motor model
float out0_old = 0.0;  // old output from LP /dc motor model
float in0 = 0.0;       // input
//float radPrSec;        // act rot hastighed

//volatile float rotCounter = 0.0;

#define PKGSZ 20
#define STARTDELIM '['
#define STOPDELIM ']'


char pkg[PKGSZ];
int pkgIndex;

char pkgState;  // 0: no pkg 1: pgk on way 2: pkg received
                // after receiving pkg you call resetNewPkg,
                // pakke format ["ascii panaer [ og ]"]

void resetNewPkg() {
  pkgIndex = 0;
  pkgState = 0;
}

int newPkg() {
  int ch;
  char c;

  if (0 < Serial.available()) {
    ch = Serial.read();
    if (ch < 0)
      return -1;
    c = (char)ch;

    if (STARTDELIM == c) {
      resetNewPkg();
      pkgState = 1;
      return pkgState;
    }

    if (pkgIndex < PKGSZ) {
      if (1 == pkgState) {
        if (STOPDELIM == c) {
          pkg[pkgIndex] = 0x00;
          pkgState = 2;
        } else {
          pkg[pkgIndex] = c;
          pkgIndex++;
        }
      }
    } else {
      resetNewPkg();
    }
    return pkgState;
  }
}
char testForNewInput() {
  int Pkg_state;
  // any new input from serial port ?
  Pkg_state = newPkg();
  if (Pkg_state == 2) {  // got a new line
    Serial.print(in0);
    Serial.print(" ");
    Serial.print(out0);
    Serial.print(" ");
    in0 = atof(pkg);
    Serial.println(in0);
    resetNewPkg();  // rdy for next
    return 1;       // new
  }
  return 0;  // no news
}


//****************


//  FILTER or dc motor model
// GAIN 1
float firstOrderLP(float in, float oldOut, float weight) {
  return (weight * in) + (1 - weight) * oldOut;
}

float clip(float val, float max) {
  if (max < val)
    return max;
  else if (val < -max)
    return -max;
  else
    return val;
}

void doMotor() {

  in0 = clip(in0, vInMax);

  out0_old = out0;
  out0 = firstOrderLP(in0, out0, aWeight);  // use in0 result in  out0;

  out0 = clip(out0, vOutMax);
}

void calcPerAndDirections() {
  float dT;

  // change in direction ?
  if (out0_old * out0 < 0)
    QUADPIN2_state = !QUADPIN2_state;

  /* or we just look at signbit
  if ( (char)(out0) & 0x80  != (char)(out0_old) & 0x80 ) 
    DIRPIN_state = !DIRPIN_state;
  */

  if (abs(out0) > 1.0e-9)
    dT = abs(1 / out0);
  else
    dT = 1.0e20;

  //QUADPIN1
  if (TimeCnt > abs(dT * 10)) {
    TimeCnt = 0;
    shift = false;
    QUADPIN1_state = !QUADPIN1_state;
  } else {
    TimeCnt++;
  }
  // QUADPIN1
  if ((TimeCnt > abs(dT * 5)) && !shift) {
    shift = true;
    QUADPIN2_state = !QUADPIN2_state;
  }
}

void setOutPins() {
  digitalWrite(QUADPIN1, QUADPIN1_state);
  digitalWrite(QUADPIN2, QUADPIN2_state);
}

void loop() {

  testForNewInput();

  doMotor();

  calcPerAndDirections();

  setOutPins();

  //   0 usec ~  165 usec Fs ~= 6,1 kHz
  //  45 usec  ~ 202 usec Fs ~= 5.0 kHz
  //  50 usec  ~ 207 usec Fs ~= 4.8 kHz
  // 100 usec ~ 255 usec  Fs ~= 3.9 kHz
  // 200 usec ~ 360 usec  Fs ~= 2.8 kHz
  delayMicroseconds(45);

  digitalWrite(LOOPTIMEPIN, !digitalRead(LOOPTIMEPIN));
}

void setup() {
  Serial.begin(115200);
  delay(100);
  pinMode(QUADPIN1, OUTPUT);
  pinMode(QUADPIN2, OUTPUT);
  pinMode(LOOPTIMEPIN, OUTPUT);

  resetNewPkg();
}
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx


/*
https://www.dsp-weimich.com/digital-signal-processing/iir-first-order-digital-filter/
Example of the A value calculation
Sampling fs=20kHz
Filter passband fb=1kHz
Then
RC=1/(2πfb)=1/(2*3.14*1000Hz) ≈ 1.6e-4s
Ts=1/fs=1/20000Hz=0.5e-4s
A=Ts/(Ts+RC)=0.5e-4/(0.5e-4+1,6e-4) ≈ 0.24
y(n)=y(n-1)+0.24*(x(n)-y(n-1))

JDN

fs = 50 Hz
lowpass freq  10 Hz

-> 
RC = 1/(2*pi*10) = 15.9 * 10^-3
TS = 1/Fs = 1/50 = 20*10^-3  
A = TS/(TS+RC) = 20/(15.9+20) = 0.55
*/