#include "./motorstate.h"
#include "./motor.h"
#include "./state.h"
#include "./settings.h"
#include "./debug.h"
#include "include/config.h"


void motorStateMoveTo(uint16_t height)
{
  dl("motorStateMoveTo: "); dln(height);

  State.MoveTarget = height;
  State.MoveDirection = height > State.CurrentHeight ? Direction::Up : Direction::Down;
}


bool motorStateCheckTargetReached()
{
  dl("motorStateCheckTargetReached: direction = "); dl((uint8_t)State.MoveDirection); dl(", currentHeight = "); dln(State.CurrentHeight);

  switch (State.MoveDirection)
  {
    case Direction::Up:
      if (State.CurrentHeight >= State.MoveTarget - Config::HeightMeasurementDeltaStop)
      {
        if (State.CurrentHeight - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget)
          State.CurrentHeight = State.MoveTarget;

        motorStateStop();
        return true;
      }
      break;

    case Direction::Down:
      if (State.CurrentHeight <= State.MoveTarget + Config::HeightMeasurementDeltaStop)
      {
        if (State.MoveTarget - State.CurrentHeight <= Config::HeightMeasurementDeltaOnTarget)
          State.CurrentHeight = State.MoveTarget;

        motorStateStop();
        return true;
      }
      break;

    default:
      break;
  }

  return false;
}


bool motorStateCheckOverCurrent()
{
  if (motorIsOverCurrent())
  {
    dln("motorStateCheckOverCurrent: overcurrent detected!");

    motorStateStop();
    return true;
  }

  return false;
}


void motorStateStop()
{
  dln("motorStateStop");

  motorStop();
  State.MoveDirection = Direction::None;
}


void getDisplayHeight(char* buffer, uint16_t value)
{
  uint8_t displayValue = (value + Settings.Height.Offset) / 10;

  if (displayValue > 99)
    buffer[0] = '0' + ((displayValue / 100) % 10);
  else
    buffer[0] = '0';

  buffer[1] = '.';
  buffer[2] = '0' + ((displayValue / 10) % 10);
  buffer[3] = '0' + (displayValue % 10);
  buffer[4] = 'm';
  buffer[5] = 0;
}