#include <math.h>

//defining pins at the begging so they are easier to change
const int MOTOR_PIN_1 = 7;
const int MOTOR_PIN_2 = 6;
const int DIR_PIN_1 = 5;
const int DIR_PIN_2 = 4;
const int potX = A0;
const int potY = A1;

//defines the set up for motor channels
struct channel {
    int pin;   // Pin number
    int stepRate;  // Frequency (analog pin for potentiometer)
    int disp; //total displacment from target
    int index; // Index for timing
    int dir;  //Stepper Direction
    int stepCount; // total number of steps per channel
};

//initializes the motor channels
channel channels[] = {
    {MOTOR_PIN_1, 0, 0, 0, DIR_PIN_1, 0}, // Channel 1
    {MOTOR_PIN_2, 0, 0, 0, DIR_PIN_2, 0}  // Channel 2
};

void StepperRunner(channel &chan);
int* getCurrentPos();
void target(int target[]);
int* getTarget();

void setup() {
    pinMode(MOTOR_PIN_1, OUTPUT);
    pinMode(MOTOR_PIN_2, OUTPUT);
    pinMode(DIR_PIN_1, OUTPUT);
    pinMode(DIR_PIN_2, OUTPUT);
    Serial.begin(9600);
}

void loop() {

  //runs StepperRunner for each channel
  for (int i = 0; i < sizeof(channels) / sizeof(channels[0]); i++) {
    StepperRunner(channels[i]);
  }

  //updates the target coordinates
  target(getTarget());

  /*
  Serial.print("  ");
  Serial.print("Step Count: (");
  Serial.print(channels[0].stepCount);
  Serial.print(",");
  Serial.print(channels[1].stepCount);
  Serial.print(") ");
  //*/
}

//StepperRunner runs the motor channels
void StepperRunner(channel &chan) {
    int maxsps = 5000; //max steps per second
    float milliperstep = abs(1000/(chan.stepRate*maxsps));

    if (milliperstep >= chan.index) {// Check if the pulse is above the threshold
        digitalWrite(chan.pin, !digitalRead(chan.pin)); //toggle chan.pin
        chan.index = 0; // Reset index for this channel
    
        if(chan.disp < 0){
          digitalWrite(chan.dir, HIGH);
          chan.stepCount++;
        }
        else if(chan.disp > 0){
          digitalWrite(chan.dir, LOW);
          chan.stepCount--;
        }
    
      chan.index++; // Increment index for this channel
    }
}

//gets position of motors in steps
int* getCurrentPos() {
  static int coords[2];
    coords[0] = channels[0].stepCount;
    coords[1] = channels[1].stepCount;
    return coords;
}

//the target function updates the displacment and direction of the channels
void target(int target[]){

  //gets motor coordinates
  int* currentPos = getCurrentPos();

  // initialize stepRate variables 
  float stepRateY;
  float stepRateX;

  //get x and y displacment to target
  float dispY = currentPos[1] - target[1];
  float dispX = currentPos[0] - target[0];

  //get absolut distance to target
  float dispTotal = sqrt((pow(dispY,2))+(pow(dispX,2)));

  //checks we're not at target
  if(dispTotal < 1 && dispTotal >-1){

    //set motor speeds based on displacment to target divided by total displacment
    stepRateY = (dispY/dispTotal);
    stepRateX = (dispX/dispTotal);
  }
  else{
    //sets step rate to 0 if we're at target
    stepRateY = 0;
    stepRateX = 0;

  }

  //updates the step rate and displacment for the x and y channels
  channels[0].stepRate = stepRateX;
  channels[1].stepRate = stepRateY;
  channels[0].disp = dispX;
  channels[1].disp = dispY;
/*
  Serial.print("stepRate :(");
  Serial.print(stepRateX);
  Serial.print(",");
  Serial.print(stepRateY);
  Serial.print(") Displacment :(");
  Serial.print(dispX);
  Serial.print(",");
  Serial.print(dispY);
  Serial.print(") Total Displacment:");
  Serial.println(dispTotal);
//*/
     
}

//the getTarget function returns target coordinates and can be modified for diffrent user inputs
int* getTarget(){

  //maps pot values to coordinates
  int mapX = map(analogRead(potX), 0, 1023, -1000, 1000);
  int mapY = map(analogRead(potY), 0, 1023, -1000, 1000);

  //saves coordinates to array (x,y)
  static int coords[2];
    coords[0] = 1200;
    coords[1] = 5;

    return coords;
}