#include <Stepper.h>
#include <neotimer.h>
#include "CommandHandler.h"
// Wind vane and stepper motor logic
#define steps_per_revolution 4096
Stepper stepper(steps_per_revolution, 3, 2);
int16_t target_step = 0;
int16_t current_step = 0;
int16_t previous_step = current_step;

#define girouette_pin A1
#define MAX_MESSAGE_LENGTH 24

CommandHandler commandHandler(10, 5, 64);

float step_values[] = {0, 256, 512, 768, 1024, 1280, 1536, 1792, 2048, 2304, 2560, 2816, 3072, 3328, 3584, 3840};
int voltage_values[] = {788, 705, 889, 830, 946, 602, 633, 246, 92, 126, 185, 65, 289, 83, 464, 408};

Neotimer rotateTimer = Neotimer(5000);

String calibrateCommand(const String args[], size_t argCount) {
    if (argCount == 0) { // Check argument count instead of args.empty()
        return "Error: Missing arguments";
    }
    int target_step = constrain(args[0].toInt(), -15, 15) * 256;
    // Assuming 'stepper' and 'current_step' are already defined
    stepper.step(target_step);
    current_step = 0;
    rotateTimer.reset();
    rotateTimer.start();
    return "Calibrated by " + String(target_step);
}

String echoCommand(const String args[], size_t argCount) {
    String result = "";
    for (size_t i = 0; i < argCount; i++) {
        result += args[i] + " ";
    }
    result.trim(); // Modify result in place
    return result;
}

String infoCommand(const String args[], size_t argCount) {
    if (argCount == 0) {
        return "No arguments provided";
    } else {
        String response = "Arguments: ";
        for (size_t i = 0; i < argCount; i++) {
            response += args[i] + " ";
        }
        response.trim();
        return response;
    }
}

String multiplyCommand(const String* args, size_t argCount) {
    int a = args[0].toInt();
    int b = args[1].toInt();
    return "Result: " + String(a * b);
}

void setup() {
  stepper.setSpeed(10);
  Serial.begin(115200);
  Serial.println("Ready!");
  rotateTimer.start();
  CommandHandler::ArgType calibArgs[] = {CommandHandler::INT};
  commandHandler.registerCommand("calib", calibrateCommand, "Calibrates the stepper motor by the given offset.", 1, 1, calibArgs, 1);
  CommandHandler::ArgType echoInfoArgs[] = {CommandHandler::STRING, CommandHandler::INT};
  commandHandler.registerCommand("echo", echoCommand, "Returns the inputted arguments.", 1, 5, echoInfoArgs, 2);
  commandHandler.registerCommand("info", infoCommand, "", 0, 5, echoInfoArgs, 2);
  // Define the argument types: INT, INT
  CommandHandler::ArgType multiplyArgs[] = {
      CommandHandler::INT,
      CommandHandler::INT
  };

  commandHandler.registerCommand(
      "multiply",
      multiplyCommand,
      "Multiplies two integers.",
      2, 2,  // Requires exactly 2 arguments
      multiplyArgs, 2
  );
}

void loop() {
  if (rotateTimer.done()) {
    target_step = convert_voltage_to_steps(analogRead(girouette_pin));
    stepper.step(target_step - current_step);
    previous_step = current_step;
    current_step = target_step;
    rotateTimer.reset();
    rotateTimer.start();
  }
  commandHandler.loop();
}

int convert_voltage_to_steps(int voltage) {
    int closest_index = 0;
    float min_difference = abs(voltage_values[0] - voltage);
    for (int i = 1; i < 16; ++i) {
        float difference = abs(voltage_values[i] - voltage);
        if (difference < min_difference) {
            min_difference = difference;
            closest_index = i;
        }
    }
    return step_values[closest_index];
}