int TO_RPI = 4;
int FR_RPI = 5;

// NON BLOCKING SERIAL!

int millisSince(unsigned long start){
  return (millis()-start);
}

void waitUntil(int millis, unsigned long start){
  delay(millis - millisSince(start));
}

int headerFromMillis(int millis){
  if ((millis > 85) || (millis < 15)){ return 0; }
  else if ((millis > 15) && (millis < 25)) { return 1; }
  else if ((millis > 35) && (millis < 45)) { return 2; }
  else if ((millis > 55) && (millis < 65)) { return 3; }
  else if ((millis > 75) && (millis < 85)) { return 4; }
}

void writeBit(bool state){
  digitalWrite(4, LOW);
  if (state){ delay(10); } else { delay(5); }
  digitalWrite(4, HIGH);
    if (state){ delay(5); } else { delay(10); }
}

int message = 1;
long centiVolts;
byte encodedVolts;

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(TO_RPI, OUTPUT);
  pinMode(FR_RPI, INPUT);
}

void loop() {

  unsigned long loop_start = millis();

  digitalWrite(LED_BUILTIN, HIGH);
  digitalWrite(TO_RPI, LOW);
  delay(20 * (message));
  digitalWrite(TO_RPI, HIGH);

  if (message == 1){
    delay(20); // make sure RPi is ready
    // STANDIN - will be a analogue read
    centiVolts = random(0, 1000);
    encodedVolts = map(centiVolts,0,1000,0,255);

    // Serial.print("Data binary (big-endian): ");
    for (byte mask = 00000001; mask>0; mask <<= 1){
     if (encodedVolts & mask) {
        // Serial.print("1");
        writeBit(1);
      } else {
        // Serial.print("0");
        writeBit(0);
      }
    }
  }

  Serial.print("Sent ");
  Serial.print(message);
  Serial.print(", Pulse Length: ");
  Serial.print(20 * (message));
  if (message == 1){
    Serial.print(", Volts: ");
    Serial.print(float(centiVolts)/100.0);
    // Serial.print(", Data: ");
    // Serial.print(encodedVolts);
  }
  Serial.println("");

  digitalWrite(LED_BUILTIN, LOW);

  waitUntil(950, loop_start);

  int previous_state = digitalRead(FR_RPI);
  int this_state;
  unsigned long fall_start = 0;
  unsigned long pulseLength = 0;
  int header = 0;
  while (millisSince(loop_start) < 1950){
    this_state = digitalRead(FR_RPI);
    if (fall_start == 0){
      if (previous_state) {
        if (!this_state) { // FALLING EDGE
          fall_start = millis();
        }
      }
    }
    else {
      if (this_state) { // RISING EDGE
        pulseLength = millisSince(fall_start);
        header = headerFromMillis(pulseLength);
        break;
      }
    }
  }

  Serial.print("Got ");
  Serial.print(header);
  Serial.print(", Pulse Length: ");
  Serial.print(pulseLength);
  Serial.println("");
  Serial.println("");

  message = (message % 4) + 1;

  waitUntil(2000, loop_start);

}