/**************************************************************************************************
 *
 *      OLED display simple none blocking menu System - i2c version SSD1306 - 15mar23
 *
 *      This sketch is on Github:   
 *
 **************************************************************************************************

 The sketch displays a menu on the oled and when an item is selected it sets a
 flag and waits until the event is acted upon.  Max menu items on a 128x64 oled
 is four.

 Notes:   text size 1 = 21 x 8 characters on the larger oLED display
          text size 2 = 10 x 4

 For more oled info    see: https://randomnerdtutorials.com/guide-for-oled-display-with-arduino/

 See the "menus below here" section for examples of how to use the menus

 Note: If you get garbage on the display and the device locking up etc. it may just be a poor connection
       to the rotary encoder


 **************************************************************************************************/
// Sketch for CamSlider
// code: u2gcZInumkk
// 06/04/2019 by RZtronics <[email protected]>
// Project homepage: http://RZtronics.com/
///////////////////////////////////////////////////////////////////////////////////////
//Terms of use
///////////////////////////////////////////////////////////////////////////////////////
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//THE SOFTWARE.
///////////////////////////////////////////////////////////////////////////////////////

/***************************************************************************************************

March 15, 2023
I have taken this code and I have made some modifications:
  1. It was designed for Arduino Nano, now it is conditioned for an ESP32
  2. I have added some signaling lights, for signaling, when the motors are on

***************************************


When I using an ESP32, I get runtime errors, 
I'll leave it as it is and try with an ARDUINO NANO,
 which is the original board for this project.

 It is possible that it is a problem with the simulator
  or perhaps physically if it works correctly.
***************************************************************************************************/



// ----------------------------------------------------------------
//                        L I B R A R I E S
// ----------------------------------------------------------------

#include <Wire.h>
#include <Adafruit_GFX.h>  //ok
#include <Adafruit_SSD1306.h>  //ok
#include <AccelStepper.h>
#include <MultiStepper.h>
#include "bitmap.h"


// ----------------------------------------------------------------
//                         S E T T I N G S
// ----------------------------------------------------------------

// limitSwitch
#define limitSwitch 34  // Original data 11

// Rotary Encoder
#define PinSW  14  // Original data 2   (SW)
#define PinDT  13  // Original data 8   (DT)
#define PinCLK 12  // Original data 3   (CLK)
#define OLEDC 0   // oled clock pin (set to 0 for default)
#define OLEDD 0                           // oled data pin
#define OLEDE 0                           // oled enable pin
#define rDIR 26
#define yDIR 27
#define rSTEP 15
#define ySTEP 2


// Display Oled
#define OLED_RESET 21 // SDA Original data 4   

    // oLED
#define OLED_ADDR 0x3C                    // OLED i2c address
#define SCREEN_WIDTH 128                  // OLED display width, in pixels (usually 128)
#define SCREEN_HEIGHT 64                  // OLED display height, in pixels (64 for larger oLEDs)
#define OLED_RESET -1                     // Reset pin gpio (or -1 if sharing Arduino reset pin)

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);





// Steppers 
AccelStepper stepper2(1, ySTEP, yDIR); // eje Y (AMARILLO)  (Type:driver, STEP, DIR)
AccelStepper stepper1(1, rSTEP, rDIR); // eje X (ROJO)


MultiStepper StepperControl; 

long gotoposition[2]; 

volatile long XInPoint=0;
volatile long YInPoint=0;
volatile long XOutPoint=0;
volatile long YOutPoint=0;  
volatile long totaldistance=0;
int flag=0; 
int temp=0;
int i,j;
unsigned long switch0=0;
unsigned long rotary0=0;
float setspeed=200;
float motorspeed;
float timeinsec;
float timeinmins;
volatile boolean TurnDetected;  
volatile boolean rotationdirection;  

const int serialDebug = 1;

void Switch()  
{
 if(millis()-switch0>50)
 {
 flag=flag+1;
 }
 switch0=millis();
}

void Rotary()  
{
delay(75);
if (digitalRead(PinCLK))
rotationdirection= digitalRead(PinDT);
else
rotationdirection= !digitalRead(PinDT);
TurnDetected = true;
delay(75);
}


// ----------------------------------------------------------------
//                         S E T U P
// ----------------------------------------------------------------

void setup() 
{
stepper1.setPinsInverted (true,false,false);
 




  //display.setRotation(2);
  Serial.begin(115200);
  stepper1.setMaxSpeed(1000);
  stepper1.setSpeed(200);
  stepper2.setMaxSpeed(1000);
  stepper2.setSpeed(200);
  
  pinMode(limitSwitch, INPUT_PULLUP);
  pinMode(PinSW,INPUT_PULLUP); 
  pinMode(PinCLK,INPUT_PULLUP); 
  pinMode(PinDT,INPUT_PULLUP);  
  
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); 
  display.clearDisplay();
  
  // Create instances for MultiStepper - Adding the 2 steppers to the StepperControl instance for multi control
  StepperControl.addStepper(stepper1);
  StepperControl.addStepper(stepper2);

  attachInterrupt (digitalPinToInterrupt(PinSW),Switch,RISING); // SW connected to D2
  attachInterrupt (digitalPinToInterrupt(PinCLK),Rotary,RISING); // CLK Connected to D3
   
  // display Boot logo
  display.drawBitmap(0, 0, CamSlider, 128, 64, 1);
  display.display();
  delay(2000);
  display.clearDisplay(); 
  
  Home(); // Move the slider to the initial position - homing
  
}

void Home()
{
  stepper1.setMaxSpeed(1000);
  stepper1.setSpeed(100);
  stepper2.setMaxSpeed(1000);
  stepper2.setSpeed(100);
  if(digitalRead(limitSwitch)==0)
  {
    display.drawBitmap(0, 0, Homing, 128, 64, 1);
    display.display(); 
  }
  
  while (digitalRead(limitSwitch)== 0) 
  {
    stepper1.setSpeed(-1000);
    stepper1.runSpeed();
    
 }
  delay(100);
  stepper1.setCurrentPosition(0);
    stepper1.moveTo(200);
    while(stepper1.distanceToGo() != 0)
    {
      stepper1.setSpeed(3000);
      stepper1.runSpeed();
    }
    stepper1.setCurrentPosition(0);
    display.clearDisplay();
}

void SetSpeed()
{
  display.clearDisplay();
  while( flag==6)
  {
  if (TurnDetected)  
  {
        TurnDetected = false;  // do NOT repeat IF loop until new rotation detected
        if (rotationdirection) 
        { 
         setspeed = setspeed + 30;
        }
        if (!rotationdirection) 
        { 
         setspeed = setspeed - 30;        
         if (setspeed < 0) 
        { 
         setspeed = 0;
        }
        }

          display.clearDisplay();
          display.setTextSize(2);
          display.setTextColor(WHITE);
          display.setCursor(30,0);
          display.print("Speed");
          motorspeed=setspeed/80;
          display.setCursor(5,16);
          display.print(motorspeed);
          display.print(" mm/s");   
          totaldistance=XOutPoint-XInPoint;

          if(totaldistance<0)
            {
              totaldistance=totaldistance*(-1);
            }
            else
            {
              
            }
          timeinsec=(totaldistance/setspeed);
          timeinmins=timeinsec/60;
          display.setCursor(35,32);
          display.print("Time");
          display.setCursor(8,48);
          if(timeinmins>1)
          {
          display.print(timeinmins);
          display.print(" min");
          }
          else
          { 
          display.print(timeinsec);
          display.print(" sec");
          }
          display.display();
  }
          display.clearDisplay();
          display.setTextSize(2);
          display.setTextColor(WHITE);
          display.setCursor(30,0);
          display.print("Speed");
          motorspeed=setspeed/80;
          display.setCursor(5,16);
          display.print(motorspeed);
          display.print(" mm/s");   
          totaldistance=XOutPoint-XInPoint;
           if(totaldistance<0)
            {
              totaldistance=totaldistance*(-1);
            }
            else
            {
              
            }
          timeinsec=(totaldistance/setspeed);
          timeinmins=timeinsec/60;
          display.setCursor(35,32);
          display.print("Time");
          display.setCursor(8,48);
          if(timeinmins>1)
          {
          display.print(timeinmins);
          display.print(" min");
          }
          else
          { 
          display.print(timeinsec);
          display.print(" sec");
          }
          display.display();
  }
 
}

void stepperposition(int n)
{
  stepper1.setMaxSpeed(1000);
  stepper1.setSpeed(200);
  stepper2.setMaxSpeed(1000);
  stepper2.setSpeed(200);
  if (TurnDetected)  
  {
        TurnDetected = false;  // do NOT repeat IF loop until new rotation detected
     if(n==1)
     {
        if (!rotationdirection) 
        { 
          if( stepper1.currentPosition()-500>0 )
          {
          stepper1.move(-500);
          while(stepper1.distanceToGo() != 0)
            {
              stepper1.setSpeed(-3000);
              stepper1.runSpeed();
            }
          }
            else
            {
                while (stepper1.currentPosition()!=0) 
                 {
                      stepper1.setSpeed(-3000);
                      stepper1.runSpeed();
                 }
            }
        }

        if (rotationdirection) 
        { 
          if( stepper1.currentPosition()+500<61000 )
          {
          stepper1.move(500);
          while(stepper1.distanceToGo() != 0)
            {
              stepper1.setSpeed(3000);
              stepper1.runSpeed();
            }
          }
          else
          {
            while (stepper1.currentPosition()!= 61000) 
             {
                  stepper1.setSpeed(3000);
                  stepper1.runSpeed();
    
             } 
          }
        }
     }
     if(n==2)
     {
       if (rotationdirection) 
       { 
         stepper2.move(-100);
         while(stepper2.distanceToGo() != 0)
           {
             stepper2.setSpeed(-3000);
             stepper2.runSpeed();
           }      
       }
        if (!rotationdirection) 
       { 
         stepper2.move(100);
         while(stepper2.distanceToGo() != 0)
           {
             stepper2.setSpeed(3000);
             stepper2.runSpeed();
           } 
       }
     }
     }
} 


// ----------------------------------------------------------------
//                         L O O P
// ----------------------------------------------------------------

void loop()
{ 
  //Begin Setup
  if(flag==0)
  {
    display.clearDisplay();
    display.drawBitmap(0, 0, BeginSetup, 128, 64, 1);
    display.display(); 
    setspeed=200;
  }
  
  //SetXin   (MOTOR ROJO)
  if(flag==1)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(10,28);
    display.println("Set X In");
    display.display(); 
    while(flag==1)
    {
    stepperposition(1);  
    }
    XInPoint=stepper1.currentPosition();
  }
  //SetYin   (MOTOR AMARILLO)
  if(flag==2)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(10,28);
    display.println("Set Y In");
    display.display(); 
    while(flag==2)
    {
    stepperposition(2);
    }
    stepper2.setCurrentPosition(0);
    YInPoint=stepper2.currentPosition();
  }
  //SetXout
  if(flag==3)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(10,28);
    display.println("Set X Out");
    display.display(); 
    while(flag==3)
    {
    stepperposition(2);
    Serial.println(stepper1.currentPosition());
    }
    XOutPoint=stepper1.currentPosition();
    
  }
  //SetYout
  if(flag==4)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(10,28);
    display.println("Set Y Out");
    display.display();
    while(flag==4)
    {
    stepperposition(1);
    }
    YOutPoint=stepper2.currentPosition();
    display.clearDisplay();
        
    // Go to IN position
    gotoposition[0]=XInPoint;
    gotoposition[1]=YInPoint;    
    display.clearDisplay();
    display.setCursor(8,28);
    display.println(" Preview  ");
    display.display(); 
    stepper1.setMaxSpeed(1000);
    StepperControl.moveTo(gotoposition);
    StepperControl.runSpeedToPosition();
  }

  //Display Set Speed
  if(flag==5)
  {
    display.clearDisplay();
    display.setCursor(8,28);
    display.println("Set Speed");
    display.display();  
  }
  //Change Speed
  if(flag==6)
  {
    display.clearDisplay();
    SetSpeed();
  }
  //DisplayStart
  if(flag==7)
  {
    display.clearDisplay();
    display.setCursor(30,27);
    display.println("Start");
    display.display();
  }
  //Start
  if(flag==8)
  { 
    display.clearDisplay();
    display.setCursor(20,27);
    display.println("Running");
    display.display();
    Serial.println(XInPoint);
    Serial.println(XOutPoint);
    Serial.println(YInPoint);
    Serial.println(YOutPoint);

    gotoposition[0]=XOutPoint;
    gotoposition[1]=YOutPoint;
    stepper1.setMaxSpeed(setspeed);
    StepperControl.moveTo(gotoposition);
    StepperControl.runSpeedToPosition();
    flag=flag+1;
  }
  //Slide Finish
   if(flag==9)
   {
    display.clearDisplay();
    display.setCursor(24,26);
    display.println("Finish");
    display.display();
   }
  //Return to start
   if(flag==10)
   {
    display.clearDisplay();
    Home();
    flag=0;
   }  
}