#include "Arduino.h"

//motor spot
const int spinENA = 11;
const int spinPUL = 9;
const int spinDIR = 10;

//motor Y
const int ypinENA = 8;
const int ypinPUL = 12;
const int ypinDIR = 13;

//motor cam
const int fpinENA = 3;
const int fpinPUL = 4;
const int fpinDIR  = 5;

//positions (сверять с детекторами)
long xP = 0;
long yP = 0;
long bP = 0;

//Vrefx = 0.7 * 1.68 * 0.100 * 8 = 0.9408
//Vrefy = 0.7 * 2.8 * 0.100 * 8 =  1.568
//steps
const float st = 1.8;
int stepsf = 30;
int stepsy = 2;
int stepss = 1;


//line mode
int wig_del = 4500;

int wig_st = 2;


//delays
const int dED = 6000; // 8000
const int dDP = 6000; // 8000 для сканирования
int slP = 6000; // 
int sdP = 12000; //

//ENA: HIGH - open, LOW - closed



long stepY(int del1, int del2, int del3, int del4, int d) {
  float start = millis();
  digitalWrite(ypinENA, LOW);
  digitalWrite(fpinENA, LOW);
  delayMicroseconds(del1);
  digitalWrite(ypinDIR, d);
  digitalWrite(fpinDIR, d);
  delayMicroseconds(del2);

  digitalWrite(ypinPUL, HIGH);
  digitalWrite(fpinPUL, HIGH);
  delayMicroseconds(del3);
  digitalWrite(ypinPUL, LOW);
  digitalWrite(fpinPUL, LOW);
  delayMicroseconds(del4);
  
  switch (d) {
    case 1:
      yP += 1;
      break;
    case 0:
      yP -= 1;
      break;
  }
  digitalWrite(ypinENA, HIGH);
  float end = millis();
  float diff = end - start;
  return (diff);
}

long stepS(int del1, int del2, int del3, int del4, int d) {
  float start = millis();

  digitalWrite(spinENA, LOW);
  delayMicroseconds(del1);
  digitalWrite(spinDIR, d);
  delayMicroseconds(del2);

  digitalWrite(spinPUL, HIGH);
  delayMicroseconds(del3);
  digitalWrite(spinPUL, LOW);
  delayMicroseconds(del4);
  

  digitalWrite(spinENA, HIGH);
  float end = millis();
  float diff = end - start;
  return (diff);
}

long stepF(int del1, int del2, int del3, int del4, int d) {
  float start = millis();
  //total_step_time = del1 / 1000000 + del2 / 1000000 + del3 / 1000000 + del4 / 1000000
  digitalWrite(fpinENA, LOW);
  delayMicroseconds(del1);
  digitalWrite(fpinDIR, d);
  delayMicroseconds(del2);

  digitalWrite(fpinPUL, HIGH);
  delayMicroseconds(del3);
  digitalWrite(fpinPUL, LOW);
  delayMicroseconds(del4);
  
 
  digitalWrite(fpinENA, HIGH);
  float end = millis();
  float diff = end - start;
  return (diff);
}




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

  pinMode(spinENA, OUTPUT);
  pinMode(spinDIR, OUTPUT);
  pinMode(spinPUL, OUTPUT);
  
  pinMode(ypinENA, OUTPUT);
  pinMode(ypinDIR, OUTPUT);
  pinMode(ypinPUL, OUTPUT);
  
  pinMode(fpinENA, OUTPUT);
  pinMode(fpinDIR, OUTPUT);
  pinMode(fpinPUL, OUTPUT);

  
  digitalWrite(spinENA, LOW);
  digitalWrite(fpinENA, LOW);
  digitalWrite(ypinENA, LOW);

  digitalWrite(11, HIGH);
  Serial.println('AMOGUS');
}

void loop() {


  int req = Serial.read();
  
  switch(req) {
    case '1':
      wig_del = 4500;
      Serial.println("> Line mode delay set to: 4500 microsec.");
      break;
    case '2':
      wig_del = 3000;
      Serial.println("> Line mode delay set to: 3000 microsec.");
      break;
    case '3': 
      if ( wig_st > 2) {Serial.println("> Prohibited delay for this steps mode!");}
      else {
        wig_del = 2500;
        Serial.println("> Line mode delay set to: 2500 microsec.");
      }
      break; 
    case 'w':
      for (int yu = 0; yu < stepsy; yu++) {
        stepY(dED, dDP, slP, sdP, LOW);
      }
      break;
    case 's':
      for (int yd = 0; yd < stepsy; yd++) {
        stepY(dED, dDP, slP, sdP, HIGH);
      }
      break;
    case '-':
      for (int sc = 0; sc < stepss; sc++)
      {
        stepS(dED, dDP, wig_del*2, wig_del*2, HIGH);
      }
      break;
    case '+':
      for (int fc = 0; fc < stepss; fc++)
      {
        stepS(dED, dDP, slP, sdP, LOW);
      }
      break;
    case 'f':
      int req2 = Serial.read();
      int sw2 = HIGH;
      float k = 0.4;
      while (1) {
        req2 = Serial.read();
        if (req2 == 'F') {
          break;
        }
        for (int a = 0; a < wig_st; a++) {
          stepY(dED, dDP, wig_del, wig_del, sw2); 
        }
        if (sw2 == HIGH) {sw2 = LOW;}
        else {sw2 = HIGH;}
      }     
  }
}