int portLeft = 4;
int portRight = 3;

long sampleMetro = 10;
long printMetro = 100;

int16_t stateLeft = 0;
int16_t stateRight = 0;

int16_t minLeft = 1024;
int16_t maxLeft = 0;
int16_t nextMinLeft = 1024;
int16_t nextMaxLeft = 0;
int16_t minRight = 1024;
int16_t maxRight = 0;
int16_t nextMinRight = 1024;
int16_t nextMaxRight = 0;

int16_t margin = 5;
int16_t rotation = 1; // right = 1, normal, -1 is reverse

int16_t lightLeft = 0; // latest reading
int16_t lightRight = 0;

double prevMs = 0; // timestamp of last peak
long previousSample = 0;
long previousPrint = 0;
double len = 0; // delay between the last two peaks

void setup() {
  Serial.begin(57600);
  Serial.println("\n[Monitoring a Ferraris Meter]");

  //portLeft.mode(INPUT);
  //portRight.mode(INPUT);
}

void loop() {
  unsigned long currentMillis = millis();
  // take a reading from both sensors
  if(currentMillis - previousSample > sampleMetro) {
    previousSample = currentMillis;
    // read sensors
    lightLeft = analogRead(portLeft);
    lightRight = analogRead(portRight);

    // to take into account changing peak sizes, we follow the peak size continuously
    // the found peak size serves as the target (minus a small margin) for the next peak detection
    // update minimum and maximum for the left sensor
    if ( lightLeft > nextMaxLeft ) {
      nextMaxLeft = lightLeft;
    }
    // we also do this for the valley
    if ( lightLeft < nextMinLeft ) {
      nextMinLeft = lightLeft;
    }

    // and the same for the right sensor
    // update minimum and maximum for the right sensor
    if ( lightRight > nextMaxRight ) {
      nextMaxRight = lightRight;
    }
    if ( lightRight < nextMinRight ) {
      nextMinRight = lightRight;
    }

    if (( stateLeft == 0 ) && (lightLeft > maxLeft - margin )) {
      // make state = 1 when light above threshold and state == 0
      stateLeft = 1;

      // we are now going to a minimum, reset the minimum value of the left sensor
      minLeft = nextMinLeft;
      nextMinLeft = 1024;

      // calculate rotation direction
      if (( stateRight == 0 ) && (rotation < 1)) {
        rotation++;
      } else if ((stateRight == 1 ) && (rotation > -1)) {
        rotation--;
      }

      // calculate delay between the last two peaks, now and prevMs
      double ms = millis();
      len = ms - prevMs;
      prevMs = ms;

    } else if ((stateLeft == 1 ) && (lightLeft < minLeft + margin)) {
      // make state = 0 when light below threshold and state == 1
      stateLeft = 0;

      // we are now going to a maximum, reset the maximum value of the left sensor
      maxLeft = nextMaxLeft;
      nextMaxLeft = 0;

    }

    if (( stateRight == 0 ) && (lightRight > maxRight - margin )) {
      // make state = 1 when light above threshold and state == 0
      stateRight = 1;

      // we are now going to a minimum, reset the minimum value of the right sensor
      minRight = nextMinRight;
      nextMinRight = 1024;

    } else if ((stateRight == 1 ) && (lightRight < minRight + margin)) {
      // make state = 0 when light below threshold and state == 1
      stateRight = 0;

      // we are now going to a maximum, reset the maximum value of the right sensor
      maxRight = nextMaxRight;
      nextMaxRight = 0;

    }
  }


  // print current status
  if(currentMillis - previousPrint > printMetro)  {
    previousPrint = currentMillis; 
    // calculate new delay between now and the previous peak (prevMs)
    double ms = millis();
    double tempDelay = ms - prevMs;

    // if 'tempDelay' is larger than the delay between the last two peaks 'len' it means that the disc is spinning slower and power usage is decreasing
    // so max power consumption at this moment is not len, but tempDelay.
    if ( len > tempDelay ) {
      tempDelay = len;
    }

    int watt = rotation * 4800000 / tempDelay; // my kwh meter says 600 rotations per kwh

    Serial.print(watt);
    Serial.print(" | ");
    Serial.print(rotation);
    Serial.print(" | ");
    Serial.print(lightLeft);  
    Serial.print(" | "); 
    Serial.println(lightRight);
    }
}