// 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();

  /*Serial.println("The calculated values are:");
  print_x_values();
  print_y_values();*/



  //Find the angles that result in de x and y closest to the given (x,y).
  Serial.print("Looking for a minimal distance (first time) ...");
  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);



  //Find the angles again. (A second time.)
  Serial.print("Looking for a minimal distance (second time) ... ");
  x += 0.02;
  y += 0.02;

  start_timer();
  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);


  
  //Find the angles again. (A thirs time, more efficient.)
  Serial.print("Looking for a minimal distance (third time, more efficient) ... ");
  x -= 0.01;
  y -= 0.01;
  int center_a = a;
  int center_b = b;
  int r = 7;

  start_timer();
  d = update_angles(x, y, center_a, center_b, r);
  Serial.println(" [DONE]");
  end_timer();
  
  //Report the result.
  Serial.printf("Found the point (%f, %f) at index (%i, %i), starting from index (%i, %i) with radius %i. The error is %f.\n\n", x, y, a, b, center_a, center_b, r, d);
}

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 = sqrt( pow(x-x_value[0][0],2) + pow(y-y_value[0][0],2) );
  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);
}

//************** 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]);
  }
}