#include <ArduinoJson.h>
#include <Ramp.h>
//#include <LiquidCrystal_I2C.h>
//#include <Ps3Controller.h>







// NOT WORKING BECAUSE MEMORY ERROR













// Settings
char defaultConfig[] = "{'defaultPosition': 'Home', 'servos': {'A': {'pin': 13, 'min': 1638, 'max': 8192}, 'B': {'pin': 12, 'min': 1638, 'max': 8192}, 'C': {'pin': 14, 'min': 1638, 'max': 8192}, 'D': {'pin': 27, 'min': 1638, 'max': 8192}}, 'positions': {'Home':{'A':90,'B':65,'C':155,'D':60}}}";


// Create Display, Position List and Servo List
//LiquidCrystal_I2C lcd(0x27, 16, 2);
DynamicJsonDocument config(1024);
ramp ramps[16] = {};


void controllerOnEvent() {

}

void controllerOnConnect(){
    Serial.println("Connected.");
}

void createServoChannels() {
  int i = 0;
  JsonObject object = config["servos"].as<JsonObject>();
  for (JsonPair kv : object) {
    i++;
    ledcSetup(i, 50, 16);
    ledcAttachPin(kv.value().as<JsonObject>()["pin"], i);
  }
}

bool moveToPosition(String position, int time) {
  int i = 0;
  JsonObject object = config["servos"].as<JsonObject>();
  for (JsonPair kv : object) {
    if (config["positions"][position].containsKey(kv.key())) {
      ramps[i].go(config["positions"][position][kv.key()], time);
    }
    i++;
  }
  return true;
}

void raise(String error) {
  Serial.print("Error: ");
  Serial.println(error);
}

int countCharInString(String str, char character) {
  int count = 0;
  for (uint8_t i = 0; i < str.length(); ++i)
    if (str[i] == character) count++;
    return count;
}

void split(String str, String delimiter, String* outputArray) {
  char chars[str.length() + 1];
  str.toCharArray(chars, str.length() + 1);
  //str = strtok(chars, delimiter.c_str());
  String outStr = "";
  int i2 = 0;
  for (int i = 0; i < str.length(); i++) {
    if (String(str.substring(i, i + delimiter.length())) == delimiter) {
      outputArray[i2] = outStr;
      outStr = "";
      i2 ++;
    }
    else outStr += str[i];
  }
  outputArray[i2] = outStr;
}

void processCommands(String commands) {
  String commandsArray[countCharInString(commands, '\n')] = {};
  split(commands, "\n", commandsArray);

  for (int i = 0; i < sizeof(commandsArray) / sizeof(commandsArray[0]); ++i) {
    processCommand(commandsArray[i]);
  }
}

void processCommand(String command) {
  String params[countCharInString(command, ' ') + 1] = {};
  split(command, " ", params);
  command = params[0];
  command.toLowerCase();

  if (command == "set") {
    if (config["positions"].containsKey(params[1])) {
      Serial.println("Pos");
    } else {
      for (int i = 0; i < params[1].length(); i++) {
        String upper = String(params[1][i]);
        upper.toUpperCase();
        JsonObject object = config["servos"].as<JsonObject>();
        int i2 = 0;
        for (JsonPair kv : object) {
          String upperKey = kv.key().c_str();
          upperKey.toUpperCase();
          if (upper == upperKey) {
            ramps[i2].go(params[2].toInt(), 1000);
            Serial.print("Motor ");
            Serial.print(kv.key().c_str());
            Serial.print(" bewegt sich nun zu: ");
            Serial.print(params[2]);
            Serial.println("°");
          }
          i2++;
        }
      }
    }
  }
  else raise("Command '" + params[0] + "' not found!");
}

void setup() {
  delay(1000);
  Serial.begin(115200);
  
  // Create Config
  deserializeJson(config, defaultConfig);

  // Initialize and light up LCD
  //lcd.init();
  //lcd.backlight();

  // Link Functions to Controller
  //Ps3.attach(controllerOnEvent);
  //Ps3.attachOnConnect(controllerOnConnect);

  // Create Servo Channels
  createServoChannels();

  // Move to Default Position
  moveToPosition(config["defaultPosition"], 1000);

  Serial.println("[RoboCommander] Initialisierung abgeschlossen.");
}

int n = 0;
void loop() {
  if (Serial.available() > 0) {
    processCommands(Serial.readString());
  }
  n++;
  int i = 0;
  JsonObject object = config["servos"].as<JsonObject>();
  for (JsonPair kv : object) {
    if (n % 1000 == 0) {
      Serial.print(kv.key().c_str());
      Serial.println(map(ramps[i].update(), 0, 180, kv.value().as<JsonObject>()["min"], kv.value().as<JsonObject>()["max"]));
    }
    ledcWrite(i + 1, map(ramps[i].update(), 0, 180, kv.value().as<JsonObject>()["min"], kv.value().as<JsonObject>()["max"]));
    i++;
  }
}