#define IDENTITY "Generic Dobby Interface (HobbyDobby)\n"
#define MAX_CHARS 80

// Serial (USB) interface
int serial_chars=0;               // Number of characters in the serial input buffer
char serial_string[MAX_CHARS]=""; // Serial input buffer
long serial_time=0;               // Used to close serial string on timeout (timer)
const int serial_timeout=100;     // Digest Serial string after timeout
char serial_idle=1;               // Tracks activity on the serial bus
char HostPresent = 0;             // set when some host is identified
const int BUTTON_UP=0; 
const int BUTTON_DOWN=1;
unsigned long shaft_state=0x00000000, shaft_cue=0x00000000, oldshafts=0x00000000; // tracking current & next desired states of shaft controls
const int ADVANCE =00; // analog input channel for Advance button push
// Кнопки
#define BTN_PIN01 2        // sw1 NEXT ON/OFF 
#define BTN_PIN02 3        // sw2 CURRENT ON/OFF
#define BTN_PINa1 A1        //программы стандартные схемы
#define BTN_PINa2 A2        // 
#define BTN_PINa3 A3        // 
#define BTN_PINa4 A4        // 


#include "GyverButton.h"
GButton butt_sw1(BTN_PIN01);
GButton butt_sw2(BTN_PIN02);
GButton butt_swA1(BTN_PINa1);
GButton butt_swA2(BTN_PINa2);
GButton butt_swA3(BTN_PINa3);
GButton butt_swA4(BTN_PINa4);

#include <GyverStepper2.h>
GStepper2<STEPPER2WIRE> stepper1(400, 24, 22);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper2(400, 28, 26);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper3(400, 25, 23);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper4(400, 29, 27);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper5(400, 30, 32);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper6(400, 36, 34);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper7(400, 33, 31);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper8(400, 37, 35);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper9(400, 40, 38);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper10(400, 44, 42);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper11(400, 41, 39);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper12(400, 45, 43);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper13(400, 48, 46);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper14(400, 52, 50);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper15(400, 49, 47);                   // драйвер step-dir
GStepper2<STEPPER2WIRE> stepper16(400, 53, 51);                   // драйвер step-dir

int MaxSpeed=7000;     // 10000 work - 400 debug
int Acceleration=3000;
int MaxPos=1700;       // 5000 work - 50 debug
int MaxPosDebug=50;
int BufferRead=0;  // Прочитана команда
int iPolCurentPosition=0;    // счетчик полотняного переплетения


//////////////////////////////////////////////////////////////////////////
// Send shaft controls to hardware
/////////////////////////////////////////////////////////////////////////
void set_shafts(unsigned long shafts)
{ // set controls to state defined by input word

 shaft_state=shafts; // backup to global
  if (shafts==0)
  {
    stepper1.setTarget(0);
    stepper2.setTarget(0);
    stepper3.setTarget(0);
    stepper4.setTarget(0);
    stepper5.setTarget(0);
    stepper6.setTarget(0);
    stepper7.setTarget(0);
    stepper8.setTarget(0);
    stepper9.setTarget(0);
    stepper10.setTarget(0);
    stepper11.setTarget(0);
    stepper12.setTarget(0);
    stepper13.setTarget(0);
    stepper14.setTarget(0);
    stepper15.setTarget(0);
    stepper16.setTarget(0);
  }
  else
  {
    if(shafts&0x8000) stepper16.setTarget(MaxPos);//Control bit is high
    else stepper16.setTarget(0); //Control bit is low
    if(shafts&0x4000) stepper15.setTarget(MaxPos);//Control bit is high
    else stepper15.setTarget(0); //Control bit is low
    if(shafts&0x2000) stepper14.setTarget(MaxPos);//Control bit is high
    else stepper14.setTarget(0); //Control bit is low
    if(shafts&0x1000) stepper13.setTarget(MaxPos);//Control bit is high
    else stepper13.setTarget(0); //Control bit is low 
    if(shafts&0x800) stepper12.setTarget(MaxPos);//Control bit is high
    else stepper12.setTarget(0); //Control bit is low
    if(shafts&0x400) stepper11.setTarget(MaxPos);//Control bit is high
    else stepper11.setTarget(0); //Control bit is low
    if(shafts&0x200) stepper10.setTarget(MaxPos);//Control bit is high
    else stepper10.setTarget(0); //Control bit is low
    if(shafts&0x100) stepper9.setTarget(MaxPos);//Control bit is high
    else stepper9.setTarget(0); //Control bit is low 
    if(shafts&0x080) stepper8.setTarget(MaxPos);//Control bit is high
    else stepper8.setTarget(0); //Control bit is low
    if(shafts&0x040) stepper7.setTarget(MaxPos);//Control bit is high
    else stepper7.setTarget(0); //Control bit is low
    if(shafts&0x020) stepper6.setTarget(MaxPos);//Control bit is high
    else stepper6.setTarget(0); //Control bit is low
    if(shafts&0x010) stepper5.setTarget(MaxPos);//Control bit is high
    else stepper5.setTarget(0); //Control bit is low 
    if(shafts&0x008) stepper4.setTarget(MaxPos);//Control bit is high
    else stepper4.setTarget(0); //Control bit is low
    if(shafts&0x004) stepper3.setTarget(MaxPos);//Control bit is high
    else stepper3.setTarget(0); //Control bit is low
    if(shafts&0x002) stepper2.setTarget(MaxPos);//Control bit is high
    else stepper2.setTarget(0); //Control bit is low
    if(shafts&0x001) stepper1.setTarget(MaxPos);//Control bit is high
    else stepper1.setTarget(0); //Control bit is low 
     
  }
}

//////////////////////////////////////////////////////////////////////////
// Preset- cycle all shaft controls sequentially Начальная калибровка
/////////////////////////////////////////////////////////////////////////
void preset()
{
 //int Td=200; // delay between setting shafts - 2.4 sec first to last
 
 //set_shafts(0x0fff); // ramp all shafts up
 //set_shafts(0x0000); // then back down
}

//////////////////////////////////////////////////
// SERIAL INTERFACE (USB) FUNCTIONS //
//////////////////////////////////////////////////
// USB input commands:
// 1,2,3,n Set shafts 1,2,3,n
// ? ID request, respond with ID
unsigned long get_shafts(char* command)
{
  int chars=strlen(command);
  int i=0;
  unsigned long shafts=0, bits;
  char shaft=0, digit=0, character;
  char message[1000];
  while(i<=chars)
  {
    character=command[i];
    if((character==',')||(i==chars))
    { // have field seperator or end of string, not shaft desired
      if((shaft>0)&&(shaft<=32))
      { // 'shafts' notes this shaft is required
        bits=1<<shaft-1;
        shafts|=bits;
        shaft=0;
      }
    }
    else if(('0'<=character)&&(character<='9'))
    { // have digit
      character&=0x0f; // reduce to decimal
      shaft=shaft*10+character; 
    }
    else if((character==' ')||(character=='\n')); // ignore spaces/returns
    else if((character=='a')||(character=='A')) {
      MaxPos=MaxPos+50;
      Serial.println(MaxPos);
    }
    else
    {
      Serial.println("Invalid Character detected in command:"); // debug display of command extracted
      //Serial.println(command); // debug display of command extracted
      i=chars; // quit, got crap
    } 
    i++;
  }
  return shafts;
}


void digest_serial(char* command)
{
  int chars=strlen(command);
  if(chars>0)
  {
    HostPresent=1; // note that there's something out there talking to me
    if(('0'<=command[0])&&(command[0]<='9')) // go number, should be shafts 
      shaft_cue=get_shafts(command); // convert ASCII string to hex string
    else if((command[0]=='c')||(command[0]=='t')) // test command to cycle valves
      preset(); // cycle through the valves
    else if((command[0]=='a')||(command[0]=='A')) { // Увеличить ускорение
      Acceleration=Acceleration+100;
      Serial.println(Acceleration);
    }else if((command[0]=='s')||(command[0]=='S')) { // Увеличить скорость
      MaxSpeed=MaxSpeed+100;
      Serial.println(MaxSpeed);
    }else if((command[0]=='d')||(command[0]=='D')) { // Увеличить дистанцию
      MaxPos=MaxPos+MaxPosDebug;
      Serial.println(MaxPos);
    }
    else if(command[0]=='?')
    {
      char message[256]=IDENTITY;
      message[strlen(message)-1]=0; // delete return in line
      sprintf(message,"%s [Solenoid state = 0x%3x]\n",message,shaft_state); // note what we'll do when button is pressed
      Serial.write(message);
    }
  } 
}


void serial_input()
{
  char inChar;
  char command[MAX_CHARS]="";
  while((Serial.available()>0))
  {
    inChar=char(Serial.read());
    serial_string[serial_chars]=inChar;
    serial_chars++;
    serial_string[serial_chars]=0;
    serial_time=millis();
  }
  if((serial_chars>0)&&(serial_idle)&&((serial_string[serial_chars-1]=='\n')||(millis()>(serial_time+serial_timeout))))
  {
    serial_idle=0;
    strcpy(command,serial_string);
    serial_chars=0;
    serial_string[0]=0;
    //Serial.println(command); // debug display of command extracted
    digest_serial(command);
    // done with last command, clear the input buffers
    serial_idle=1; // ready for more serial input
    
  }
}

///////////////////////////////////////////////////////////////
// Floor button handler - set shafts/tell host to advance //
///////////////////////////////////////////////////////////////
void monitor_button()
{ // check for button push to advance, sent only when button pushed (not held)
  // int level=analogRead(ADVANCE);
  char state=0;
  butt_sw1.tick();  
  butt_sw2.tick();

  // check current state
  if(butt_sw1.state()) state=BUTTON_DOWN;
  else state=BUTTON_UP;
    if(HostPresent==0)
    {
      if(butt_sw1.isPress())
      {
        Serial.write("Button down, no host\n"); 
        preset(); // cycle through the valves
      }
    }
    else
    {
      if(butt_sw1.isPress())
      { // button pressed, send command
        set_shafts(shaft_cue);
      }
      if (butt_sw1.isRelease())
      {
        oldshafts=shaft_cue;
        shaft_cue=0;
        Serial.write("ADVANCE\n"); 
        set_shafts(0);
      }
      if (butt_sw2.isPress()) set_shafts(oldshafts); 
      if (butt_sw2.isRelease()) set_shafts(0);
    }
}

///////////////////////////////////////////////////////////////
// простое полотно 1,3 - 2,4 
///////////////////////////////////////////////////////////////
void prog_polotno()
{
  if(butt_sw1.isPress())
  { // button pressed, send command
    //  Serial.println(iPolCurentPosition); // debug display of command extracted
    if (iPolCurentPosition==0) set_shafts(0x1111+0x4444);
    else set_shafts(0x2222+0x8888);
  }
  if (butt_sw1.isRelease())
  {
    if (iPolCurentPosition==0) iPolCurentPosition=1;
    else iPolCurentPosition=0;
    set_shafts(0);
  }
  if (butt_sw2.isPress())
  { if (iPolCurentPosition>0) set_shafts(0x1111+0x4444);
    else set_shafts(0x2222+0x8888);
  }
  if (butt_sw2.isRelease()) set_shafts(0);
}

///////////////////////////////////////////////////////////////
// pickUp 2,3 - 1,4 
///////////////////////////////////////////////////////////////
void prog_pickup()
{
  if(butt_sw1.isPress())
  { // button pressed, send command
    //  Serial.println(iPolCurentPosition); // debug display of command extracted
    if (iPolCurentPosition==0) set_shafts(0x002+0x004);
    else set_shafts(0x001+0x008);
  }
  if (butt_sw1.isRelease())
  {
    if (iPolCurentPosition==0) iPolCurentPosition=1;
    else iPolCurentPosition=0;
    set_shafts(0);
  }
  if (butt_sw2.isPress())
  { if (iPolCurentPosition>0) set_shafts(0x002+0x004);
    else set_shafts(0x001+0x008);
  }
  if (butt_sw2.isRelease()) set_shafts(0);
}

///////////////////////////////////////////////////////////////
// саржа 1-2-3-4 
///////////////////////////////////////////////////////////////
void prog_sarga1234()
{
  if(butt_sw1.isPress())
  { // button pressed, send command
    //  Serial.println(iPolCurentPosition); // debug display of command extracted
    if (iPolCurentPosition==0) set_shafts(0x1111);
    else if (iPolCurentPosition==1) set_shafts(0x2222);
      else if (iPolCurentPosition==2) set_shafts(0x4444);
      else set_shafts(0x8888);
  }
  if (butt_sw1.isRelease())
  {
    iPolCurentPosition = iPolCurentPosition + 1;
    if (iPolCurentPosition == 4) iPolCurentPosition = 0;
    set_shafts(0);
  }
  if (butt_sw2.isPress())
  { 
    if (iPolCurentPosition==0) set_shafts(0x8888);
    else if (iPolCurentPosition==1) set_shafts(0x1111);
      else if (iPolCurentPosition==2) set_shafts(0x2222);
      else set_shafts(0x4444);
  
  }
  if (butt_sw2.isRelease()) set_shafts(0);
}

///////////////////////////////////////////////////////////////
// саржа 12-23-34-41 
///////////////////////////////////////////////////////////////
void prog_sarga12233441()
{
  if(butt_sw1.isPress())
  { // button pressed, send command
    //  Serial.println(iPolCurentPosition); // debug display of command extracted
    if (iPolCurentPosition==0) set_shafts(0x1111+0x2222);
    else if (iPolCurentPosition==1) set_shafts(0x2222+0x4444);
      else if (iPolCurentPosition==2) set_shafts(0x4444+0x8888);
      else set_shafts(0x8888+0x1111);
  }
  if (butt_sw1.isRelease())
  {
    iPolCurentPosition = iPolCurentPosition + 1;
    if (iPolCurentPosition == 4) iPolCurentPosition = 0;
    set_shafts(0);
  }
  if (butt_sw2.isPress())
  { 
    if (iPolCurentPosition==0) set_shafts(0x8888+0x1111);
    else if (iPolCurentPosition==1) set_shafts(0x1111+0x2222);
      else if (iPolCurentPosition==2) set_shafts(0x2222+0x4444);
      else set_shafts(0x4444+0x8888);
  
  }
  if (butt_sw2.isRelease()) set_shafts(0);
}

///////////////////////////////////////////////////////////////
// саржа 1-3-2-4   рваная 
///////////////////////////////////////////////////////////////
void prog_sarga1324()
{
  if(butt_sw1.isPress())
  { // button pressed, send command
    //  Serial.println(iPolCurentPosition); // debug display of command extracted
    if (iPolCurentPosition==0) set_shafts(0x1111);
    else if (iPolCurentPosition==1) set_shafts(0x4444);
      else if (iPolCurentPosition==2) set_shafts(0x2222);
      else set_shafts(0x8888);
  }
  if (butt_sw1.isRelease())
  {
    iPolCurentPosition = iPolCurentPosition + 1;
    if (iPolCurentPosition == 4) iPolCurentPosition = 0;
    set_shafts(0);
  }
  if (butt_sw2.isPress())
  { 
    if (iPolCurentPosition==0) set_shafts(0x8888);
    else if (iPolCurentPosition==1) set_shafts(0x1111);
      else if (iPolCurentPosition==2) set_shafts(0x4444);
      else set_shafts(0x2222);
  
  }
  if (butt_sw2.isRelease()) set_shafts(0);
}


///////////////////////////////////////////////////////////////
// Управление программным режимом 
// кнопки swA1-swA4 программные режимы
 ///////////////////////////////////////////////////////////////

void monitor_button_prog()
{ // программа - полотняное переплетение
  butt_sw1.tick();
  butt_sw2.tick();
  butt_swA1.tick();
  butt_swA2.tick();
  butt_swA3.tick();
  butt_swA4.tick();
  stepper1.tick();
  stepper2.tick();
  stepper3.tick();
  stepper4.tick();
  stepper5.tick();
  stepper6.tick();
  stepper7.tick();
  stepper8.tick();
  stepper9.tick();
  stepper10.tick();
  stepper11.tick();
  stepper12.tick();
  stepper13.tick();
  stepper14.tick();
  stepper15.tick();
  stepper16.tick();
  


  if (butt_swA1.state() == 1 && butt_swA2.state() == 0 && butt_swA3.state() == 0 && butt_swA4.state() == 0) prog_polotno();
  if (butt_swA1.state() == 0 && butt_swA2.state() == 1 && butt_swA3.state() == 0 && butt_swA4.state() == 0) prog_sarga1234();
  if (butt_swA1.state() == 0 && butt_swA2.state() == 0 && butt_swA3.state() == 1 && butt_swA4.state() == 0) prog_sarga12233441();
  if (butt_swA1.state() == 0 && butt_swA2.state() == 0 && butt_swA3.state() == 0 && butt_swA4.state() == 1) prog_sarga1324();
}


//////////////////////////////////////////////////
// TOP LEVEL FUNCTIONS - SETUP & LOOP //
//////////////////////////////////////////////////

void setup() {
 // USB Interface to host computer
  Serial.begin(9600); // start serial port at 9600 bps:
  Serial.print(IDENTITY); //print message on serial monitor
  // регулятор скорости
  pinMode(A5, INPUT);

  // устанавливаем опрос кнопок на автоматический
  butt_sw1.setTickMode(AUTO);
  butt_sw2.setTickMode(AUTO);
  butt_swA1.setTickMode(AUTO);
  butt_swA2.setTickMode(AUTO);
  butt_swA3.setTickMode(AUTO);
  butt_swA4.setTickMode(AUTO);
  butt_sw1.setType(HIGH_PULL);
  butt_sw2.setType(HIGH_PULL);
  butt_swA1.setType(HIGH_PULL);
  butt_swA2.setType(HIGH_PULL);
  butt_swA3.setType(HIGH_PULL);
  butt_swA4.setType(HIGH_PULL);
  //настройка шаговиков
  stepper1.setMaxSpeed(MaxSpeed);
  stepper1.setAcceleration(Acceleration);
  stepper1.setTarget(0);
  stepper2.setMaxSpeed(MaxSpeed);
  stepper2.setAcceleration(Acceleration);
  stepper2.setTarget(0);
  stepper3.setMaxSpeed(MaxSpeed);
  stepper3.setAcceleration(Acceleration);
  stepper3.setTarget(0);
  stepper4.setMaxSpeed(MaxSpeed);
  stepper4.setAcceleration(Acceleration);
  stepper4.setTarget(0);
  stepper5.setMaxSpeed(MaxSpeed);
  stepper5.setAcceleration(Acceleration);
  stepper5.setTarget(0);
  stepper6.setMaxSpeed(MaxSpeed);
  stepper6.setAcceleration(Acceleration);
  stepper6.setTarget(0);
  stepper7.setMaxSpeed(MaxSpeed);
  stepper7.setAcceleration(Acceleration);
  stepper7.setTarget(0);
  stepper8.setMaxSpeed(MaxSpeed);
  stepper8.setAcceleration(Acceleration);
  stepper8.setTarget(0);
  stepper9.setMaxSpeed(MaxSpeed);
  stepper9.setAcceleration(Acceleration);
  stepper9.setTarget(0);
  stepper10.setMaxSpeed(MaxSpeed);
  stepper10.setAcceleration(Acceleration);
  stepper10.setTarget(0);
  stepper11.setMaxSpeed(MaxSpeed);
  stepper11.setAcceleration(Acceleration);
  stepper11.setTarget(0);
  stepper12.setMaxSpeed(MaxSpeed);
  stepper12.setAcceleration(Acceleration);
  stepper12.setTarget(0);
  stepper13.setMaxSpeed(MaxSpeed);
  stepper13.setAcceleration(Acceleration);
  stepper13.setTarget(0);
  stepper14.setMaxSpeed(MaxSpeed);
  stepper14.setAcceleration(Acceleration);
  stepper14.setTarget(0);
  stepper15.setMaxSpeed(MaxSpeed);
  stepper15.setAcceleration(Acceleration);
  stepper15.setTarget(0);
  stepper16.setMaxSpeed(MaxSpeed);
  stepper16.setAcceleration(Acceleration);
  stepper16.setTarget(0);
}

void loop() {
  butt_swA1.tick();
  butt_swA2.tick();
  butt_swA3.tick();
  butt_swA4.tick();
  
  stepper1.tick();
  stepper2.tick();
  stepper3.tick();
  stepper4.tick();
  stepper5.tick();
  stepper6.tick();
  stepper7.tick();
  stepper8.tick();
  stepper9.tick();
  stepper10.tick();
  stepper11.tick();
  stepper12.tick();
  stepper13.tick();
  stepper14.tick();
  stepper15.tick();
  stepper16.tick();
  
  if (butt_swA1.isHold() || butt_swA2.isHold() || butt_swA3.isHold() || butt_swA4.isHold()) {
    monitor_button_prog();
  }
  else {
    serial_input(); // check the serial interface, handle if it comes in
    monitor_button(); // check for button push  
  }
}