#include <Arduino.h>

#include "A4988.h"

#include <Tlv493d.h>

 

 

// Magnetometer-related initialization

#define B_LOW_THRESHOLD 100.0

#define B_HIGH_THRESHOLD 450.0

// Tlv493d Object

 Tlv493d Tlv493dMagnetic3DSensor = Tlv493d();

 

// Motor-related initialization

#define STEPS_PER_REV 200

#define MICROSTEPS 1

 

#define ENA_X 7

#define STEP_X 8

#define DIR_X 9

#define ENA_Y 10

#define STEP_Y 11

#define DIR_Y 12

#define LINEAR_ACCEL 100 // mm/s^2

#define LINEAR_VEL 100   // mm/s

 

#define DOMAIN_X_MIN 70

#define DOMAIN_X_MAX 150

 

#define DOMAIN_Y_MIN 80

#define DOMAIN_Y_MAX 160

 

#define OFFSET_X 20

#define OFFSET_Y 32

 

#define MACHINE_X_MIN 0

#define MACHINE_X_MAX 180

 

#define MACHINE_Y_MIN 0

#define MACHINE_Y_MAX 180

 

#define MM_PER_REV 40

#define STEPS_PER_MM 5

 

//Home Sensor Definitions

#define SNS_FLAGGED_STATE HIGH

#define X_HOM_PIN 2

#define Y_HOM_PIN 3

#define HOME_SPEED 10 // mm/s

 

bool X_HOMED = false;

bool Y_HOMED = false;

 

// cutting params

int cmove = 0;

const int cmove_max=100;

 

float x_current;

float y_current;

 

//this was long before?

float x_next;

float y_next;

 

const int samples_x = 16;

const int samples_y = 16;

 

int gridXcurrent = 0;

int gridYcurrent = 0;

 

int gridXnext = 0;

int gridYnext = 0;

 

float B[samples_x+1][samples_y+1];

float B_x_ij = 0;

float B_y_ij = 0;

float B_z_ij = 0;

short threshold_array[samples_x+1][samples_y+1];

short visited_array[samples_x+1][samples_y+1];

 

A4988 stepper_x(STEPS_PER_REV, DIR_X, STEP_X, ENA_X);

A4988 stepper_y(STEPS_PER_REV, DIR_Y, STEP_Y, ENA_Y);

 

// Motor Related Functions

 

void homeAxes(){

  stepper_x.enable();

  stepper_y.enable();

  stepper_x.setSpeedProfile(stepper_x.CONSTANT_SPEED);

  stepper_y.setSpeedProfile(stepper_y.CONSTANT_SPEED);

  stepper_x.startMove(-100000);

  stepper_y.startMove(-100000);

  while (digitalRead(X_HOM_PIN) != SNS_FLAGGED_STATE){

    stepper_x.nextAction();

  }

  stepper_x.stop();

 

  while (digitalRead(Y_HOM_PIN) != SNS_FLAGGED_STATE){

    stepper_y.nextAction();

  }

  stepper_y.stop();

 

 

  x_current=0;

  y_current=0;

}

 

void fakeHome(){

  stepper_x.enable();

  stepper_y.enable();

  x_current=0;

  y_current=0;

}

 

void move2axis(float mm_x, float mm_y){

  stepper_x.startMove((int)(mm_x*STEPS_PER_MM));

  stepper_y.startMove((int)(mm_y*STEPS_PER_MM));

  while (stepper_x.nextAction() || stepper_y.nextAction());

  delay(50);

  stepper_x.stop();

  stepper_y.stop();

}

 

void move2axisAbsolute(float x_tgt, float y_tgt){

  float dx=x_tgt-x_current;

  float dy=y_tgt-y_current;

  if (x_tgt <= MACHINE_X_MAX && y_tgt <= MACHINE_Y_MAX){

    move2axis(dx,dy);

    x_current=x_tgt;

    y_current=y_tgt;

  }

  else {

    SerialUSB.print("ERROR: Attempt to move to X=");

    SerialUSB.print(x_tgt);

    SerialUSB.print("Y=");

    SerialUSB.print(y_tgt);

  }

}

 

//Magnetometer related functions

 

int getMagnetometerReading(int readDelay){

    Tlv493dMagnetic3DSensor.updateData();

    delay(readDelay);

    B_x_ij=(float)Tlv493dMagnetic3DSensor.getX();

    B_y_ij=(float)Tlv493dMagnetic3DSensor.getY();

    B_z_ij=(float)Tlv493dMagnetic3DSensor.getZ();

//B_x_ij=1.23;

//B_y_ij=4.56;

//B_z_ij=7.89;

}

 

// Actuator stuff

int actuatorA = 2;

int actuatorB = 3;

int actuatorDelay = 10000;

 

void actuatorExtend(int delayTime){

  digitalWrite(actuatorA,LOW);

  digitalWrite(actuatorB,HIGH);

  delay(delayTime);

  digitalWrite(actuatorA,HIGH);

  digitalWrite(actuatorB,HIGH);

}

 

void actuatorRetract(int delayTime){

  digitalWrite(actuatorA,HIGH);

  digitalWrite(actuatorB,LOW);

  delay(delayTime);

  digitalWrite(actuatorA,HIGH);

  digitalWrite(actuatorB,HIGH);

}

 

// Higher-level functions

int CentroidX;

int CentroidY;

 

bool CCW = true;

bool thshld_nbrhood[3][3];

bool visited_nbrhood[3][3];

 

void fill_nbrhoods(int gridX, int gridY){

  for (int i = gridX - 1; i <= gridX + 1; i++){

    for (int j = gridY - 1; j <= gridY + 1; j++){

      thshld_nbrhood[i-gridX+1][j-gridY+1] = threshold_array[i][j];

      visited_nbrhood[i-gridX+1][j-gridY+1] = visited_array[i][j];

    }

  }

}

 

void print_nbrhood(){

  SerialUSB.print(thshld_nbrhood[0][0]);

  SerialUSB.print(",");

  SerialUSB.print(thshld_nbrhood[0][1]);

  SerialUSB.print(",");

  SerialUSB.print(thshld_nbrhood[0][2]);

  SerialUSB.print(",\t");

 

  SerialUSB.print(visited_nbrhood[0][0]);

  SerialUSB.print(",");

  SerialUSB.print(visited_nbrhood[0][1]);

  SerialUSB.print(",");

  SerialUSB.print(visited_nbrhood[0][2]);

  SerialUSB.println("");

 

  SerialUSB.print(thshld_nbrhood[1][0]);

  SerialUSB.print(",");

  SerialUSB.print(thshld_nbrhood[1][1]);

  SerialUSB.print(",");

  SerialUSB.print(thshld_nbrhood[1][2]);

  SerialUSB.print(",\t");

 

  SerialUSB.print(visited_nbrhood[1][0]);

  SerialUSB.print(",");

  SerialUSB.print(visited_nbrhood[1][1]);

  SerialUSB.print(",");

  SerialUSB.print(visited_nbrhood[1][2]);

  SerialUSB.println("");

 

  SerialUSB.print(thshld_nbrhood[2][0]);

  SerialUSB.print(",");

  SerialUSB.print(thshld_nbrhood[2][1]);

  SerialUSB.print(",");

  SerialUSB.print(thshld_nbrhood[2][2]);

  SerialUSB.print(",\t");

 

  SerialUSB.print(visited_nbrhood[2][0]);

  SerialUSB.print(",");

  SerialUSB.print(visited_nbrhood[2][1]);

  SerialUSB.print(",");

  SerialUSB.print(visited_nbrhood[2][2]);

  SerialUSB.println("");

}

 

void promptToSetHome(){

  SerialUSB.println(F("\nSend any character to set current position as zero: "));

    while (SerialUSB.available() && SerialUSB.read()); // empty buffer

    while (!SerialUSB.available());                 // wait for data

    while (SerialUSB.available() && SerialUSB.read()); // empty buffer again

}

 

void promptToScan(){

  SerialUSB.println(F("\nSend any character to begin raster scan: "));

    while (SerialUSB.available() && SerialUSB.read()); // empty buffer

    while (!SerialUSB.available());                 // wait for data

    while (SerialUSB.available() && SerialUSB.read()); // empty buffer again

}

 

void promptToBeginCut(){

  SerialUSB.print("Centroid: ");

  SerialUSB.print(CentroidX);

  SerialUSB.println(CentroidY);

  SerialUSB.print("Starting Point: ");

  SerialUSB.print(gridXnext);

  SerialUSB.println(gridYnext);

  SerialUSB.println(F("\nSend any character to begin cut: "));

  while (SerialUSB.available() && SerialUSB.read()); // empty buffer

  while (!SerialUSB.available());                 // wait for data

  while (SerialUSB.available() && SerialUSB.read()); // empty buffer again

}

 

void init_arrays(){

  for (int i = 0; i <= samples_x; i++){

    for (int j = 0; j <= samples_x; j++){

       threshold_array[i][j]=0;

       visited_array[i][j]=0;

    }

  }

}

 

void mark_thshld_array(float B,int i, int j){

  if (B >= B_LOW_THRESHOLD && B <= B_HIGH_THRESHOLD){

    threshold_array[i][j] = 1;

  }

}

 

void scanDomain(int readDelay){

  float delx = DOMAIN_X_MAX - DOMAIN_X_MIN;

  float dely = DOMAIN_Y_MAX - DOMAIN_Y_MIN;

  SerialUSB.print("x range: ");

  SerialUSB.print(delx);

  SerialUSB.print(" y range: ");

  SerialUSB.println(dely);

 

  for (int i = 0; i <= samples_x; i++){

    SerialUSB.print("x row: ");

    SerialUSB.print(i);

    SerialUSB.print(" x pos: ");

    SerialUSB.println(DOMAIN_X_MIN + i*(delx/samples_x));

    for (int j = 0; j <= samples_y; j++){

      SerialUSB.print("y column: ");

      SerialUSB.print(j);

      SerialUSB.print(" y pos: ");

      SerialUSB.print(DOMAIN_Y_MIN + j*(dely/samples_y));

      move2axisAbsolute(DOMAIN_X_MIN + i*(delx/samples_x), DOMAIN_Y_MIN + j*(dely/samples_y));

      getMagnetometerReading(readDelay);

      B[i][j] = B_x_ij*B_x_ij + B_y_ij*B_y_ij + B_z_ij*B_z_ij;

      SerialUSB.print(" Bx ");

      SerialUSB.print(B_x_ij);

      SerialUSB.print(" By ");

      SerialUSB.print(B_y_ij);

      SerialUSB.print(" Bz ");

      SerialUSB.print(B_z_ij);

      SerialUSB.print(" ||B|| ");

      SerialUSB.println(B[i][j]);

      // THIS GUY RIGHT HERE HES FUCKING IT UP

      //threshold_array[i][j]=(B[i][j] > B_LOW_THRESHOLD && B[i][j] < B_HIGH_THRESHOLD);

      mark_thshld_array(B_x_ij*B_x_ij + B_y_ij*B_y_ij + B_z_ij*B_z_ij,i,j);

    }

  }

}

 

void writeDataToSerial(){

  SerialUSB.println("DATA:");

  for (int i = 0; i <= samples_x; i++){

    for (int j = 0; j <= samples_y; j++){

      SerialUSB.print(B[i][j]);

      if (j!=samples_y){

       SerialUSB.print(",");

      }

    }

    SerialUSB.print("\n");

  }

  SerialUSB.println(":DATA");

  SerialUSB.println("POINTS:");

  for (int i = 0; i <= samples_x; i++){

    for (int j = 0; j <= samples_y; j++){

      SerialUSB.print(threshold_array[i][j]);

      if (j!=samples_y){

       SerialUSB.print(",");

      }

    }

    SerialUSB.print("\n");

  }

  SerialUSB.println(":POINTS");

}

 

void calcCentroid(){

  int xsum = 0;

  int ysum = 0;

  int total = 0;

  for (int i = 1; i <= samples_x; i++){

    for (int j = 1; j <= samples_y; j++){

      if (threshold_array[i][j]){

        xsum = xsum + i;

        ysum = ysum + j;

        total++;

      }

    }

    CentroidX=xsum/total;

    CentroidY=ysum/total;

  }

}

 

//this is dumb but I don't wanna think of a better way - call calcCentroid before findFirstPoint

void findFirstPoint(){

  bool done=false;

    for (int i = 0; i <= samples_x; i++){

      for (int j = 0; j <= samples_y; j++){

        if (threshold_array[i][j]==1 && !done){

          gridXnext=i;

          gridYnext=j;

          done=true;

        }

      }

    }

}

 

void goToGridPoint(){

  float delx = DOMAIN_X_MAX - DOMAIN_X_MIN;

  float dely = DOMAIN_Y_MAX - DOMAIN_Y_MIN;

  SerialUSB.print("GridCurrent: ");

  SerialUSB.print(gridXcurrent);

  SerialUSB.print(", ");

  SerialUSB.print(gridYcurrent);

  SerialUSB.print(" GridNext: ");

  SerialUSB.print(gridXnext);

  SerialUSB.print(", ");

  SerialUSB.println(gridYnext);

  if (gridXnext != gridXcurrent || gridYnext != gridYcurrent){

    move2axisAbsolute(OFFSET_X + DOMAIN_X_MIN + gridXnext*(delx/samples_x), OFFSET_Y + DOMAIN_Y_MIN + gridYnext*(dely/samples_y));

    //delay(500);

    visited_array[gridXnext][gridYnext] = true;

    gridXcurrent=gridXnext;

    gridYcurrent=gridYnext;

  }

}

 

void fakeGoToGridPoint(){

  float delx = DOMAIN_X_MAX - DOMAIN_X_MIN;

  float dely = DOMAIN_Y_MAX - DOMAIN_Y_MIN;

  SerialUSB.print("GridCurrent: ");

  SerialUSB.print(gridXcurrent);

  SerialUSB.print(", ");

  SerialUSB.print(gridYcurrent);

  SerialUSB.print(" GridNext: ");

  SerialUSB.print(gridXnext);

  SerialUSB.print(", ");

  SerialUSB.println(gridYnext);

  if (gridXnext != gridXcurrent || gridYnext != gridYcurrent){

    //move2axisAbsolute(DOMAIN_X_MIN + gridXnext*(delx/samples_x), DOMAIN_Y_MIN + gridYnext*(dely/samples_y));

    delay(500);

    visited_array[gridXnext][gridYnext] = true;

    gridXcurrent=gridXnext;

    gridYcurrent=gridYnext;

  }

}

 

float cross(int rx, int ry, int vx, int vy){

  if (abs(vx)+abs(vy) < 2){

    return (float)(rx*vy-ry*vx);

  }

  // if vector to next point does not have length 1, normalize it

  else{

    vx=vx*0.7071067811865475;

    vy=vy*0.7071067811865475;

    return (float)(rx*vy-ry*vx);

  }

}

 

bool endOfPath = false;

 

void findNextPoint(){

  fill_nbrhoods(gridXcurrent, gridYcurrent);

  print_nbrhood();

  int rx=gridXcurrent-CentroidX;

  int ry=gridYcurrent-CentroidY;

  int c=-999;

  int c_last=-999;

  int ibest=1;

  int jbest=1;

  for (int i=0; i<=2; i++){

    for (int j=0; j<=2; j++){

      if( i != j && thshld_nbrhood[i][j]==1 && visited_nbrhood[i][j]==0 ){

        c=cross(rx,ry,i-1,j-1);

        SerialUSB.print("cross product score for direction ");

        SerialUSB.print(i-1);

        SerialUSB.print(",");

        SerialUSB.print(j-1);

        SerialUSB.print(" = ");

        SerialUSB.println(c);

        if (c > c_last){

          ibest=i;

          jbest=j;

          c_last = c;

        }

      }

    }

  }

  //FIX THIS -nvm i think its ok

  if (ibest == 1 && jbest == 1){

    endOfPath=true;

  }

  else{

    gridXnext=gridXcurrent+ibest-1;

    gridYnext=gridYcurrent+jbest-1;

    SerialUSB.print("Optimal direction: ");

    SerialUSB.print(ibest-1);

    SerialUSB.print(", ");

    SerialUSB.print(jbest-1);

    SerialUSB.print(" with cross product score = ");

    SerialUSB.println(c);

  }

}

 

void setup() {

  SerialUSB.begin(115200);

  while (!SerialUSB) ; // Wait for Serial monitor to open

  Tlv493dMagnetic3DSensor.begin();

  init_arrays();

  pinMode(actuatorA, OUTPUT);

  pinMode(actuatorB, OUTPUT); 

  stepper_x.begin(HOME_SPEED*60/MM_PER_REV, MICROSTEPS);

  stepper_y.begin(HOME_SPEED*60/MM_PER_REV, MICROSTEPS);

  stepper_x.disable();

  stepper_y.disable();

  actuatorRetract(actuatorDelay);

  //homeAxes();

  promptToSetHome();

  fakeHome();

  stepper_x.setRPM(LINEAR_VEL*60/MM_PER_REV);

  stepper_y.setRPM(LINEAR_VEL*60/MM_PER_REV);

 

  stepper_x.setSpeedProfile(stepper_x.LINEAR_SPEED,STEPS_PER_REV*LINEAR_ACCEL/MM_PER_REV,STEPS_PER_REV*LINEAR_ACCEL/MM_PER_REV);

  stepper_y.setSpeedProfile(stepper_y.LINEAR_SPEED,STEPS_PER_REV*LINEAR_ACCEL/MM_PER_REV,STEPS_PER_REV*LINEAR_ACCEL/MM_PER_REV);

 

  promptToScan();

  scanDomain(100);

 

  writeDataToSerial();

  calcCentroid();

  findFirstPoint();

  promptToBeginCut();

  goToGridPoint();

  actuatorExtend(actuatorDelay);

  while (!endOfPath && cmove<cmove_max){

    findNextPoint();

    goToGridPoint();

    cmove++; 

  }

  actuatorRetract(actuatorDelay);

  SerialUSB.println("End of path");

  SerialUSB.println(cmove);

  stepper_x.disable();

  stepper_y.disable();

}

 

void loop() {

//  SerialUSB.println("-->0,20");

//  move2axisAbsolute(0,20);

//  SerialUSB.println("@0,20");

//  delay(2000);

//  SerialUSB.println("-->20,20");

//  move2axisAbsolute(20,20);

//  SerialUSB.println("@20,20");

//  delay(2000);

//  SerialUSB.println("-->0,0");

//  move2axisAbsolute(0,0);

//  SerialUSB.println("@0,0");

//  delay(2000);

 

//  SerialUSB.println("-->10,20");

//  move2axisAbsolute(10,20);

//  SerialUSB.println("@10,20");

//  delay(5000);

//  SerialUSB.println("-->10,0");

//  move2axisAbsolute(10,0);

//  SerialUSB.println("@10,0");

//  delay(5000);

//  SerialUSB.println("-->0,0");

//  move2axisAbsolute(0,0);

//  SerialUSB.println("@0,0");

  delay(5000);

}