#include <Arduino.h>
#include "A4988.h" // A4988 stepper motor driver library
#include <Servo.h>  // For other actuators if needed

// Define the number of steps for the 28BYJ-48 motor (considering bipolar connection)
#define MOTOR_STEPS 2048 // Stepper motor full rotation (steps per revolution for 28BYJ-48)

// Actuatoare
  // FLAPERON R
    #define DIR_FLAPERON_R 0
    #define STEP_FLAPERON_R 1
    A4988 stepper_FLAPERON_R(MOTOR_STEPS, DIR_FLAPERON_R, STEP_FLAPERON_R);

  // FLAPERON L
    #define DIR_FLAPERON_L 2
    #define STEP_FLAPERON_L 3
    A4988 stepper_FLAPERON_L(MOTOR_STEPS, DIR_FLAPERON_L, STEP_FLAPERON_L);

  // STABILATOR R
    #define DIR_STABILATOR_R 4
    #define STEP_STABILATOR_R 5
    A4988 stepper_STABILATOR_R(MOTOR_STEPS, DIR_STABILATOR_R, STEP_STABILATOR_R);

  // STABILATOR L
    #define DIR_STABILATOR_L 6
    #define STEP_STABILATOR_L 7
    A4988 stepper_STABILATOR_L(MOTOR_STEPS, DIR_STABILATOR_L, STEP_STABILATOR_L);

  // DIRECTIE (Yaw)
    #define DIR_DIRECTIE 8
    #define STEP_DIRECTIE 9
    A4988 stepper_DIRECTIE(MOTOR_STEPS, DIR_DIRECTIE, STEP_DIRECTIE);

  // LEADING EDGE FLAP
    #define DIR_LEADING_EDGE 10
    #define STEP_LEADING_EDGE 11
    A4988 stepper_LEADING_EDGE(MOTOR_STEPS, DIR_LEADING_EDGE, STEP_LEADING_EDGE);

// SEMNALE
int semnal_mansa_ruliu = 0;
int semnal_mansa_tangaj = 0;
int semnal_palonier = 0;

// Variables pentru actuatoare
int actuator_flaperon_R = 0;
int actuator_flaperon_L = 0;
int actuator_stabilator_R = 0;
int actuator_stabilator_L = 0;
int actuator_directie = 0;
int actuator_leading_edge_flap = 0;

// Valori anterioare pentru a asigura mișcări liniare
int previous_flaperon_R = 0;
int previous_flaperon_L = 0;
int previous_stabilator_R = 0;
int previous_stabilator_L = 0;
int previous_directie = 0;
int previous_leading_edge_flap = 0;

void setup() {
  // Set motor speeds and microsteps
  stepper_FLAPERON_R.setRPM(150); // Set RPM for flaperon right
  stepper_FLAPERON_R.setMicrostep(0);

  stepper_FLAPERON_L.setRPM(150); // Set RPM for flaperon left
  stepper_FLAPERON_L.setMicrostep(0);

  stepper_STABILATOR_R.setRPM(150); // Set RPM for stabilator right
  stepper_STABILATOR_R.setMicrostep(0);

  stepper_STABILATOR_L.setRPM(150); // Set RPM for stabilator left
  stepper_STABILATOR_L.setMicrostep(0);

  stepper_DIRECTIE.setRPM(250); // Set RPM for direction (yaw)
  stepper_DIRECTIE.setMicrostep(0);

  stepper_LEADING_EDGE.setRPM(250); // Set RPM for leading edge flap
  stepper_LEADING_EDGE.setMicrostep(0);
}

void loop() {
  // Read the input signals (joystick or potentiometers)
  semnal_mansa_ruliu = analogRead(26); // Joystick input for roll
  semnal_mansa_tangaj = analogRead(27); // Joystick input for pitch
  semnal_palonier = analogRead(28); // Joystick input for yaw

  // FLAPERON DREAPTA (right)
  actuator_flaperon_R = map(semnal_mansa_ruliu, 0, 1023, -21, 23);
  stepper_FLAPERON_R.move(actuator_flaperon_R - previous_flaperon_R);
  previous_flaperon_R = actuator_flaperon_R;

  // FLAPERON STANGA (left)
  actuator_flaperon_L = map(semnal_mansa_ruliu, 0, 1023, 23, -21);
  stepper_FLAPERON_L.move(actuator_flaperon_L - previous_flaperon_L);
  previous_flaperon_L = actuator_flaperon_L;

  // STABILATOR DREAPTA (right)
  actuator_stabilator_R = map(semnal_mansa_tangaj, 0, 1023, -21, 21);
  stepper_STABILATOR_R.move(actuator_stabilator_R - previous_stabilator_R);
  previous_stabilator_R = actuator_stabilator_R;

  // STABILATOR STANGA (left)
  actuator_stabilator_L = map(semnal_mansa_tangaj, 0, 1023, -21, 21);
  stepper_STABILATOR_L.move(actuator_stabilator_L - previous_stabilator_L);
  previous_stabilator_L = actuator_stabilator_L;

  // DIRECTIE (yaw)
  actuator_directie = map(semnal_palonier, 0, 1023, 30, -30);
  stepper_DIRECTIE.move(actuator_directie - previous_directie);
  previous_directie = actuator_directie;

  // LEADING EDGE FLAP (if needed)
  // actuator_leading_edge_flap = map(semnal_mansa_ruliu, 0, 1023, 2, -25);
  // stepper_LEADING_EDGE.move(actuator_leading_edge_flap - previous_leading_edge_flap);
  // previous_leading_edge_flap = actuator_leading_edge_flap;
}