#include <AccelStepper.h>

// Define the stepper motor connections
// 1mm pitch
#define STEP_PIN_1 4
#define DIR_PIN_1 5
#define EN_PIN_1 A4
// 10mm pitch
#define STEP_PIN_2 A3
#define DIR_PIN_2 A2
#define EN_PIN_2 A5

// Create an AccelStepper object
AccelStepper stepper_1mm(1, STEP_PIN_1, DIR_PIN_1);
AccelStepper stepper_5mm(1, STEP_PIN_2, DIR_PIN_2);

//Define LED pins
int mode_1 = 6;         // Pin for LED OVDC
int mode_2 = 7;         // Pin for LED +5.4VDC
int mode_3 = 8;         // Pin for LED -5.4VDC
int mode_4 = 9;         // Pin for LED 10V 0.2HZ
int mode_5 = 10;        // Pin for LED 9.5V 0.02
int mode_6 = 11;        // Pin for LED 0.006V 0.8 Hz
int mode_7 = 12;        // Pin for LED 7.5V 0.45HZ
int mode_8 = 13;        // Pin for LED 0.016V 0.01HZ
int mode_9 = A1;        // Pin for LED 0.016V 0.8HZ
int bussy_LED = A0;     // Pin for Bussy LED
int Select_button = 2;  //Pin for Select button
int Start_button = 3;  //Pin for Start button

int cont = 0;  //defining case selector variable
int cycles = 0;       // actual count cycles
int set_cycles = 0;   // cycles to achieve

// Definig arbitrary Sine wave parameters should be modified in each Mode
float amplitude = 100;          // Amplitude of the sine wave (adjust as needed)
float frequency = 0.1;          // Frequency of the sine wave (adjust as needed)
const int steps_per_rev = 200;  // Steps per revolution of your stepper motor
int  stepper_pos=0;              //variable to store reading stepper position from currentPosition();
int stepper_previous_pos=0;     //variable used to store position -1 because when frecuency is too slow i need to know crossing zero

void setup() 
{
	Serial.begin(115200);
  pinMode(Select_button, INPUT_PULLUP); //Select button
	pinMode(Start_button, INPUT_PULLUP); //Start Button
  pinMode(mode_1, OUTPUT);
	pinMode(mode_2, OUTPUT);
	pinMode(mode_3, OUTPUT);
	pinMode(mode_4, OUTPUT);
	pinMode(mode_5, OUTPUT);
	pinMode(mode_6, OUTPUT);
	pinMode(mode_7, OUTPUT);
	pinMode(mode_8, OUTPUT);
	pinMode(mode_9, OUTPUT); 
	pinMode(bussy_LED, OUTPUT); 
	// Set the maximum speed and acceleration of the stepper motor
  stepper_1mm.setMaxSpeed(250000);  // 100 000 Steps per Second => 3000RPM  Microstepping 5000 Steps / Rev  just a reference Arduino can only go up to 4000 steps persecond using accel stepper but there is no more required
  stepper_1mm.setAcceleration(250000);
	stepper_5mm.setMaxSpeed(10000);   // 10 000 Steps per Second => 3000RPM  Microstepping 200 Steps / Rev just a reference Arduino can only go up to 4000 steps persecond using accel stepper but there is no more required
  stepper_5mm.setAcceleration(20000);
	digitalWrite(EN_PIN_1, HIGH);
	digitalWrite(EN_PIN_2, HIGH);
}

void loop() 
{
  if (digitalRead(Select_button) == LOW) 
	{
    cont++;
		delay(200);
		if(cont==10)
		{
			cont=1;
		}
  }

	switch (cont) 
	{
		case 0:
			break;
    
		case 1:  
			mode_1_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_1_void();
			}
      break;
    
		case 2:  
      mode_2_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_2_void();
			}
      break;
    
		case 3:  
      mode_3_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_3_void();
			}
      break;
		
		case 4:  
      mode_4_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_4_void();
			}
      break;
		
		case 5:  
      mode_5_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_5_void();
			}
      break;
		
		case 6:  
      mode_6_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_6_void();
			}
      break;
		
		case 7:  
      mode_7_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_7_void(); 
			}
      break;
		
		case 8:  
      mode_8_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_8_void();
			}
      break;
		case 9:  
      mode_9_ON();
			if(digitalRead(Start_button)==LOW)
			{
					mode_9_void();
			}
      break;					
  }
}

void mode_1_ON() 
{
	digitalWrite(mode_1, HIGH); //turn on Mode 1 LED the rest OFF
	digitalWrite(mode_2, LOW);
	digitalWrite(mode_3, LOW);
	digitalWrite(mode_4, LOW);
	digitalWrite(mode_5, LOW);
	digitalWrite(mode_6, LOW);
	digitalWrite(mode_7, LOW);
	digitalWrite(mode_8, LOW);
	digitalWrite(mode_9, LOW);
}

void mode_2_ON() 
{
	digitalWrite(mode_1, LOW); 
	digitalWrite(mode_2, HIGH);//turn on Mode 2 LED the rest OFF
	digitalWrite(mode_3, LOW);
	digitalWrite(mode_4, LOW);
	digitalWrite(mode_5, LOW);
	digitalWrite(mode_6, LOW);
	digitalWrite(mode_7, LOW);
	digitalWrite(mode_8, LOW);
	digitalWrite(mode_9, LOW);
}

void mode_3_ON() 
{
	digitalWrite(mode_1, LOW); 
	digitalWrite(mode_2, LOW);
	digitalWrite(mode_3, HIGH);//turn on Mode 3 LED the rest OFF
	digitalWrite(mode_4, LOW);
	digitalWrite(mode_5, LOW);
	digitalWrite(mode_6, LOW);
	digitalWrite(mode_7, LOW);
	digitalWrite(mode_8, LOW);
	digitalWrite(mode_9, LOW);
}

void mode_4_ON() 
{
	digitalWrite(mode_1, LOW); 
	digitalWrite(mode_2, LOW);
	digitalWrite(mode_3, LOW);
	digitalWrite(mode_4, HIGH);//turn on Mode 4 LED the rest OFF
	digitalWrite(mode_5, LOW);
	digitalWrite(mode_6, LOW);
	digitalWrite(mode_7, LOW);
	digitalWrite(mode_8, LOW);
	digitalWrite(mode_9, LOW);
}

void mode_5_ON() 
{
	digitalWrite(mode_1, LOW); 
	digitalWrite(mode_2, LOW);
	digitalWrite(mode_3, LOW);
	digitalWrite(mode_4, LOW);
	digitalWrite(mode_5, HIGH);//turn on Mode 5 LED the rest OFF
	digitalWrite(mode_6, LOW);
	digitalWrite(mode_7, LOW);
	digitalWrite(mode_8, LOW);
	digitalWrite(mode_9, LOW);
}

void mode_6_ON() 
{
	digitalWrite(mode_1, LOW); 
	digitalWrite(mode_2, LOW);
	digitalWrite(mode_3, LOW);
	digitalWrite(mode_4, LOW);
	digitalWrite(mode_5, LOW);
	digitalWrite(mode_6, HIGH);//turn on Mode 6 LED the rest OFF
	digitalWrite(mode_7, LOW);
	digitalWrite(mode_8, LOW);
	digitalWrite(mode_9, LOW);
}

void mode_7_ON() 
{
	digitalWrite(mode_1, LOW); 
	digitalWrite(mode_2, LOW);
	digitalWrite(mode_3, LOW);
	digitalWrite(mode_4, LOW);
	digitalWrite(mode_5, LOW);
	digitalWrite(mode_6, LOW);
	digitalWrite(mode_7, HIGH);//turn on Mode 7 LED the rest OFF
	digitalWrite(mode_8, LOW);
	digitalWrite(mode_9, LOW);
}

void mode_8_ON() 
{
	digitalWrite(mode_1, LOW); 
	digitalWrite(mode_2, LOW);
	digitalWrite(mode_3, LOW);
	digitalWrite(mode_4, LOW);
	digitalWrite(mode_5, LOW);
	digitalWrite(mode_6, LOW);
	digitalWrite(mode_7, LOW);
	digitalWrite(mode_8, HIGH);//turn on Mode 8 LED the rest OFF
	digitalWrite(mode_9, LOW);
}

void mode_9_ON() 
{
	digitalWrite(mode_1, LOW); 
	digitalWrite(mode_2, LOW);
	digitalWrite(mode_3, LOW);
	digitalWrite(mode_4, LOW);
	digitalWrite(mode_5, LOW);
	digitalWrite(mode_6, LOW);
	digitalWrite(mode_7, LOW);
	digitalWrite(mode_8, LOW);
	digitalWrite(mode_9, HIGH);//turn on Mode 9 LED the rest OFF
}

void mode_1_void()
{
amplitude = 0;        //OVDC(COMMAND fast motor) = 0 Steps
stepper_go_to();
}

void mode_2_void()
{
amplitude = 1305;     //+5.4 VDC(COMMAND fast motor) = 1305 Steps
stepper_go_to();
}

void mode_3_void()
{
amplitude = -1305;    //-5.4 VDC(COMMAND fast motor) = -1305 Steps (minus means reverse direction)
stepper_go_to();
}

void mode_4_void()
{
amplitude = 1305;     //-5.4 VDC aproximatelly 10V peak to Peak (COMMAND fast motor) = 1305 Steps
frequency = 0.2;      
set_cycles = 200;     //100 cycles
move_motor_fast();
}

void mode_5_void()
{
amplitude = 1100;      //9.5V VDC(RUDDER FORCE fast motor) 0.02Hz and no pressure means full travel of the lever
frequency = 0.02;
set_cycles = 2;        //just one cycle
move_motor_fast();
}

void mode_6_void()
{
amplitude = 763;       //+- 0.006 inch 0.8Hz(RUDDER FORCE slow Motor)
frequency = 0.8;
set_cycles = 2;        //just one cycle
move_motor_slow();
}

void mode_7_void()
{
amplitude = 1003;      //1003 original value 7.5V Amplitude (RUDDER FORCE fast motor) 0.45Hz
frequency = 0.45;
set_cycles = 2;         //just one cycle
move_motor_fast();
}

void mode_8_void()
{
amplitude = 255;      //0.016V Peak to Peak(RUDDER RESOLUTION slow motor) 0.01Hz 
frequency = 0.01;
set_cycles = 2;         //just one cycle
move_motor_slow();
}

void mode_9_void()
{
amplitude = 255;      //0.016V Peak to Peak(RUDDER RESOLUTION slow motor) 0.8Hz
frequency = 0.8;
set_cycles = 2;         //just one cycle
move_motor_slow();
}

void move_motor_slow()   //this function MOVES THE SLOW STEPPER in a sine movement
{
	digitalWrite(EN_PIN_1, LOW); //Enable Slow Motor
	stepper_1mm.setCurrentPosition(0);
	float phase = millis() * frequency * 2 * PI / 1000.0;	//Calculating Phase
	//Phase it is necesary to start sine wave always from zero
  while(cycles!=set_cycles)    // comparing the actual cycles with the desired cycles
	{
		// Calculate the position based on the sine wave
  	float angle = millis() * frequency * 2 * PI / 1000.0;  // Use millis() for time
  	int position = amplitude * sin(angle-phase);
  	stepper_1mm.moveTo(position);// Move the stepper to the calculated position
		stepper_1mm.run();// Run the stepper motor
		stepper_pos = stepper_1mm.currentPosition();
		if (stepper_pos==0 && stepper_previous_pos !=0)
		{
			cycles++;
			stepper_previous_pos=0;
		}
		if (stepper_pos!=0)
		{
			stepper_previous_pos=1;
		}
		digitalWrite(bussy_LED, HIGH);  //turning ON BUSSY LED
		//Serial.println(cycles);
	}
  digitalWrite(bussy_LED, LOW);   //turning OFF BUSSY LED
	
	cycles = 0;
	stepper_previous_pos=0;
	//resetFunc();
	phase = 0;
	amplitude = 0;
	frequency = 0;
	//position = 0;
	digitalWrite(EN_PIN_1, HIGH); //Disable Slow Motor
}

void move_motor_fast()   //this function MOVES THE FAST STEPPER in a sine movement
{
	digitalWrite(EN_PIN_2, LOW); //Enable Fast motor
	stepper_5mm.setCurrentPosition(0);
	float phase = millis() * frequency * 2 * PI / 1000.0;	//Calculating Phase
	//Phase it is necesary to start sine wave always from zero
  while(cycles!=set_cycles)    // comparing the actual cycles with the desired cycles
	{
		// Calculate the position based on the sine wave
  	float angle = millis() * frequency * 2 * PI / 1000.0;  // Use millis() for time
  	int position = amplitude * sin(angle-phase);
  	stepper_5mm.moveTo(position);// Move the stepper to the calculated position
		stepper_5mm.run();// Run the stepper motor
		stepper_pos = stepper_5mm.currentPosition();
		if (stepper_pos==0 && stepper_previous_pos !=0)
		{
			cycles++;
			stepper_previous_pos=0;
		}
		if (stepper_pos!=0)
		{
			stepper_previous_pos=1;
		}
		digitalWrite(bussy_LED, HIGH);  //turning ON BUSSY LED
		//Serial.println(stepper_pos);
	}
  digitalWrite(bussy_LED, LOW);   //turning OFF BUSSY LED
	
	cycles = 0;
	stepper_previous_pos=0;
	//resetFunc();
	phase = 0;
	amplitude = 0;
	frequency = 0;
	//position = 0;
	digitalWrite(EN_PIN_2, HIGH); //Disable Fast Motor
}

void stepper_go_to()    //this function moves the motor to desired position just once
{
digitalWrite(EN_PIN_2, LOW); //Enable Fast motor
stepper_5mm.moveTo(amplitude);  
stepper_pos = stepper_5mm.currentPosition();
while(stepper_pos!=amplitude) // to run stepper.run() until desired amplitude will be aceived
{
stepper_5mm.run();  
stepper_pos = stepper_5mm.currentPosition();
digitalWrite(bussy_LED, HIGH);  //turning ON BUSSY LED
}
digitalWrite(bussy_LED, LOW);   //turning OFF BUSSY L
digitalWrite(EN_PIN_2, HIGH); //Disable Fast Motor
}