#include <Adafruit_MotorShield.h>
#include <Servo.h>

//General
uint8_t regions[1] = {BACKWARD};
uint8_t regions_opposite[1] = {FORWARD}; 
int region = 0;
int button_val = 0;
int button_port = 0;
bool stuck = false;
int amber_LED = 4;
int blink_counter = 0;
unsigned long t = 0;
unsigned long prev_millis = 0;
int GREEN_LED = 5;

//Motors
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motors[] = {AFMS.getMotor(3), AFMS.getMotor(4)};
#define left 0 //index value in motor pointer array
#define right 1

//IR
#define white 1
#define black 0
#define num_IR 4
#define IR_floating_avg_num 3
int IR_threshold[num_IR] = {300, 300, 1024, 1024};
float IR_avgs[num_IR] = {0}; //LRFB
int IR_colours[num_IR] = {0};
int IR_locns[num_IR] = {A0, A3, A1, A2};
float IR_vals[num_IR][IR_floating_avg_num] = {0}; //check
int IR_float_counter[num_IR] = {0};
int cur_status = 0;
long cur_loop_count = 0;

//US
#define echoPin 2
#define trigPin 3
#define US_float_avg_num 1
float duration;
float new_US_distance;
int US_index = 0;
float US_distance_vals[US_float_avg_num] = {0};
float US_distance_avg = 0;

//Metal Detector
int metal_detector = 13;
int metal_float_counter = 0;
#define metal_floating_avg_num 1
float metal_avg = 0;
float metal_vals[metal_floating_avg_num] = {0};
bool is_metal = false;
//int is_metal = 0;
//#define metal_lower_bound 30.0
//#define metal_upper_bound 34.0
//#define non_metal_lower_bound 36.0
//#define non_metal_upper_bound 40.0
#define metal_threshold 9.7
float min_val = 100;


//Servo
#define servo_position 180
Servo servoright;
Servo servoleft;

//Ramp
int ramp_detect_threshold = 500; //Up, Down
int ramp_detect_counter = 0;
bool ramp_on = false;
int left_right_velocity = 200;

//Turning around
int turn_around_time = 2800;
long turn_around_time_start;
long turn_around_time_end;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  if (!AFMS.begin()) {         // create with the default frequency 1.6KHz
  // if (!AFMS.begin(1000)) {  // OR with a different frequency, say 1KHz
    Serial.println("Could not find Motor Shield. Check wiring.");
    while (1);
  }
  for(int j=0; j<2; j++) {
    motors[j]->run(RELEASE);
  }
  for(int i=0; i< num_IR; i++) {
    pinMode(IR_locns[i], INPUT);
  }
  pinMode(button_port, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(amber_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  digitalWrite(GREEN_LED, LOW);
  servoright.attach(10);
  servoleft.attach(9);
  servoright.write(90);
  servoleft.write(90);
}

void loop() {
  // put your main code here, to run repeatedly:
  for(int i=0; i < 2*IR_floating_avg_num; i++) { //Get some values in before evaluation starts
    update_IR_vals();
  }
  for(int j = 0; j<2; j++) {
    motors[j]->run(regions[0]);
  }
  move_both(motors, 150);

  //servo.write(servo_position);
  if (region == 0 || region == 2) {
    Serial.print("We are in region ");
    Serial.println(region);
    prev_millis = millis();
    while (true) {
      int return_code = basic_region(700, false);
      if (return_code) {
        continue;
      }

      //Update region 
      t = millis() - prev_millis;
      if (t > 15000) {
        ramp_on = true;
      }
      if (not ramp_on) {
        continue;
      }
      ramp_detect_counter += 1;
      if (ramp_detect_counter % 3000 == 0) {
        Serial.println("US");
        float US = update_region(ramp_detect_threshold);
        if (US) {
          break;
        }
      }
    }
  }
  else if (region == 1 || region == 3) { //Junction detect region
    Serial.print("We are in region ");
    Serial.println(region);
    while (true) {
      int return_code = basic_region(700, true);
      if (return_code == 1) {
        Serial.println("THIS SHOULD NOT HAPPEN");
        continue;
      }
      else if (return_code == 2) {
        //region += 1;
        Serial.println("IT WORKED");
        break;
      }
    }
    //Moving robot back
    /*
    for(int j = 0; j < 2; j++) {
      motors[j]->run(regions_opposite[0]);
    }
    move_both(motors, 200);
    delay(200);
    move_both(motors, 0);
    for(int j = 0; j < 2; j++) {
      motors[j]->run(regions[0]);
    }
    */

    //Move Servo
    bring_down_servos(65);
    for(int i = 0; i< 10000*metal_floating_avg_num; i++) {
      update_metal_detector();
    }
    //Metal Detect
    for(int i = 0; i< 10000; i++) {
      update_metal_detector();
      if (metal_avg < min_val) {
        min_val = metal_avg;
        Serial.println(min_val);
      }
    }
    if (min_val <= metal_threshold) {
      is_metal = true;
      Serial.println("METAL DETECTED");
    }
    else {
      digitalWrite(GREEN_LED, HIGH);
      Serial.println("NOT DETECTED");
    }

    turn_around(turn_around_time, 255);
    region += 1;
  }
}

bool recovery() {
  if (stuck) {
    Serial.println("RECOVERY CODE");
    move_both(motors, 255);
    ramp_on = true;
    left_right_velocity = 200;
    ramp_detect_counter = 0;
    return true;
  }
  return false;
}

int basic_region(int clock_cycle, bool junction) {
  blink_amber(clock_cycle);
  check_push_button(motors);
  update_IR_vals();
  int IR_check_val = IR_check(IR_colours);
  check_if_in_loop(IR_check_val);
  bool rec = recovery();
  if (rec) {
    return 1;
  }
  bool repeat = check_if_repeat_IR(IR_check_val);
  int line_sense_return_code = line_sensing(motors, IR_check_val, repeat, left_right_velocity, junction);
  if (line_sense_return_code) {
    return 2;
  }
  return 0;
}

void blink_amber(int clock_cycle) {
  blink_counter += 1;
  if (blink_counter % clock_cycle == 0) {
    blink_counter = 0;
    int curVal = digitalRead(amber_LED);
    digitalWrite(amber_LED, !curVal);
  }
}

int update_region(int ramp_detect_threshold) {
  float dist = check_ultrasonic_distance();
  if (dist > ramp_detect_threshold) {
    ramp_detect_counter = 0;
    ramp_on = false;
    region += 1;
    left_right_velocity = 200;
    return 1;
  }
  else {
    return 0;
  }
}

int line_sensing(Adafruit_DCMotor *motors[], int IR_check_val, bool repeat, int velocity, bool junction) {
  if (repeat) {
    return 0;
  }
  
  if (IR_check_val == 0) {
    move_both(motors, 255);
    Serial.println("FORWARD");
  }
  else if (IR_check_val == 1) {
    move_one_motor(motors, velocity, left);
    Serial.println("TURN LEFT");
  }
  else if (IR_check_val == 2) {
    move_one_motor(motors, velocity, right);
    Serial.println("TURN RIGHT");
  }
  else if (IR_check_val == 3) {
    Serial.println("Junction");
    if (junction) {
      Serial.println("STOP JUNCTIONNNNNNNN");
      move_both(motors, 0);
      return 1;
    }
    else {
      move_both(motors, 150);
    }
  }
  else if (IR_check_val == 4) {
    Serial.println("Ramp off");
  }
  else if (IR_check_val == 5) {
    Serial.println("Ramp on");
  }
  else {
    move_both(motors, 150);
    Serial.println("Else block fml");
  }
  return 0;
}


void move_one_motor(Adafruit_DCMotor *motors[], int velocity, int side) { //May need to account for direction later
  motors[(side+1)%2]->setSpeed(velocity);
  motors[side]->setSpeed(0);
}
void move_both(Adafruit_DCMotor *motors[], int velocity) {
  for(int j=0; j<2; j++) {
    motors[j]->setSpeed(velocity);
  }
}

void rotate(Adafruit_DCMotor *motors[], int velocity, int side) { //May need to account for direction later
  //If you want to turn right, the left motor moves forward but the right motor moves in the other direction
  motors[side]->run(regions_opposite[0]);
  motors[(side+1)%2]->run(regions[0]);
  move_both(motors, velocity);
}

float floating_average(int *index, float vals[], float curAvg, float newVal, int arr_len) {
  curAvg = ((curAvg * float(arr_len)) - vals[*index] + newVal)/float(arr_len);
  vals[*index] = newVal;
  *index = (*index + 1) % arr_len;
  return curAvg;
}

int IR_check(int LRFB[]) {
  if (LRFB[0] == black && LRFB[1] == black && LRFB[2] == white && LRFB[3] == white) {
    return 0;
  }
  else if (LRFB[0] == white && LRFB[1] == black) {
    return 1;
  }
  else if (LRFB[0] == black && LRFB[1] == white) {
    return 2;
  }
  else if (LRFB[0] == white && LRFB[1] == white && LRFB[2] == white && LRFB[3] == white) {
    return 3;
  }
  else if (LRFB[0] == black && LRFB[1] == black && LRFB[2] == white && LRFB[3] == black) {
    return 4;
  }
  else if (LRFB[0] == black && LRFB[1] == black && LRFB[2] == black && LRFB[3] == white) {
    return 5;
  }
  else {
    return 6;
  }
}

void update_IR_vals() {
  for(int i=0; i<num_IR; i++) {
    int new_val = analogRead(IR_locns[i]);
    IR_avgs[i] = floating_average(&IR_float_counter[i], IR_vals[i], IR_avgs[i], new_val, IR_floating_avg_num);
    if (IR_avgs[i] > IR_threshold[i]) {
      IR_colours[i] = black;
    }
    else {
      IR_colours[i] = white;
    }
  }
}

int update_metal_detector() {
  unsigned long new_val = pulseIn(metal_detector, HIGH);
  float new_val_Hz = 1000.0/float(new_val);
  metal_avg = floating_average(&metal_float_counter, metal_vals, metal_avg, new_val_Hz, metal_floating_avg_num);
}
void check_push_button(Adafruit_DCMotor *motors[]) {
  button_val = digitalRead(button_port);
  if (button_val == 0) {
    Serial.println("Button pushed. Exiting");
    for(int j = 0; j < 2; j++) {
      motors[j]->run(RELEASE);
    }
    while (1);
  }
}

bool check_if_repeat_IR(int IR_check_val) {
  bool is_repeat_IR = false;
  if (IR_check_val == cur_status) {
    is_repeat_IR = true;
  }
  else {
    cur_status = IR_check_val;
  }
  return is_repeat_IR;
}

void check_if_in_loop(int IR_check_val) {
  if (IR_check_val != 0 && cur_status == IR_check_val) { //Remember to change value after binary
    cur_loop_count += 1;
    if (cur_loop_count > 5000) {
      stuck = true;
    }
    else {
      stuck = false;
    }
  }
  else {
    stuck = false;
    cur_loop_count = 0;
  }
}

float check_ultrasonic_distance(void){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  new_US_distance = duration * 0.034 / 2;
  US_distance_avg = floating_average(&US_index, US_distance_vals, US_distance_avg, new_US_distance, US_float_avg_num);
  Serial.println(US_distance_avg);
  return US_distance_avg;
}

int turn_around(int turn_around_time, int turn_around_velocity){
  turn_around_time_start = millis();
  rotate(motors, turn_around_velocity, right);
  while(true){
    turn_around_time_end = millis();
    if (turn_around_time_end > turn_around_time_start + turn_around_time){
      move_both(motors, 0);
      motors[0]->run(regions[0]);
      motors[1]->run(regions[0]);
      return 0;
    }
  }
}

void bring_down_servos(int angle){
  servoright.write(angle);
  servoleft.write(180 - angle);
}

void bring_up_servos(void){
  servoright.write(90);
  servoleft.write(90);
}