PulseAlign Listen to your heart pulse

/*
 *  player.ino - Simple sound player example application
 *  Copyright 2018 Sony Semiconductor Solutions Corporation
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <SDHCI.h>
#include <Audio.h>

SDClass theSD;
AudioClass *theAudio;

File myFile;

#define MP3HEADER_OFFSET 

bool ErrEnd = false;

/**
 * @brief Audio attention callback
 *
 * When audio internal error occurc, this function will be called back.
 */

static void audio_attention_cb(const ErrorAttentionParam *atprm)
{
  puts("Attention!");
  
  if (atprm->error_code >= AS_ATTENTION_CODE_WARNING)
    {
      ErrEnd = true;
   }
}



class HeartRate {
  private:
    const int max_heartpulse_duty = 2000;
    bool data_effect = true;
    unsigned char counter;
    unsigned long temp[21];
    unsigned long sub;
  public:
    unsigned int heart_rate = 80;
    boolean debug;

    unsigned int getRate() {
      return heart_rate;
    }

    void sum() {
      if (data_effect) {
        heart_rate = 1200000 / (temp[20] - temp[0]); // 60 * 20 * 1000 / 20_total_time;
        printf("Has heat rate %d\n", heart_rate);
      }
      data_effect = 1;
    }

    void interrupt() {
      temp[counter] = millis();
      if (counter == 0) {
        sub = temp[counter] - temp[20];
      } else {
        sub = temp[counter] - temp[counter - 1];
      }
      if (sub > max_heartpulse_duty) {
        data_effect = 0;
        counter = 0;
        arrayInit();
      }
      if (counter == 20 && data_effect) {
        counter = 0;
        sum();
      } else if (counter != 20 && data_effect) {
        counter += 1;
      } else {
        counter = 0;
        data_effect = 1;
      }
    }

    void arrayInit() {
      for (unsigned char i = 0; i < 20; i++) {
        temp[i] = 0;
      }
      temp[20] = millis();
    }


    int started = 0;

    void initPlaying() {
      started = millis();
    }

    bool hasBeat() {
      // beats/minute * 1/beats_minute*60/1000 = beat_minute*1000
      printf("HR: %d", getRate());
      if (millis() - started > getRate() * 16) {
        started = millis();
        puts("restarting -- beat");
        return true;
      }
      return false;
    }

    boolean lastSeen = true;
    void updateState(boolean state) {
      if (state != lastSeen) {
        if (!lastSeen && state) {
          printf("HR SEEN %d\n", getRate());
          interrupt();
        }
        lastSeen = state;
      }
    }  
};

HeartRate r1;


void setup() {
  // start audio system
  theAudio = AudioClass::getInstance();

  theAudio->begin(audio_attention_cb);

  /* Set clock mode to normal */
  theAudio->setRenderingClockMode(AS_CLKMODE_NORMAL);
  theAudio->setPlayerMode(AS_SETPLAYER_OUTPUTDEVICE_SPHP, AS_SP_DRV_MODE_LINEOUT);
  err_t err = theAudio->initPlayer(AudioClass::Player0, AS_CODECTYPE_MP3, "/mnt/sd0/BIN", AS_SAMPLINGRATE_AUTO, AS_CHANNEL_STEREO);

  /* Verify player initialize */
  if (err != AUDIOLIB_ECODE_OK) {
      printf("Player0 initialize error\n");
      exit(1);
    }

  /* Open file placed on SD card */
  myFile = theSD.open("/BIN/shm.mp3");

  /* Verify file open */
  if (!myFile) {
      printf("File open error\n");
      exit(1);
    }
  printf("Open! %d\n",myFile);

  myFile.seek(0);

  /* Send first frames to be decoded */
  err = theAudio->writeFrames(AudioClass::Player0, myFile);

  if ((err != AUDIOLIB_ECODE_OK) && (err != AUDIOLIB_ECODE_FILEEND)) {
      printf("File Read Error! =%d\n",err);
      myFile.close();
      exit(1);
    }

  puts("Play!");

  // Setup HR PinMode
  pinMode(3, INPUT);

  /* Main volume set to -16.0 dB */
  theAudio->setVolume(-160);
  theAudio->startPlayer(AudioClass::Player0);

  r1.arrayInit();
  r1.initPlaying();

  pinMode(LED0, OUTPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  
}

int lastMillis = 0;
void loop() {
  r1.updateState(digitalRead(3));
  digitalWrite(LED0, digitalRead(3));
  digitalWrite(LED1, !digitalRead(3));
  puts(digitalRead(3) ? "HasHR": "NoHR");
  if (r1.hasBeat()) {
    digitalWrite(LED2, HIGH);
    digitalWrite(LED3, LOW);
    myFile.seek(44);
    lastMillis = millis();
  } else {
    digitalWrite(LED2, LOW);
    digitalWrite(LED3, HIGH);
  }
  

  int err = theAudio->writeFrames(AudioClass::Player0, myFile);

  //  Tell when player file ends
  if (err == AUDIOLIB_ECODE_FILEEND) {
     myFile.seek(44);
  }
/*

  // Show error code from player and stop
  if (err != AUDIOLIB_ECODE_FILEEND) {
      printf("Main player error code: %d\n", err);
      goto stop_player;
    }

  if (ErrEnd) {
      printf("Error End\n");
      goto stop_player;
    }
*/

  /* This sleep is adjusted by the time to read the audio stream file.
     Please adjust in according with the processing contents
     being processed at the same time by Application.
  */

  usleep(40000);


  /* Don't go further and continue play */
  return;

stop_player:
  sleep(1);
  theAudio->stopPlayer(AudioClass::Player0);
  myFile.close();
  exit(1);
}