#include <OneWire.h>
#include <DallasTemperature.h>

// #define NDEBUG

#define ONE_WIRE_BUS 2
#define PULSE_PIN    A0
#define ECG_PIN      A1
#define BLINK_PIN    13

#define ECG1         10
#define ECG2         11

#define ESP_RESET    12

// Volatile Variables, used in the interrupt service routine!
int blinkPin = 13;                // pin to blink led at each beat
volatile int BPM;                   // int that holds raw Analog in 0. updated every 2mS
volatile int Signal;                // holds the incoming raw data
volatile int IBI = 600;             // int that holds the time interval between beats! Must be seeded!
volatile boolean Pulse = false;     // "True" when User's live heartbeat is detected. "False" when not a "live beat".
volatile boolean QS = false;        // becomes true when Arduoino finds a beat.

static boolean serialVisual = true;   // Set to 'false' by Default.  Re-set to 'true' to see Arduino Serial Monitor ASCII Visual Pulse

volatile int rate[10];                      // array to hold last ten IBI values
volatile unsigned long sampleCounter = 0;          // used to determine pulse timing
volatile unsigned long lastBeatTime = 0;           // used to find IBI
volatile int P = 512;                      // used to find peak in pulse wave, seeded
volatile int T = 512;                     // used to find trough in pulse wave, seeded
volatile int thresh = 525;                // used to find instant moment of heart beat, seeded
volatile int amp = 100;                   // used to hold amplitude of pulse waveform, seeded
volatile boolean firstBeat = true;        // used to seed rate array so we startup with reasonable BPM
volatile boolean secondBeat = false;      // used to seed rate array so we startup with reasonable BPM


OneWire one_wire( ONE_WIRE_BUS );
DallasTemperature sensors( &one_wire );



typedef enum Mode_
{
  MODE_NONE       = 0,
  MODE_ECG        = 1,
  MODE_TEMP       = 2,
  MODE_BPM        = 3,
  MODE_TEMP_BPM   = 4
} Mode_t;
int selected_mode = MODE_NONE;
boolean transmission_mode = true;



/* protos :) */
int getBPM();
void transmit( int bpm, int temperature );
double getECG();
double getTemperature();


void setup()
{
  Serial.begin( 115200 );
  Serial1.begin( 115200 );

  pinMode( ECG1, INPUT );
  pinMode( ECG2, INPUT );

  sensors.begin();

  Serial.println( "Resetting ESP..." );
  pinMode( ESP_RESET, OUTPUT );
  digitalWrite( ESP_RESET, 0 );
  delay( 2000 );
  digitalWrite( ESP_RESET, 1 );


  Serial.println( "Waiting for ESP connectivity..." );
  while ( 1 )
  {
    while ( Serial1.available() > 0 ) Serial1.read();

    Serial1.print( 'r' );
    delay( 50 );
    if ( Serial1.read() != 'r' )
    {
      Serial.print( "." );
      delay( 2000 );
      continue;
    }
    else break;
  }
  Serial.println();
  Serial.println( "ESP ready OK." );


   interruptSetup();                 // sets up to read Pulse Sensor signal every 2mS
                                    // IF YOU ARE POWERING The Pulse Sensor AT VOLTAGE LESS THAN THE BOARD VOLTAGE,
                                    // UN-COMMENT THE NEXT LINE AND APPLY THAT VOLTAGE TO THE A-REF PIN
                                    //   analogReference(EXTERNAL);
}

void loop()
{
  if ( Serial.available() > 0 )
  {
    String mode = Serial.readString();
    if ( mode.indexOf("temp") != -1 )
      selected_mode = MODE_TEMP;
    else if ( mode.indexOf("ecg") != -1 )
    {
      selected_mode = MODE_ECG;
      Serial.println( "ECG mode. You can change this mode at any time. " );
      Serial.println( "Options: temp, ecg, bpm, temp+bpm, transmission=[true/false], none" );
      delay( 2000 );
    }
    else if ( mode.indexOf("bpm") != -1 )
      selected_mode = MODE_BPM;
    else if ( mode.indexOf("transmission=true") != -1 )
    {
      transmission_mode = true;
      Serial.println( "Transmission ON." );
    }
    else if ( mode.indexOf("transmission=false") != -1 )
    {
      transmission_mode = false;
      Serial.println( "Transmission OFF." );
    }
    else if ( mode.indexOf("temp+bpm") != -1 )
      selected_mode = MODE_TEMP_BPM;
    else if ( mode.indexOf("none") != -1 )
      selected_mode = MODE_NONE;
    else
    {
      Serial.println( "Invalid mode :(! Options: temp, ecg, bpm, temp+bpm, transmission=[true/false], none" );
      selected_mode = MODE_NONE;
    }
  }

  if ( selected_mode == MODE_NONE )
  {
    Serial.println( "No mode selected. Options: temp, ecg, bpm, temp+bpm, transmission=[true/false], none" );
    while ( Serial.available() <= 0 ){}
    return;
  }

  if ( selected_mode == MODE_TEMP )
  {
    Serial.println( "Reading temperature.." );
    int temperature = getTemperature();
    Serial.print( "Temperature: " );
    Serial.println( temperature );
    if ( transmission_mode )
      transmit( -1, temperature );
    selected_mode = MODE_NONE;
    return;
  }

  if ( selected_mode == MODE_ECG )
  {
    Serial.print( getECG() );
    Serial.print( " " );
    return;
  }

  if ( selected_mode == MODE_BPM )
  {
    int bpm = getBPM();
    if ( transmission_mode )
      transmit( bpm, -1 );
    selected_mode = MODE_NONE;
    return;
  }

  if ( selected_mode == MODE_TEMP_BPM )
  {
    Serial.println( "Reading temperature.." );
    int temperature = getTemperature();
    Serial.print( "Temperature: " );
    Serial.println( temperature );

    int bpm = getBPM();

    if ( transmission_mode )
      transmit( bpm, temperature );

    selected_mode = MODE_NONE;
    return;
  }
}

void transmit( int bpm, int temperature )
{

  if ( bpm < 0 && temperature < 0 )
  {
    Serial.println( "Both BPM and temperature are below 0. Skipping transmission." );
    return;
  }

  Serial.print( "Transmitting.." );

  Serial1.print( 'r' );
  while ( Serial1.read() != 'r' )
    Serial.print( "." );
  Serial.println( "ESP ready OK." );

  char temp = (char) temperature;
  char hrt  = (char) bpm;

  if ( bpm < 0 )
  {
    String send = "t";
    send += temp;
    #ifndef NDEBUG
      Serial.print( "Sending: " );
      Serial.print( send[0] );
      Serial.print( (int) send[1] );
    #endif /* NDEBUG */
    Serial1.print( send );
  }
  else if ( temperature < 0 )
  {
    String send = "h";
    send += hrt;
    #ifndef NDEBUG
      Serial.print( "Sending: " );
      Serial.print( send[0] );
      Serial.print( (int) send[1] );
    #endif /* NDEBUG */
    Serial1.print( send );
  }
  else
  {
    String send = "b";
    send += temp;
    send += hrt;
    #ifndef NDEBUG
      Serial.print( "Sending: " );
      Serial.print( send[0] );
      Serial.print( (int) send[1] );
      Serial.print( (int) send[2] );
    #endif /* NDEBUG */
    Serial1.print( send );
  }

  Serial.print( "Waiting for confirmation from ESP..." );
  while ( Serial1.read() != 'd' )
    Serial.print( "." );
  Serial.println();
  Serial.println( "ESP confirmed receipt." );

  while ( Serial1.available() > 0 ) Serial1.read();
}

int getBPM()
{
  serialOutput();

  if (QS == true) // A Heartbeat Was Found
    {
      // BPM and IBI have been Determined
      // Quantified Self "QS" true when arduino finds a heartbeat
      serialOutputWhenBeatHappens(); // A Beat Happened, Output that to serial.
      QS = false; // reset the Quantified Self flag for next time
    }

  delay(20); //  take a break
  return BPM;
}

void interruptSetup()
{
  // Initializes Timer2 to throw an interrupt every 2mS.
  TCCR2A = 0x02;     // DISABLE PWM ON DIGITAL PINS 3 AND 11, AND GO INTO CTC MODE
  TCCR2B = 0x06;     // DON'T FORCE COMPARE, 256 PRESCALER
  OCR2A = 0X7C;      // SET THE TOP OF THE COUNT TO 124 FOR 500Hz SAMPLE RATE
  TIMSK2 = 0x02;     // ENABLE INTERRUPT ON MATCH BETWEEN TIMER2 AND OCR2A
  sei();             // MAKE SURE GLOBAL INTERRUPTS ARE ENABLED
}

void serialOutput()
{   // Decide How To Output Serial.
 if (serialVisual == true)
  {
     arduinoSerialMonitorVisual('-', Signal);   // goes to function that makes Serial Monitor Visualizer
  }
 else
  {
      sendDataToSerial('S', Signal);     // goes to sendDataToSerial function
   }
}

void serialOutputWhenBeatHappens()
{
 if (serialVisual == true) //  Code to Make the Serial Monitor Visualizer Work
   {
     Serial.print(" Heart-Beat Found ");  //ASCII Art Madness
     Serial.print("BPM: ");
     Serial.println(BPM/4);
   }
 else
   {
     sendDataToSerial('B',BPM);   // send heart rate with a 'B' prefix
     sendDataToSerial('Q',IBI);   // send time between beats with a 'Q' prefix
   }
}

void arduinoSerialMonitorVisual(char symbol, int data )
{
  const int sensorMin = 0;      // sensor minimum, discovered through experiment
  const int sensorMax = 1024;    // sensor maximum, discovered through experiment
  int sensorReading = data; // map the sensor range to a range of 12 options:
  int range = map(sensorReading, sensorMin, sensorMax, 0, 11);
  // do something different depending on the
  // range value:
}


void sendDataToSerial(char symbol, int data )
{
   Serial.print(symbol);
   Serial.println(data);
}

ISR(TIMER2_COMPA_vect) //triggered when Timer2 counts to 124
{
  cli();                                      // disable interrupts while we do this
  Signal = analogRead(PULSE_PIN);              // read the Pulse Sensor
  sampleCounter += 2;                         // keep track of the time in mS with this variable
  int N = sampleCounter - lastBeatTime;       // monitor the time since the last beat to avoid noise
                                              //  find the peak and trough of the pulse wave
  if(Signal < thresh && N > (IBI/5)*3) // avoid dichrotic noise by waiting 3/5 of last IBI
    {
      if (Signal < T) // T is the trough
      {
        T = Signal; // keep track of lowest point in pulse wave
      }
    }

  if(Signal > thresh && Signal > P)
    {          // thresh condition helps avoid noise
      P = Signal;                             // P is the peak
    }                                        // keep track of highest point in pulse wave

  //  NOW IT'S TIME TO LOOK FOR THE HEART BEAT
  // signal surges up in value every time there is a pulse
  if (N > 250)
  {                                   // avoid high frequency noise
    if ( (Signal > thresh) && (Pulse == false) && (N > (IBI/5)*3) )
      {
        Pulse = true;                               // set the Pulse flag when we think there is a pulse
        digitalWrite(blinkPin,HIGH);                // turn on pin 13 LED
        IBI = sampleCounter - lastBeatTime;         // measure time between beats in mS
        lastBeatTime = sampleCounter;               // keep track of time for next pulse

        if(secondBeat)
        {                        // if this is the second beat, if secondBeat == TRUE
          secondBeat = false;                  // clear secondBeat flag
          for(int i=0; i<=9; i++) // seed the running total to get a realisitic BPM at startup
          {
            rate[i] = IBI;
          }
        }

        if(firstBeat) // if it's the first time we found a beat, if firstBeat == TRUE
        {
          firstBeat = false;                   // clear firstBeat flag
          secondBeat = true;                   // set the second beat flag
          sei();                               // enable interrupts again
          return;                              // IBI value is unreliable so discard it
        }
      // keep a running total of the last 10 IBI values
      word runningTotal = 0;                  // clear the runningTotal variable

      for(int i=0; i<=8; i++)
        {                // shift data in the rate array
          rate[i] = rate[i+1];                  // and drop the oldest IBI value
          runningTotal += rate[i];              // add up the 9 oldest IBI values
        }

      rate[9] = IBI;                          // add the latest IBI to the rate array
      runningTotal += rate[9];                // add the latest IBI to runningTotal
      runningTotal /= 10;                     // average the last 10 IBI values
      BPM = 60000/runningTotal;               // how many beats can fit into a minute? that's BPM!
      QS = true;                              // set Quantified Self flag
      // QS FLAG IS NOT CLEARED INSIDE THIS ISR
    }
  }

  if (Signal < thresh && Pulse == true)
    {   // when the values are going down, the beat is over
      digitalWrite(blinkPin,LOW);            // turn off pin 13 LED
      Pulse = false;                         // reset the Pulse flag so we can do it again
      amp = P - T;                           // get amplitude of the pulse wave
      thresh = amp/2 + T;                    // set thresh at 50% of the amplitude
      P = thresh;                            // reset these for next time
      T = thresh;
    }

  if (N > 2500)
    {                           // if 2.5 seconds go by without a beat
      thresh = 512;                          // set thresh default
      P = 512;                               // set P default
      T = 512;                               // set T default
      lastBeatTime = sampleCounter;          // bring the lastBeatTime up to date
      firstBeat = true;                      // set these to avoid noise
      secondBeat = false;                    // when we get the heartbeat back
    }

  sei();                                   // enable interrupts when youre done!
}// end isr


double getECG()
{
  int ecg;

  if ( (digitalRead(ECG1) == 1 ) || digitalRead(ECG2) == 1 )
    Serial.println( "!" );
  else
  {
    ecg = analogRead( ECG_PIN );
    Serial.println( ecg );
  }

  delay(1);

  return ecg;
}

double getTemperature()
{
  sensors.requestTemperatures();
  return sensors.getTempFByIndex( 0 );
}