#define X_MIN_PIN           3
#ifndef X_MAX_PIN
#define X_MAX_PIN         2
#endif
#define Y_MIN_PIN          14
#define Y_MAX_PIN          15
#define Z_MIN_PIN          18
#define Z_MAX_PIN          19

#define X_STEP_PIN         54
#define X_DIR_PIN          55
#define X_ENABLE_PIN       38
#define X_CS_PIN           53

#define Y_STEP_PIN         60
#define Y_DIR_PIN          61
#define Y_ENABLE_PIN       56
#define Y_CS_PIN           49

#define Z_STEP_PIN         46
#define Z_DIR_PIN          48
#define Z_ENABLE_PIN       62
#define Z_CS_PIN           40

int x;
int y;
int z;
int krokix=0;
int krokiy=0;
int krokiz=0;
int tmp;
bool wyslane = true;
int xmin;

String s;


void setup() {

  Serial.begin(57600);

pinMode(X_MAX_PIN, INPUT);
digitalWrite(X_MAX_PIN,HIGH);
pinMode(X_MIN_PIN, INPUT);

pinMode(Y_MAX_PIN, INPUT);
digitalWrite(Y_MAX_PIN,HIGH);

pinMode(Z_MAX_PIN, INPUT);
digitalWrite(Z_MAX_PIN,HIGH);

//silnik X
pinMode(X_ENABLE_PIN,OUTPUT);
digitalWrite(X_ENABLE_PIN, LOW);
pinMode(X_DIR_PIN,OUTPUT);
digitalWrite(X_DIR_PIN,LOW);
pinMode(X_STEP_PIN,OUTPUT);

//silnik Y
pinMode(Y_ENABLE_PIN,OUTPUT);
digitalWrite(Y_ENABLE_PIN, LOW);
pinMode(Y_DIR_PIN,OUTPUT);
digitalWrite(Y_DIR_PIN,LOW);
pinMode(Y_STEP_PIN,OUTPUT);

//silnik Z
pinMode(Z_ENABLE_PIN,OUTPUT);
digitalWrite(Z_ENABLE_PIN, LOW);
pinMode(Z_DIR_PIN,OUTPUT);
digitalWrite(Z_DIR_PIN,LOW);
pinMode(Z_STEP_PIN,OUTPUT);



}

void loop() {

  //kontrola nad liczba krokow
while (Serial.available()>0)
{
  s = Serial.readStringUntil('\n');
  sscanf(s.c_str(),"x %d y %d z %d \n", &krokix, &krokiy, &krokiz);
  wyslane= false;

if(krokix < 0)
  {
    digitalWrite(X_DIR_PIN,HIGH);
    krokix = -krokix;
  }
  else
  {
    digitalWrite(X_DIR_PIN,LOW);
  }
  if(krokiy < 0)
  {
    digitalWrite(Y_DIR_PIN,HIGH);
    krokiy = -krokiy;
  }
  else
  {
    digitalWrite(Y_DIR_PIN,LOW);
  }
  if(krokiz < 0)
  {
    digitalWrite(Z_DIR_PIN,HIGH);
    krokiz = -krokiz;
  }
  else
  {
    digitalWrite(Z_DIR_PIN,LOW);
  }
//  tmp=Serial.parseInt();
//  if (tmp>0)
//  {
//    krokix=tmp;
//    krokiy=tmp;
//    krokiz=tmp;
//  }
}
  x = digitalRead(X_MAX_PIN);
  //Serial.println(x);

   y = digitalRead(Y_MAX_PIN);
  //Serial.println(y);

   z = digitalRead(Z_MAX_PIN);
  //Serial.println(z);
if (x==0 && krokix>0)
{
  digitalWrite(X_STEP_PIN, HIGH);
  krokix--;
}
  if(y==0 && krokiy>0)
{
  digitalWrite(Y_STEP_PIN, HIGH);
  krokiy--;
}

if(z==0 && krokiz>0)
{
digitalWrite(Z_STEP_PIN, HIGH);
krokiz--;
}




delayMicroseconds(500);
digitalWrite(X_STEP_PIN, LOW);
digitalWrite(Y_STEP_PIN, LOW);
digitalWrite(Z_STEP_PIN, LOW);
delayMicroseconds(500);

if(krokix ==0 && krokiy == 0 && krokiz == 0 && wyslane == false)
 {
  Serial.println("ok");
  wyslane = true;
  }
xmin=digitalRead(X_MIN_PIN);
if(xmin == 0)
{
  digitalWrite(X_ENABLE_PIN,HIGH);
  digitalWrite(Y_ENABLE_PIN,HIGH);
  digitalWrite(Z_ENABLE_PIN,HIGH);

  }
  else
  {
    digitalWrite(X_ENABLE_PIN,LOW);
    digitalWrite(Y_ENABLE_PIN,LOW);
    }
}




///////////////////////////////MATLAB



t=0:0.1:2*pi;

traj1=round(500*sin(t));
traj2=round(500*sin(t+(2*pi/3)));
traj3=round(500*sin(t+(4*pi/3)));


kroki1=traj1(2:end)-traj1(1:end-1);
kroki2=traj2(2:end)-traj2(1:end-1);
kroki3=traj3(2:end)-traj3(1:end-1);

for i=1:length(kroki1)
    fprintf(s1,'x %d y %d z %d\n',[kroki1(i),kroki2(i),kroki3(i)]);
    [kroki1(i),kroki2(i),kroki3(i)]
    while(s1.BytesAvailable == 0)
    end
    while(s1.BytesAvailable > 0)
    fscanf(s1)
    end
end








///////////////////////////////////////////////OBSŁUGA GRZAŁKI I WENTYLATORA. REGULACJA TEMPERATURY.

#include <LiquidCrystal.h>

#define TEMP0 13
#define a -7.81086316639610e-07
#define b 0.00135944941813236
#define c -0.865762638670255
#define d 292.399926740249
#define HEATER 10
#define FAN 44
//#define FAN2 9

LiquidCrystal lcd(16, 17, 23, 25, 27, 29);
int x;
double temp, e, u, i = 0,tempz;



void setup() {
  pinMode(HEATER, OUTPUT);
  digitalWrite(HEATER, LOW);
  pinMode(FAN, OUTPUT);
  digitalWrite(FAN, HIGH);
  //pinMode(FAN2, OUTPUT);
  Serial.begin(57600);
  lcd.begin(20, 4);
  lcd.print("");
}

void loop() {
  x = analogRead(TEMP0);
  Serial.println(x);
  temp = a * (double)pow(x, 3) + b * (double)pow(x, 2) + c * (double)x + d;
  lcd.setCursor(0, 0);
  lcd.print(temp);
  tempz=150;
  e = tempz - temp; //blad
  i = i + (e * 0.01); //calkowanie (pole pow. = blad * opoznienie(10 ms))
  u = 255 * (e / (tempz-30)) + (i /10); //sterowanie
  if (temp < tempz)
  {
    analogWrite(HEATER, u > 255 ? 255 : floor(u)); //if(u>255){255}else{floor(u)}
    lcd.setCursor(0, 1);
    //lcd.print("Grzalka wlaczona");
    lcd.print(e);
    lcd.setCursor(0, 2);
    lcd.print(u);
    lcd.setCursor(0, 3);
    lcd.print(i);
  }
  else
  {
    digitalWrite(HEATER, LOW);
    //lcd.setCursor(0, 2);
    //lcd.print("Grzalka wylaczona");
    lcd.setCursor(0, 1);
    lcd.print(e);
    lcd.setCursor(0, 2);
    lcd.print(u);
    lcd.setCursor(0, 3);
    lcd.print(i);
  }
  delay(10);
}