#define measurepin 0
#define pwmPin 3
#define maxPwmChange 25
#define pwmChangeTreshold 250

unsigned long currentRPM = 0;
volatile bool durUpToDate = false;
volatile unsigned long start = 0;
volatile unsigned long dur = 0;

int currentPWM = 0;
unsigned long wantedRPM = 2300;

long long_abs(long val){
  if(val >= 0){
    return val;
  }
  return -val;
}

long long_sign(long val){
  if(val > 0){
    return 1;
  }else if( val < 0){
    return -1;
  }
  return 0;
}

long long_map(long value, double istart, double istop, double ostart, double ostop) {
    return (long)(ostart + (ostop - ostart) * ((value - istart) / (istop - istart)));
}

void setup() {
  Serial.begin(9600);
  pinMode(pwmPin,OUTPUT);

  start = micros();
  attachInterrupt(measurepin, rise, RISING);
}

long currentPWMChange = 0;

void loop() {
  for(int i =0; i< 10; i++){
    delay(50);

    if(durUpToDate){
      durUpToDate = false;
      double rpm = (1000000.0/dur)/16; //approx. pulses per second. divided by 16 gives revolutions per second.
      rpm*=60; //per minute
      currentRPM = (unsigned long) rpm;
    }else{
      currentRPM = 0;
    }

    long desired = wantedRPM-currentRPM;
    long desiredMag = long_abs(desired);
    desired = long_sign(desired);

    if(desiredMag < pwmChangeTreshold){
      desired *= long_map(desiredMag,0,pwmChangeTreshold,0,maxPwmChange); // map(d,0,100,0,maxspeed);
    }else{
      desired *= maxPwmChange;
    }

    long mod = desired - currentPWMChange;
    currentPWMChange += mod;

    currentPWM += currentPWMChange;

    if(currentPWM >255){
      currentPWM = 255;
    }else if(currentPWM < 0){
      currentPWM = 0;
    }

    analogWrite(pwmPin,255-currentPWM); //reversed cause npn negates
  }
  Serial.print("RPM: ");
  Serial.println(currentRPM);
  Serial.print("DUR: ");
  Serial.println(dur);
  Serial.print("PWM: ");
  Serial.println(currentPWM);
}

void rise(){
  unsigned long end_ = micros();
  dur = end_ - start;
  durUpToDate = true;
  start = end_;
}