#include <AccelStepper.h>
#include <WiFi.h>
#include <WebServer.h>
#include <esp_wifi.h>

///////////////////////////////////
// motor
///////////////////////////////////
// #define dirPin 3
// #define stepPin 2
#define dirPin GPIO_NUM_3
#define stepPin GPIO_NUM_2

#define homeLimitSwitchPin 0
#define endLimitSwitchPin 1

#define LimitSwitchPressed 0
#define LimitSwitchReleased 1

#define enableStepperPin GPIO_NUM_6
constexpr auto motorInterfaceType = AccelStepper::MotorInterfaceType::FULL2WIRE;
//#define motorMaxStepsPerSecond 65535
#define motorMaxStepsPerSecond 1600
// #define motorStepsPerSecond 1600
#define motorStepsPerSecond 800
#define motorAcceleration 30000

//uint8_t newMACAddress[] = {0x32, 0xAE, 0xA4, 0x09, 0x0D, 0x66};

AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);

bool stepperRun{false};
//bool stepperRun{true};

///////////////////////////////////
// wifi
///////////////////////////////////
constexpr bool enableWifi{true};
//constexpr bool enableWifi{false};

int reconnectsCount = 0;
//const char* ssid = "toya99760449_5GHz";
const char* ssid = "toya99760449";
const char* password = "92rK34Umvc";
const uint16_t port{80};

const String accelerationArg = "acceleration";
const String currentPositionArg = "currentPosition";
const String maxSpeedArg = "maxSpeed";
const String speedArg = "speed";
const String stepsArg = "steps";


#define s Serial

WebServer server(port);

///////////////////////////////////
// time
///////////////////////////////////
unsigned long prevMillis{0};
bool step{true};

void handleRoot()
{
  server.send(200, "text/plain", "hello http\n");
}

void handleStart()
{
  server.send(200, "text/plain", "started\n");
  stepperRun = true;
  stepper.enableOutputs();

  //stepper.moveTo(8000);
  bool backward = rand() % 2;
  stepper.move(8000 * (backward ? -1 : 1));
}

void handleSetParams()
{
  String reply;
  
  if (server.hasArg(maxSpeedArg)) {
    const auto maxSpeedStr = server.arg(maxSpeedArg);
    const auto maxSpeed = maxSpeedStr.toFloat();
    stepper.setMaxSpeed(maxSpeed);
    reply += "set " + maxSpeedArg + " to " + maxSpeedStr + '\n';
  }
  
  if (server.hasArg(speedArg)) {
    const auto speedStr = server.arg(speedArg);
    const auto speed = speedStr.toFloat();
    stepper.setSpeed(speed);
    reply += "set " + speedArg + " to " + speedStr + '\n';
  }
  
  if (server.hasArg(currentPositionArg)) {
    const auto currentPositionStr = server.arg(currentPositionArg);
    const auto currentPosition = currentPositionStr.toFloat();
    stepper.setCurrentPosition(currentPosition);
    reply += "set " + currentPositionArg + " to " + currentPositionStr + '\n';
  }
  
  if (server.hasArg(accelerationArg)) {
    const auto accelerationStr = server.arg(accelerationArg);
    const auto acceleration = accelerationStr.toFloat();
    stepper.setAcceleration(acceleration);
    reply += "set " + accelerationArg + " to " + accelerationStr + '\n';
  }

  if (reply.length() == 0)
  {
    server.send(500, "text/plain", "missing params\n");
  }
  
  server.send(200, "text/plain", reply);
}

void handleMove()
{
  // TODO: implement 'mm' floating-point argument
  if (!server.hasArg(stepsArg)) {
    server.send(500, "text/plain", "'steps' arg is missing\n");
    return;
  }

  const auto stepsStr = server.arg(stepsArg);
  s.print("handleMove: arg(stepsArg): ");
  s.println(stepsStr);
  
  server.send(200, "text/plain", "move " + stepsStr + " steps\n");
  stepperRun = true;
  stepper.enableOutputs();

  //stepper.moveTo(8000);
  const auto steps = stepsStr.toInt();
  //bool backward = rand() % 2;
  //stepper.move(8000 * (backward ? -1 : 1));
  stepper.move(steps);
}

void handleStop()
{
  server.send(200, "text/plain", "stopped\n");
  stepperRun = false;
  stepper.disableOutputs();
}

void handleDisconnectWiFi()
{
  server.send(200, "text/plain", "disconnecting\n");
  delay(500);
  WiFi.disconnect();
  s.println("disconnected from WiFi");
}

void setupWifi()
{
  //delay(1000);
  if (!WiFi.begin(ssid, password, 10))
  {
    s.println("cannot begin wifi");
  }
  
  //WiFi.setTxPower(WIFI_POWER_MINUS_1dBm);
  //WiFi.setTxPower(WIFI_POWER_2dBm);
  WiFi.setTxPower(WIFI_POWER_15dBm);
  //WiFi.setTxPower(WIFI_POWER_8_5dBm);
  
  while (WiFi.status() != WL_CONNECTED) {
  delay(500);
  s.print("connecting ");
  s.println(reconnectsCount);
  ++reconnectsCount;
  }
  
  s.println("");
  s.println("WiFi connected.");
  s.println("IP address: ");
  s.println(WiFi.localIP());
  
  server.on("/", handleRoot);
  server.on("/start", handleStart);
  server.on("/setParams", handleSetParams);
  server.on("/move", handleMove);
  server.on("/stop", handleStop);
  server.on("/disconnectWiFi", handleDisconnectWiFi);
  
  server.begin();
  s.println("HTTP server started");
}

bool atHome()
{
  return digitalRead(homeLimitSwitchPin) == LimitSwitchPressed;
}

bool atEnd()
{
  return digitalRead(endLimitSwitchPin) == LimitSwitchPressed;
}

void setup() {
  pinMode(homeLimitSwitchPin, INPUT);
  pinMode(endLimitSwitchPin, INPUT);
  
  s.begin(115200);

  if (enableWifi)
  {
    setupWifi();
  }

  stepper.setMaxSpeed(motorMaxStepsPerSecond);
  stepper.setEnablePin(enableStepperPin);
  stepper.setPinsInverted(false, false, true); // invert enable pin
  stepper.setSpeed(motorStepsPerSecond);
  stepper.setAcceleration(motorAcceleration);
}

void loop() {
  const bool doStop = (stepperRun == true && (atHome() || atEnd()));

  if (doStop)
  {
    stepperRun = false;
    stepper.disableOutputs();
  }

  if (stepperRun && stepper.distanceToGo() != 0)
  {
    stepper.run();
    // processing wifi requests takes too much time,
    // so returning here. limit switches should do the rest
    return;
  }
  else
  {
    stepperRun = false;
    //stepper.disableOutputs();
  }
  
  if (enableWifi)
  {
    server.handleClient();
  }
}