#include <ArduinoJson.h>
#include <Ramp.h>



int move_time = 1000;
char default_list[] = "{'Home':{'A':90,'B':65,'C':155,'D':60}}";
String default_pos = "Home";



DynamicJsonDocument position_list(1024);

ramp ramp_a;
ramp ramp_b;
ramp ramp_c;
ramp ramp_d;

String serial_line = "";

bool auto_execute = true;

int target_a;
int target_b;
int target_c;
int target_d;

int mom_a;
int mom_b;
int mom_c;
int mom_d;


void move(int time) {
  ramp_a.go(mom_a, time);
  ramp_b.go(mom_b, time);
  ramp_c.go(mom_c, time);
  ramp_d.go(mom_d, time);
}


void setup() {
  Serial.begin(115200);

  deserializeJson(position_list, default_list);
  target_a = position_list[default_pos]["A"];
  target_b = position_list[default_pos]["B"];
  target_c = position_list[default_pos]["C"];
  target_d = position_list[default_pos]["D"];
  mom_a = position_list[default_pos]["A"];
  mom_b = position_list[default_pos]["B"];
  mom_c = position_list[default_pos]["C"];
  mom_d = position_list[default_pos]["D"];
  

  ledcSetup(1, 50, 16);
  ledcSetup(2, 50, 16);
  ledcSetup(3, 50, 16);
  ledcSetup(4, 50, 16);
  ledcAttachPin(13, 1);
  ledcAttachPin(12, 2);
  ledcAttachPin(14, 3);
  ledcAttachPin(27, 4);


  move(3000);
  Serial.println("[RoboCMD] Initialisierung abgeschlossen.");
}

void loop() {
  if (Serial.available() > 0) {
    serial_line = Serial.readString();
    serial_line.replace("\n", "");
    if (serial_line == "auto") {
      auto_execute = !auto_execute;
      Serial.print("Automatisches ausführen ist jetzt ");
      if (auto_execute) {
        Serial.println("Aktiv.");
      } else {
        Serial.println("Inaktiv.");
      }
    } else if (serial_line == "export") {
      String exported_list;
      serializeJson(position_list, exported_list);
      Serial.println(exported_list);
    } else if (serial_line.substring(0, 5) == "time ") {
        move_time = serial_line.substring(5).toInt();
        Serial.print("Eine bewegung dauert nun ");
        Serial.print(move_time / 1000);
        Serial.println(" Sekunden");
    } else if (serial_line.substring(0, 5) == "goto ") {
      if (position_list.containsKey(serial_line.substring(5))) {
        mom_a = position_list[serial_line.substring(5)]["A"];
        mom_b = position_list[serial_line.substring(5)]["B"];
        mom_c = position_list[serial_line.substring(5)]["C"];
        mom_d = position_list[serial_line.substring(5)]["D"];
        Serial.print("Zu '");
        Serial.print(serial_line.substring(5));
        Serial.println("' gegangen!");
      } else {
        Serial.print("[ERROR] '");
        Serial.print(serial_line.substring(5));
        Serial.println("' konnte nicht gefunden werden!");
      }
    } else if (serial_line.substring(0, 7) == "target ") {
      if (position_list.containsKey(serial_line.substring(7))) {
        target_a = position_list[serial_line.substring(7)]["A"];
        target_b = position_list[serial_line.substring(7)]["B"];
        target_c = position_list[serial_line.substring(7)]["C"];
        target_d = position_list[serial_line.substring(7)]["D"];
        Serial.print("Position '");
        Serial.print(serial_line.substring(7));
        Serial.println("' wurde als Ziel festgelegt!");
      } else {
        Serial.print("[ERROR] '");
        Serial.print(serial_line.substring(7));
        Serial.println("' konnte nicht gefunden werden!");
      }
    } else if (serial_line.substring(0, 5) == "save ") {
      if (!position_list.containsKey(serial_line.substring(5))) {
        position_list[serial_line.substring(5)]["A"] = mom_a;
        position_list[serial_line.substring(5)]["B"] = mom_b;
        position_list[serial_line.substring(5)]["C"] = mom_c;
        position_list[serial_line.substring(5)]["D"] = mom_d;
        Serial.print("Als '");
        Serial.print(serial_line.substring(5));
        Serial.println("' gespeichert!");
      } else {
        Serial.print("[ERROR] '");
        Serial.print(serial_line.substring(5));
        Serial.println("' existiert bereits!");
      }
    } else if (serial_line.substring(0, 4) == "del ") {
      if (position_list.containsKey(serial_line.substring(4))) {
        position_list.remove(serial_line.substring(4));
        Serial.print("Position '");
        Serial.print(serial_line.substring(4));
        Serial.println("' gelöscht!");
      } else {
        Serial.print("[ERROR] '");
        Serial.print(serial_line.substring(4));
        Serial.println("' konnte nicht gefunden werden!");
      }
    } else if (serial_line.substring(0, 5) == "load ") {
      deserializeJson(position_list, serial_line.substring(5));
      Serial.print(position_list.size());
      Serial.println(" Positionen geladen!");
    } else if (serial_line.substring(0, 4) == "list") {
      Serial.println("  -=== Gespeicherte Positionen ===-");
      JsonObject root = position_list.as<JsonObject>();
      int n = 0;
      for (JsonPair kv : root) {
        n++;
        Serial.print(n);
        Serial.print(". '");
        Serial.print(kv.key().c_str());
        Serial.print("'");
        if (serial_line.substring(4, 5) == "v") {
          Serial.print(": ");
          serializeJson(kv.value().as<JsonObject>(), Serial);
        }
        Serial.println();
      }
    } else if (serial_line.substring(0, 1) == "a") {
      target_a = serial_line.substring(1).toInt();
      if (auto_execute) {
        Serial.print("Die Postion von Motor A wurde auf ");
        mom_a = target_a;
      } else {
        Serial.print("Das Ziel von Motor A wurde auf ");
      }
      Serial.print(target_a);
      Serial.println("° gesetzt");
    } else if (serial_line.substring(0, 1) == "b") {
      target_b = serial_line.substring(1).toInt();
      if (auto_execute) {
        Serial.print("Die Postion von Motor B wurde auf ");
        mom_b = target_b;
      } else {
        Serial.print("Das Ziel von Motor B wurde auf ");
      }
      Serial.print(target_b);
      Serial.println("° gesetzt");
    } else if (serial_line.substring(0, 1) == "c") {
      target_c = serial_line.substring(1).toInt();
      if (auto_execute) {
        Serial.print("Die Postion von Motor C wurde auf ");
        mom_c = target_c;
      } else {
        Serial.print("Das Ziel von Motor C wurde auf ");
      }
      Serial.print(target_c);
      Serial.println("° gesetzt");
    } else if (serial_line.substring(0, 1) == "d") {
      target_d = serial_line.substring(1).toInt();
      if (auto_execute) {
        Serial.print("Die Postion von Motor D wurde auf ");
        mom_d = target_d;
      } else {
        Serial.print("Das Ziel von Motor D wurde auf ");
      }
      Serial.print(target_d);
      Serial.println("° gesetzt");
    } else if (serial_line == "status") {
      Serial.println("  -=== Aktueller Status ===-");
      if (auto_execute) {
        Serial.print("Motor A");
      } else {
        Serial.print("Das Ziel von Motor A ist ");
        Serial.print(target_a);
        Serial.print("° er");
      }
      Serial.print(" ist gerade auf ");
      Serial.print(mom_a);
      Serial.println("°");
      if (auto_execute) {
        Serial.print("Motor B");
      } else {
        Serial.print("Das Ziel von Motor B ist ");
        Serial.print(target_b);
        Serial.print("° er");
      }
      Serial.print(" ist gerade auf ");
      Serial.print(mom_b);
      Serial.println("°");
      if (auto_execute) {
        Serial.print("Motor C");
      } else {
        Serial.print("Das Ziel von Motor C ist ");
        Serial.print(target_c);
        Serial.print("° er");
      }
      Serial.print(" ist gerade auf ");
      Serial.print(mom_c);
      Serial.println("°");
      if (auto_execute) {
        Serial.print("Motor D");
      } else {
        Serial.print("Das Ziel von Motor D ist ");
        Serial.print(target_d);
        Serial.print("° er");
      }
      Serial.print(" ist gerade auf ");
      Serial.print(mom_d);
      Serial.println("°");
    }
    if (serial_line == "execute" && !auto_execute) {
      Serial.println("  -=== Änderungen ===-");
      if (mom_a != target_a) {
        Serial.print("Motor A wurde von ");
        Serial.print(mom_a);
        Serial.print("° auf ");
        Serial.print(target_a);
        Serial.println("° gesetzt.");
      }
      if (mom_b != target_b) {
        Serial.print("Motor B wurde von ");
        Serial.print(mom_b);
        Serial.print("° auf ");
        Serial.print(target_b);
        Serial.println("° gesetzt.");
      }
      if (mom_c != target_c) {
        Serial.print("Motor C wurde von ");
        Serial.print(mom_c);
        Serial.print("° auf ");
        Serial.print(target_c);
        Serial.println("° gesetzt.");
      }
      if (mom_d != target_d) {
        Serial.print("Motor D wurde von ");
        Serial.print(mom_d);
        Serial.print("° auf ");
        Serial.print(target_d);
        Serial.println("° gesetzt.");
      }
      mom_a = target_a;
      mom_b = target_b;
      mom_c = target_c;
      mom_d = target_d;
    }
    move(move_time);
  }
  ledcWrite(1, map(ramp_a.update(), 0, 180, 1638, 8192));
  ledcWrite(2, map(ramp_b.update(), 0, 180, 1638, 8192));
  ledcWrite(3, map(ramp_c.update(), 0, 180, 1638, 8192));
  ledcWrite(4, map(ramp_d.update(), 0, 180, 1638, 8192));
}