#ifndef _4AXISROBOTARM_H_ ///If not defined, define library
#define _4AXISROBOTARM_H_

#include <Arduino.h>
#include <math.h> //Math Self Explanatory
#include <AccelStepper.h> //Stepper Motor Library
#include <MultiStepper.h> //Stepper Motor Library

/// @brief Stores Stepper Information
/// Struct used to define a stepper motor with all it's parameters and automatically caluculate DegPerStep when necessary.
struct stepper {

  byte stepPin;
  byte dirPin;
  byte enPin;
  byte endSwitchPin;
  double LowRange; /// Lower Angular range of the motor
  double HighRange; /// Higher range of the motor
  double MotorStepsRange; /// Max number of steps the motor can can do in the range
  double DegPerStep; /// Stores the number degrees a single step is equivlent to

  //Constructor to Handle auto calculation of DegPerStep
  stepper(byte stepPin, byte dirPin, byte enPin, byte endSwitchPin, double LowRange, double HighRange, long MotorStepsRange);
  
  //Constructor to Handle manual input of DegPerStep
  stepper(byte stepPin, byte dirPin, byte enPin, byte endSwitchPin, double LowRange, double HighRange, double DegPerStep, long MotorStepsRange = -1);

  

};

double doubleMap(double x, double in_min, double in_max, double out_min, double out_max);

class RobotArm {
  public: 
    
    RobotArm(stepper stepper1, 
             stepper stepper2, 
             stepper stepper3, 
             stepper stepper4,
             int ArmLength1,
             int ArmLength2,
             int ArmLength3,
             bool Safety);
    /// @brief Function Should be ran as often as possible for predictable RobotMotion             
    void RobotArmRun();
    /// @brief Homes Robot Arm
    /// @param HomeMot1 if True homes the first Stepper Motor
    /// @param HomeMot2 if True homes the second Stepper Motor
    /// @param HomeMot3 if True homes the third Stepper Motor
    /// @param HomeMot4 if True homes the fourth Stepper Motor
    void Home(bool HomeMot1 = true, bool HomeMot2 = true, bool HomeMot3 = true, bool HomeMot4 = true);
    
    /// @brief Moves Robot Arm to a specific coordinate with a specific End Orrientation
    /// @param x X Coordinate in Euclidean Space
    /// @param y Y Coordinate in Eculedean Space
    /// @param z Z Coordinate in Eculedean Space
    /// @param Orio End Effector Orienation of the Robot Arm
    void MoveRobotArmToPosition(double x, double y, double z, double Orio);

    /// @brief Handles the position of joint Angles (Moves joints)
    /// @param ZOrio Angular Base Orientation in Z Axis
    /// @param Joint1Angle Angle of the first joint. Note. Joints are considered from bottom to top
    /// @param Joint2Angle Angle of the second joint. Note. Joints are considered from bottom to top
    /// @param Joint3Angle Angle of the third joint. Note. Joints are considered from bottom to top
    void MoveRobotArmJoints(double ZOrio, double Joint1Angle, double Joint2Angle, double Joint3Angle);

    /// @brief returns the time left to finish current motion
    double getMoveDuration();

    void pause(unsigned long time);

    /// @brief Exposes private steppers object isStillRunning Publicly for the robotArm Object
    bool IsRobotMoving();


    

  private:
    //Intialising RobotArmClass Variables
      stepper stepper1; /// Holds the data for the first stepper
      stepper stepper2; /// Holds the data for the second stepper
      stepper stepper3; /// Holds the data for the third stepper
      stepper stepper4; /// Holds the data for the fourth stepper
      int ArmLength1; /// Holds the length of the first Arm in mm
      int ArmLength2; /// Holds the length of the second Arm in mm
      int ArmLength3; /// Holds the length of the third Arm in mm
      bool Safety; /// Whether the Robot should Home when an end switch is hit 
      bool Pause; /// Pause state tracker
      unsigned long time_pause_since_call; // Stores the time of the pause call
      unsigned long pause_time; // Stores the pause time passed by the pause call
      
    
    //Declaring AccelStepper MotorObjects
        AccelStepper Motor1;
        AccelStepper Motor2;
        AccelStepper Motor3;
        AccelStepper Motor4;
    //Declaring MultiStepper Object
        MultiStepper steppers;
  /// @brief Stores Robot Arm Angles
    struct RobotAngles{
      double theta0;
      double theta1;
      double theta2;
      double theta3;
  };
  /// @brief Converts X Y Z, and End Effector Angle into Robot Arm Angles 
  RobotAngles InverseKinematics3D(float x, float y, float z, float Orio);
  
  /// @brief Stops the robot arm when an end switch is reached.
  void SafetyStop();

  /// @brief HomeMotor
  
  public:
    RobotAngles Angles; /// The angles stored when kinematic function is used



};

#endif