#include "EggDuino.h"
#include "motion_planner.h"

namespace
{
#ifdef ESP32
   constexpr char kDeviceNamePrefix[] = "EggBot_";
#endif

   int clampServoAngle(int angle)
   {
      if (angle < 0)
      {
         return 0;
      }
      if (angle > 180)
      {
         return 180;
      }
      return angle;
   }

   uint_fast16_t servoSpeedFromRate(int rate)
   {
      // EBB rate values are implementation-specific. We map them to ServoEasing degrees/second.
      // Higher rate means faster movement.
      if (rate <= 0)
      {
         return 70;
      }
      int speed = 20 + (rate / 2);
      if (speed < 10)
      {
         speed = 10;
      }
      if (speed > 360)
      {
         speed = 360;
      }
      return (uint_fast16_t)speed;
   }
}

#ifdef ESP32
void buildDeviceName(char *nameBuffer, size_t bufferSize)
{
   if ((nameBuffer == NULL) || (bufferSize == 0))
   {
      return;
   }

   // Tasmota-style chip ID uses the lower 24 bits of the ESP32 efuse MAC.
   const uint32_t chipId = static_cast<uint32_t>(ESP.getEfuseMac() & 0xFFFFFFULL);
   snprintf(nameBuffer, bufferSize, "%s%06X", kDeviceNamePrefix, chipId);
}
#endif

void updateStepCorrectionFactors()
{
   if (g_iRotMicrostep <= 0)
   {
      g_iRotMicrostep = kDefaultRotMicrostep;
   }
   if (g_iPenMicrostep <= 0)
   {
      g_iPenMicrostep = kDefaultPenMicrostep;
   }
   fROT_STEP_CORRECTION = 16.0f / (float)g_iRotMicrostep;
   fPEN_STEP_CORRECTION = 16.0f / (float)g_iPenMicrostep;
}

XYMotionPlanner *g_pPlanner = nullptr;

void initHardware()
{
   if (!initConfigStore())
   {
      g_iPenUpPos = 5;
      g_iPenDownPos = 20;
   }
   updateStepCorrectionFactors();
   g_iPenState = g_iPenDownPos;

   g_stepEngine.init();
   g_pStepperRotate = g_stepEngine.stepperConnectToPin(g_iRotStepPin);
   if (g_pStepperRotate)
   {
      // rotMotor.setMaxSpeed(2000.0);
      // rotMotor.setAcceleration(10000.0);

      g_pStepperRotate->setDirectionPin(g_iRotDirPin);
      g_pStepperRotate->setEnablePin(g_iRotEnablePin);
      g_pStepperRotate->setAcceleration(g_iMaxAcclSpeed);
      g_pStepperRotate->setAutoEnable(false);
   }

   // Stepper pen init
   g_pStepperPen = g_stepEngine.stepperConnectToPin(g_iPenStepPin);
   if (g_pStepperPen)
   {
      // penMotor.setMaxSpeed(2000.0);
      // penMotor.setAcceleration(10000.0);
      g_pStepperPen->setDirectionPin(g_iPenDirPin);
      g_pStepperPen->setEnablePin(g_iPenEnablePin);
      g_pStepperPen->setAcceleration(g_iMaxAcclSpeed);
      g_pStepperPen->setAutoEnable(false);
   }

   motorsOff();

   g_pPlanner = new XYMotionPlanner(g_pStepperPen, g_pStepperRotate);
   g_pPlanner->setBaseLimits(4000, g_iMaxAcclSpeed);
   g_pPlanner->setMinimums(50, 100);
   g_pPlanner->setCurrentPosition(0, 0);

   g_iPenState = clampServoAngle(g_iPenState);
   penServo.attach(g_iServoPin, g_iPenState);
   penServo.setEasingType(EASE_QUADRATIC_IN_OUT);
}

void movePenServoTo(int targetPos)
{
   targetPos = clampServoAngle(targetPos);
   int currentPos = clampServoAngle(g_iPenState);
   if (currentPos == targetPos)
   {
      return;
   }

   uint_fast16_t speed = 0;
   speed = servoSpeedFromRate(g_iServoRateDown);
   penServo.easeTo(targetPos, speed);

   g_iPenState = targetPos;
}

void storePenUpPosInEE()
{
   saveConfigToFile();
}

void storePenDownPosInEE()
{
   saveConfigToFile();
}

void sendAck()
{
   Log(__FUNCTION__);
   protocolWrite("OK\r\n");
}

void sendError()
{
   Log(__FUNCTION__);
   protocolWrite("unknown CMD\r\n");
}

void motorsOff()
{
   Log(__FUNCTION__);
   g_pStepperPen->disableOutputs();
   g_pStepperRotate->disableOutputs();
   g_bMotorsEnabled = 0;
}

void motorsOn()
{
   Log(__FUNCTION__);
   g_pStepperPen->enableOutputs();
   g_pStepperRotate->enableOutputs();
   g_bMotorsEnabled = 1;
}

void toggleMotors()

{
   Log(__FUNCTION__);
   if (g_bMotorsEnabled)
   {
      motorsOff();
   }
   else
   {
      motorsOn();
   }
}

bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB)
{
   char *arg1 = NULL;
   char *arg2 = NULL;
   char *arg3 = NULL;
   arg1 = nextCommandArg();
   if (arg1 != NULL)
   {
      *duration = atoi(arg1);
      arg2 = nextCommandArg();
   }
   if (arg2 != NULL)
   {
      *penStepsEBB = atoi(arg2);
      arg3 = nextCommandArg();
   }
   if (arg3 != NULL)
   {
      *rotStepsEBB = atoi(arg3);

      return true;
   }

   return false;
}

void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB)
{
   if (!g_bMotorsEnabled)
   {
      motorsOn();
   }

   g_pPlanner->moveBy(penStepsEBB / fPEN_STEP_CORRECTION, rotStepsEBB / fROT_STEP_CORRECTION);
}

void moveOneStep()
{
   Log("moveOneStep");
   while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning() || g_pPlanner->isRunning())
   {
      g_pPlanner->update();
   }
   g_pPlanner->update();
   Log("done");
}

void moveToDestination()
{
   Log("moveToDestination");
   while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning() || g_pPlanner->isRunning())
   {
      g_pPlanner->update();
   }
   g_pPlanner->update();
   Log("done");
}

void setprgButtonState()
{
   g_bPrgButtonState = 1;
}
