#include "components/motion/MotionController.h"

//#include <task.h>

#include "utility/Math.h"

using namespace Pinetime::Controllers;

namespace {
  constexpr inline int32_t Clamp(int32_t val, int32_t min, int32_t max) {
    return val < min ? min : (val > max ? max : val);
  }

  // only returns meaningful values if inputs are acceleration due to gravity
  int16_t DegreesRolled(int16_t y, int16_t z, int16_t prevY, int16_t prevZ) {
    int16_t prevYAngle = Pinetime::Utility::Asin(Clamp(prevY * 32, -32767, 32767));
    int16_t yAngle = Pinetime::Utility::Asin(Clamp(y * 32, -32767, 32767));

    if (z < 0 && prevZ < 0) {
      return yAngle - prevYAngle;
    }
    if (prevZ < 0) {
      if (y < 0) {
        return -prevYAngle - yAngle - 180;
      }
      return -prevYAngle - yAngle + 180;
    }
    if (z < 0) {
      if (y < 0) {
        return prevYAngle + yAngle + 180;
      }
      return prevYAngle + yAngle - 180;
    }
    return prevYAngle - yAngle;
  }
}

void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
//  if (this->nbSteps != nbSteps && service != nullptr) {
//    service->OnNewStepCountValue(nbSteps);
//  }
//
//  if (service != nullptr && (this->x != x || yHistory[0] != y || zHistory[0] != z)) {
//    service->OnNewMotionValues(x, y, z);
//  }
//
//  lastTime = time;
//  time = xTaskGetTickCount();

  lastX = this->x;
  this->x = x;
  yHistory++;
  yHistory[0] = y;
  zHistory++;
  zHistory[0] = z;

  stats = GetAccelStats();

  int32_t deltaSteps = nbSteps - this->nbSteps;
  if (deltaSteps > 0) {
    currentTripSteps += deltaSteps;
  }
  this->nbSteps = nbSteps;
}

MotionController::AccelStats MotionController::GetAccelStats() const {
  AccelStats stats;

  for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
    stats.yMean += yHistory[histSize - i];
    stats.zMean += zHistory[histSize - i];
    stats.prevYMean += yHistory[1 + i];
    stats.prevZMean += zHistory[1 + i];
  }
  stats.yMean /= AccelStats::numHistory;
  stats.zMean /= AccelStats::numHistory;
  stats.prevYMean /= AccelStats::numHistory;
  stats.prevZMean /= AccelStats::numHistory;

  for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
    stats.yVariance += (yHistory[histSize - i] - stats.yMean) * (yHistory[histSize - i] - stats.yMean);
    stats.zVariance += (zHistory[histSize - i] - stats.zMean) * (zHistory[histSize - i] - stats.zMean);
  }
  stats.yVariance /= AccelStats::numHistory;
  stats.zVariance /= AccelStats::numHistory;

  return stats;
}

bool MotionController::ShouldRaiseWake() const {
  return false;
//  constexpr uint32_t varianceThresh = 56 * 56;
//  constexpr int16_t xThresh = 384;
//  constexpr int16_t yThresh = -64;
//  constexpr int16_t rollDegreesThresh = -45;
//
//  if (x < -xThresh || x > xThresh) {
//    return false;
//  }
//
//  // if the variance is below the threshold, the accelerometer values can be considered to be from acceleration due to gravity
//  if (stats.yVariance > varianceThresh || (stats.yMean < -724 && stats.zVariance > varianceThresh) || stats.yMean > yThresh) {
//    return false;
//  }
//
//  return DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < rollDegreesThresh;
}

bool MotionController::ShouldShakeWake(uint16_t thresh) {
  return false;
//  /* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */
//  int32_t speed =
//    std::abs(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 + (x - lastX) / 4) * 100 / (time - lastTime);
//  // (.2 * speed) + ((1 - .2) * accumulatedSpeed);
//  accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5;
//
//  return accumulatedSpeed > thresh;
}

bool MotionController::ShouldLowerSleep() const {
  return false;
//  if (stats.yMean < 724 || DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < 30) {
//    return false;
//  }
//
//  for (uint8_t i = AccelStats::numHistory + 1; i < yHistory.Size(); i++) {
//    if (yHistory[i] < 265) {
//      return false;
//    }
//  }
//
//  return true;
}

void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) {
  switch (types) {
    case Drivers::Bma421::DeviceTypes::BMA421:
      this->deviceType = DeviceTypes::BMA421;
      break;
    case Drivers::Bma421::DeviceTypes::BMA425:
      this->deviceType = DeviceTypes::BMA425;
      break;
    default:
      this->deviceType = DeviceTypes::Unknown;
      break;
  }
}