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

StaticJsonDocument<200> writeDoc;
JsonArray pozice;

DeserializationError error;

String matlab;
float cube;

// nastavení propojovacích pinů modulu
#define pinX A0 //joystick
#define pinY A1
#define pinKey 10

// proměnné pro uložení hodnoty

int nulaX, nulaY; 
float aktXX =0;
float oldX =0;
float aktYY =0;
float oldY =0;
float aktZZ =0;
int open_close_;

const int buttonPin = 2;  
int buttonState;  


void setup() {
  
  // inicializace tlačítka joysticku
  pinMode(pinKey, INPUT_PULLUP);
  // komunikace přes sériovou linku rychlostí 9600 baud
  Serial.begin(9600);
   while (!Serial) continue;
  // načtení a uložení hodnot pro x a y osy
  nulaX = analogRead(pinX);
  nulaY = analogRead(pinY);
  
   // inicializujte pin tlačítka jako vstup:
    pinMode(buttonPin, INPUT);

}

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;
    }
//joystick
 // vytvoření proměnných pro uložení
  // hodnot pro osy x, y a stav tlačítka
  float aktX, aktY, stavTlac;
  // načtení analogových hodnot osy x a y
  aktX = analogRead(pinX) - nulaX;
  aktY = analogRead(pinY) - nulaY;
  // načtení stavu tlačítka
  
  stavTlac = digitalRead(pinKey);
  
  if (aktX > 0) {
    aktX = map(aktX, 0, 1023-nulaX, 0, 1);
  }
  else {
    aktX = map(aktX, 0, -nulaX, 0, -1);
  }
  if (aktY > 0) {
    aktY = map(aktY, 0, 1023-nulaY, 0, 1);
  }
  else {
    aktY = map(aktY, 0, -nulaY, 0, -1);
  }
  // vytištění informací o souřadnicích

aktXX = (aktX/50) + oldX;
oldX=aktXX;
aktYY = (aktY/50) + oldY;
oldY=aktYY;

aktZZ=0.75;
  if(stavTlac == LOW) {
    aktZZ=0.63;
  }

 buttonState = digitalRead(buttonPin);      
        if (buttonState == HIGH) {            
            // Po stisknutí zavři gripper
            open_close_= 0;
        }
        else {            
            open_close_ = 1;
        }
  delay(10);
    
    long xxx0 = readDoc["pozice"][0];
    long yyy0 = readDoc["pozice"][1];
    long zzz0 = readDoc["pozice"][2];
    
    long time_ = readDoc["time"];
 
    //Zápis poloh do seriove komunikace
  if (time_ < 0.5){
    writeDoc["pozice"][0] = aktXX;
    writeDoc["pozice"][1] = aktYY;
    writeDoc["pozice"][2] = aktZZ;
    writeDoc["speed"] = 1;
    writeDoc["open_close"] = open_close_;  
  }    
    writeDoc["time"] = time_;
   
  serializeJson(writeDoc, Serial);  
  Serial.println();
  delay(500);
}
