#include "HeartRateTask.h"
#include <drivers/Hrs3300.h>
#include <components/heartrate/HeartRateController.h>
#include <nrf_log.h>

using namespace Pinetime::Applications;

HeartRateTask::HeartRateTask(Drivers::Hrs3300 &heartRateSensor, Controllers::HeartRateController& controller) :
      heartRateSensor{heartRateSensor},
      controller{controller},
      ppg{static_cast<float>(heartRateSensor.ReadHrs())} {
  messageQueue = xQueueCreate(10, 1);
  controller.SetHeartRateTask(this);
}

void HeartRateTask::Start() {
  if (pdPASS != xTaskCreate(HeartRateTask::Process, "Heartrate", 500, this, 0, &taskHandle))
    APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
}

void HeartRateTask::Process(void *instance) {
  auto *app = static_cast<HeartRateTask *>(instance);
  app->Work();
}

void HeartRateTask::Work() {
  int lastBpm = 0;
  while (true) {
    Messages msg;
    uint32_t delay;
    if (state == States::Running) {
      if (measurementStarted) delay = 40;
      else delay = 100;
    } else
      delay = portMAX_DELAY;

    if (xQueueReceive(messageQueue, &msg, delay)) {
      switch (msg) {
        case Messages::GoToSleep:
          StopMeasurement();
          state = States::Idle;
          break;
        case Messages::WakeUp:
          state = States::Running;
          break;
        case Messages::StartMeasurement:
          lastBpm = 0;
          StartMeasurement();
          ppg.SetOffset(heartRateSensor.ReadHrs());
          break;
        case Messages::StopMeasurement:
          StopMeasurement();
          break;
      }
    }

    if (measurementStarted) {
      auto hrs = heartRateSensor.ReadHrs();
      ppg.Preprocess(hrs);
      auto bpm = ppg.HeartRate();

      if (lastBpm == 0 && bpm == 0) controller.Update(Controllers::HeartRateController::States::NoTouch, 0);
      if(bpm != 0) {
        lastBpm = bpm;
        controller.Update(Controllers::HeartRateController::States::Running, lastBpm);
      }
    }
  }
}

void HeartRateTask::PushMessage(HeartRateTask::Messages msg) {
  BaseType_t xHigherPriorityTaskWoken;
  xHigherPriorityTaskWoken = pdFALSE;
  xQueueSendFromISR(messageQueue, &msg, &xHigherPriorityTaskWoken);
  if (xHigherPriorityTaskWoken) {
    /* Actual macro used here is port specific. */
    // TODO : should I do something here?
  }
}

void HeartRateTask::StartMeasurement() {
  heartRateSensor.Enable();
  measurementStarted = true;
}

void HeartRateTask::StopMeasurement() {
  heartRateSensor.Disable();
  measurementStarted = false;
}