// C++ code
//
//const float PI = 3.14159265359; --> is al gedefinieerd
const int A = 91;     //Maximum angles (from 0 to 91°).
const int B = 91;
const float L1 = 3.0; //Length of the robot arms.
const float L2 = 4.0;
float x_value[A][B];  //Lookup tables (matrix of size A x B).
float y_value[A][B];
int a, b;             //Global variables used to return the found angles of find_angles().
unsigned long start_time; //Global variable for the start_timer() and end_timer() functions.

void setup()
{
  //Open the serial port.
  Serial.begin(115200);

  //Prefill the lookup tables.
  Serial.print("\nFilling lookup tables...");
  start_timer();
  for(int a=0; a<A; a++){
    for(int b=0; b<B; b++){
      x_value[a][b] = L1*cosd(a) + L2*cosd(a+b);
      y_value[a][b] = L1*sind(a) + L2*sind(a+b);
    }
  }
  Serial.println(" [DONE]");
  end_timer();  Serial.println();



  //Find the angles that result in de x and y closest to the given (x,y).
  Serial.println("------- Testing find_angles() -------");
  Serial.print("Looking for a minimal distance ...");
  float x = 3;
  float y = 6.2;

  start_timer();
  float d = find_angles(x, y);
  Serial.println(" [DONE]");
  end_timer();
  
  //Report the result.
  Serial.printf("Found the point (%f, %f) at index (%i, %i). The error is %f.\n\n", x, y, a, b, d);



  Serial.println("------- Testing go_from_point_to_point() -------");
  //go_from_point_to_point(3.0, 6.0, 3.0, 7.0, 0.1, 1000);
  //go_from_point_to_point(3.0, 6.0, 4.0, 7.0, 20, 1000);
  go_from_point_to_point(1.0, 6.0, 2.0, 5.0, 20, 1000);
}

void loop()
{/*
  Serial.println("Hello, ESP32!");
  delay(5000);*/
}

//************** Function to find a point in the LU-table **************
float find_angles(float x, float y) {
  //Initialise distance for the coordinate at (0, 0).
  float d = pow(x-x_value[0][0],2) + pow(y-y_value[0][0],2);

  //Loop over the whole matrix.
  for(int i=0; i<A; i++) {
    for(int j=0; j<B; j++){
      //Calculate the distance.
      //float distance = sqrt( pow(x-x_value[i][j],2) + pow(y-y_value[i][j],2) );
      //float distance = pow(x-x_value[i][j],2) + pow(y-y_value[i][j],2);
      float distance = (x-x_value[i][j])*(x-x_value[i][j]) + (y-y_value[i][j])*(y-y_value[i][j]);
      //If this distance is smaller: remember this angle pair.
      if(distance < d) {
        a = i;
        b = j;
        d = distance;
      }
    }
  }

  return sqrt(d);
}

//************** Second function to find a point in the LU-table (with given center and radius) **************
float update_angles(float x, float y, int center_a, int center_b, int r) {
  //Initialise distance for the coordinate at (center_a, center_b).
  float d = pow(x-x_value[center_a][center_b],2) + pow(y-y_value[center_a][center_b],2);

  //Set the bounds so that we stay within the bounds of the matrix.
  int min_i = max(0, center_a-r);
  int min_j = max(0, center_b-r);
  int max_i = min(center_a+r, A);
  int max_j = min(center_b+r, B);

  //Loop over PART OF matrix.
  for(int i=min_i; i<max_i; i++) {
    for(int j=min_j; j<max_j; j++){
      //Calculate the distance.
      //float distance = pow(x-x_value[i][j],2) + pow(y-y_value[i][j],2);
      float distance = (x-x_value[i][j])*(x-x_value[i][j]) + (y-y_value[i][j])*(y-y_value[i][j]);
      //If this distance is smaller: remember this angle pair.
      if(distance < d) {
        a = i;
        b = j;
        d = distance;
      }
    }
  }

  return sqrt(d);
}


void go_from_point_to_point(float x1, float y1, float x2, float y2, unsigned int num_steps, unsigned long duration) {
  float stepsize = 1.0 / num_steps;
  float currentstep = 0.0;

  while(currentstep < 1.0+stepsize/2.0) {
    float x = x2*currentstep + x1*(1.0-currentstep);
    float y = y2*currentstep + y1*(1.0-currentstep);
    float err = find_angles(x, y);  //Result in a and b.
    Serial.printf("x = %f, y = %f, a = %i, b = %i, error = %f\n", x, y, a, b, err);
    //Serial.printf("Found the point (%f, %f) at index (%i, %i). The error is %f.\n\n", x, y, a, b, err);

    currentstep += stepsize;
    delay(duration / num_steps);
  }
}
/*
void go_from_point_to_point(float x1, float y1, float x2, float y2, float stepsize, unsigned long steptime) {
  float currentstep = 0.0;
  while(currentstep < 1.0+stepsize/2.0) {
    float x = x2*currentstep + x1*(1.0-currentstep);
    float y = y2*currentstep + y1*(1.0-currentstep);
    float err = find_angles(x, y);  //Result in a and b.
    Serial.printf("x = %f, y = %f, a = %f, b = %f, error = %f\n", x, y, a, b, err);

    currentstep += stepsize;
    delay(steptime);
  }
}*/

//************** Helper functions **************
void start_timer(){
  //Save the start time.
  start_time = micros();
}
void end_timer(){
  //Compare the time now to the start time and print the result.
  unsigned long end_time = micros();
  float milli_seconds = (end_time - start_time)/1000.0;
  //Serial.printf("Time elapsed: %ims.\n", end_time - start_time);
  Serial.printf("Time elapsed: %.3fms.\n", milli_seconds);
}

float degree_to_radian(float degree){
  return degree * PI / 180;
}
float sind(float a){
  return sin(degree_to_radian(a));
}
float cosd(float a){
  return cos(degree_to_radian(a));
}

void print_x_values(){
  Serial.println("x_values:");

  // Print the matrix.
  for(int a=0; a<A; a++){
    for(int b=0; b<B-1; b++){
      Serial.printf("% 3.2f,", x_value[a][b]);
    }
    Serial.printf("% 3.2f\n", x_value[a][B-1]);
  }
}
void print_y_values(){
  Serial.println("y_values:");

  // Print the matrix.
  for(int a=0; a<A; a++){
    for(int b=0; b<B-1; b++){
      Serial.printf("% 3.2f,", y_value[a][b]);
    }
    Serial.printf("% 3.2f\n", y_value[a][B-1]);
  }
}