#include <Stepper.h>
#include <Keypad.h>

/* all units used in this project are in meters */

const int steps_per_revolution = 200;
const float wheel_radius = 0.03/*meters*/;
const float meters_per_step = (2 * PI * wheel_radius)/200;

const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

unsigned char colPins[COLS] = { 31, 33, 35, 37 }; // Pins connected to C1, C2, C3, C4
unsigned char rowPins[ROWS] = { 23, 25, 27, 29 }; // Pins connected to R1, R2, R3, R4

Stepper right_stepper(steps_per_revolution, 2, 3, 4, 5);
Stepper left_stepper(steps_per_revolution, 6, 7, 8, 9);

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);


void turn_left() {
  Serial.println("Turning left");
  for (int i = 0; i < (steps_per_revolution/4); i++) {
    left_stepper.step(1);
    right_stepper.step(1);
  }
}

void turn_right() {
  Serial.println("Turning right");
  for (int i = 0; i < (steps_per_revolution/4); i++) {
    left_stepper.step(-1);
    right_stepper.step(-1);
  }
}

void drive_forward(float distance_in_meters) {
  Serial.print("Driving ");
  Serial.print(distance_in_meters);
  Serial.println(" meters");
  for (float i = 0.0f; i < distance_in_meters; i += meters_per_step) {
    int duration = 0;
    do {
      digitalWrite(39, HIGH);
      delayMicroseconds(10);
      digitalWrite(39, LOW);
      duration = pulseIn(41, HIGH) / 58;
    } while (duration <= 100);
    left_stepper.step(-1);
    right_stepper.step(1);
  }
}

// regarding restaurant layout

int kitchen_dist_from_rect = 5.0/*meters*/;
unsigned int tables_per_row = 10;
unsigned int tables_per_column = 10;
float table_row_size = 10.0/*meters*/;
float table_col_size = 15.0/*meters*/;
unsigned int number_of_tables = tables_per_row * tables_per_column;

void setup() {
  Serial.begin(9600);
  pinMode(39, OUTPUT);
  pinMode(41, INPUT);
}

void loop() {
  /* input table number */
  int table_number = 0;
  int input = 0;
  while ((input = keypad.getKey()) != '#') {
    if (input != NO_KEY) {
      if (isDigit(input)) {
        table_number *= 10;
        table_number += (input - '0');
      }
    }
  }
  Serial.print("Driving to Table #");
  Serial.println(table_number);
  if (1 <= table_number && table_number <= number_of_tables) {
    int goto_row = (table_number / tables_per_row) + 1;
    int goto_col = table_number % tables_per_row;
    /* drive from kitchen to grid */
    drive_forward(kitchen_dist_from_rect);
    /* drive to specific row */
    drive_forward(((table_col_size/tables_per_column) * goto_col));
    turn_right();
    drive_forward((table_row_size/tables_per_row) * goto_row);
    turn_left();
    Serial.println("Arrived at the table");
    Serial.println("Press # to return to the kitchen");
    while ((input = keypad.getKey()) != '#');
    Serial.println("Returning to the kitchen");
    turn_left();
    drive_forward((table_row_size/tables_per_row) * goto_row);
    turn_left;
    drive_forward(((table_col_size/tables_per_column) * goto_col));
    drive_forward(kitchen_dist_from_rect);
    turn_right();
    turn_right();
    Serial.println("Arrived at the kitchen");
  }
}