#include <ArduinoJson.h> //Nacteni knihovny JSON

StaticJsonDocument<200> writeDoc;
JsonArray pozice;

DeserializationError error;

String matlab;
float cube;

void setup() {
  
  // Initialize Serial port
  Serial.begin(9600);
  while (!Serial) continue;

  pozice = writeDoc.createNestedArray("pozice");
  pozice.add(0);
  pozice.add(0);
  pozice.add(0);
}

void loop() {
  matlab = "";  
  while (Serial.available()) {
    delay(1);  //pauza než se naplní buffer
    if (Serial.available() > 0) {
      char c = Serial.read();  
      matlab += c; //vytvoření stringu matlab      
      if(c == "\n"){
        break;
      }
     }
    } 
 //Nacteni struktury matlab ze seriove komunikace format JSON   
    if(matlab == "\n"){
      return;
    }        
    StaticJsonDocument<200> readDoc;
    error = deserializeJson(readDoc, matlab);
    if (error) {
      Serial.print(F("deserializeJson() failed: "));
      Serial.println(error.f_str());
      return;
    }    
    //Nacteni promenych z JSON
    long xxx0 = readDoc["pozice"][0];
    long yyy0 = readDoc["pozice"][1];
    long zzz0 = readDoc["pozice"][2];
  
    long vrepspeed = readDoc["speed"];
    long vrepopen_close = readDoc["open_close"];
    long time_ = readDoc["time"];
    
   
    long PSensordist = readDoc["PSensor"];
    long detectPoint = readDoc["cube_size"]; 
    long rvrepkuka_target = readDoc["vrepkuka_target"];
    long rvrepProximity_sensor = readDoc["vrepProximity_sensor"];
    
    //Zpracovani dat
  if (time_ < 0.5){
    writeDoc["pozice"][0] = -0.11;
    writeDoc["pozice"][1] = -0.3;
    writeDoc["pozice"][2] = 0.75;
    writeDoc["speed"] = 5;
    writeDoc["open_close"] = 0;  
  }  
  
  if (time_ > 0.5) {
     if (PSensordist > 0) {
         if (detectPoint < 167) {
          cube = 1;
        }
        else  {
          cube = 2;        
    }
    writeDoc["pozice"][0] = -0.11;
    writeDoc["pozice"][1] = -0.3;
    writeDoc["pozice"][2] = 0.65;
    writeDoc["speed"] = 1;
    writeDoc["open_close"] = 1;
      }      
    }

   if (time_ > 1.5) {
    writeDoc["pozice"][0] = -0.11;
    writeDoc["pozice"][1] = -0.3;
    writeDoc["pozice"][2] = 0.75;
    writeDoc["speed"] = 1;
    writeDoc["open_close"] = 1;   
   } 
   
// velká kostka

 if (cube <1.5){    
  if (time_ > 2.5) {
    writeDoc["pozice"][0] = 0.1;
    writeDoc["pozice"][1] = 0.5;
    writeDoc["pozice"][2] = 0.75;
    writeDoc["speed"] = 5;
    writeDoc["open_close"] = 1;
   } 
 if (time_ > 3.5) {
    writeDoc["pozice"][0] = 0.1;
    writeDoc["pozice"][1] = 0.5;
    writeDoc["pozice"][2] = 0.68;
    writeDoc["speed"] = 0.1;
    writeDoc["open_close"] = 0;
   } 
 if (time_ > 4.5) {
    writeDoc["pozice"][0] = 0.1;
    writeDoc["pozice"][1] = 0.5;
    writeDoc["pozice"][2] = 0.75;
    writeDoc["speed"] = 1;
    writeDoc["open_close"] = 0;
   } 
  
// malá kostka

 }  else if (cube > 1.5) {  
  if (time_ > 2.5) {
    writeDoc["pozice"][0] = 0.1;
    writeDoc["pozice"][1] = 0.2;
    writeDoc["pozice"][2] = 0.75;
    writeDoc["speed"] = 5;
    writeDoc["open_close"] = 1;
   } 
 if (time_ > 3.5) {
    writeDoc["pozice"][0] = 0.1;
    writeDoc["pozice"][1] = 0.2;
    writeDoc["pozice"][2] = 0.65;
    writeDoc["speed"] = 0.1;
    writeDoc["open_close"] = 0;
   } 
 if (time_ > 4.5) {
    writeDoc["pozice"][0] = 0.1;
    writeDoc["pozice"][1] = 0.2;
    writeDoc["pozice"][2] = 0.75;
    writeDoc["speed"] = 1;
    writeDoc["open_close"] = 0;
   } 
 }
 
    //Odeslání dat do matlabu          
    writeDoc["time"] = time_;   
    writeDoc["PSensor"] = PSensordist;
    writeDoc["cube_size"] = detectPoint;   
    writeDoc["hodnotakostky"] = cube;
    writeDoc["vrepkuka_target"] = rvrepkuka_target;
    writeDoc["vrepProximity_sensor"] = rvrepProximity_sensor;
  
  serializeJson(writeDoc, Serial);  
  Serial.println();
  delay(200);
}
