// #include <Arduino.h>
// #include <HX711.h>

const int dc_dir = 2, dc_spd = 10;
const int dir_1 = 52, step_1 = 45, slp_1 = 6, mode_1a = 4, mode_1b = 7;
const int dir_2 = 53, step_2 = 44, slp_2 = 51, mode_2a = 46, mode_2b = 50;
const int wght_data = 5, wght_clk = 9;
const int dis_echo = 12, dis_trig = 13;

float position_step_1, position_step_2, position_cart = 0.0;  //position_cart misschien niet nodig

// HX711 scale;

float stepValue()
{
  if (digitalRead(mode_1a) == 0 && digitalRead(mode_1b) == 0)
    return 1.8;
  if (digitalRead(mode_1a) == 1 && digitalRead(mode_1b) == 0)
    return 1.8 / 2;
  if (digitalRead(mode_1a) == 0 && digitalRead(mode_1b) == 1)
    return 1.8 / 4;
  if (digitalRead(mode_1a) == 1 && digitalRead(mode_1b) == 1)
    return 1.8 / 16;
  return 0.0;
}

float getDistance()
{
  // Set low to ensure clean high signal:
  digitalWrite(dis_trig, LOW);
  delayMicroseconds(2);
  // Start a new measurement:
  digitalWrite(dis_trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(dis_trig, LOW);

  // Read the result:
  float duration = pulseIn(dis_echo, HIGH);

  // Distance in cm:
  float distance = duration / 58.0;  // MOET NOG GECALIBREERD WORDEN

  return distance;
}

void invert_pin(int signed pin)
{
  int state = digitalRead(pin);
  if (state == 0)
    digitalWrite(pin, HIGH);
  else
    digitalWrite(pin, LOW);
}

void move_to_angle(float angle)
{
  float step = stepValue();
  bool inverted_step_1 = false;
  float totalAngle = angle - position_step_1;
  int stepNumber = totalAngle / step;
  position_step_1 = position_step_1 + (stepNumber * step);
  if (totalAngle < 0)
  {
    invert_pin(dir_1);
    totalAngle = -totalAngle;
    inverted_step_1 = true;
  }
  stepNumber = totalAngle / step;
  for (int x = 0; x < stepNumber; x++)
  {
    digitalWrite(step_1, HIGH);
    delay(1);
    digitalWrite(step_1, LOW);
    delay(1);
  }
  if (inverted_step_1)
  {
    invert_pin(dir_1);
    inverted_step_1 = false;
  }
}

float middleAngleDistance()
{
  float startAngle = 60; // nog aan te passen
  float endAngle = 300; // nog aan te passen
  move_to_angle(startAngle);
  delay(1000);
  int x = 0;
  while (getDistance() > 100 && position_step_1 <= endAngle) // nog aan te passen
  {
    x++;
    move_to_angle(startAngle + x);
  }
  float edgeAngle1 = position_step_1;
  delay(1000);
  x = 0;
  while (getDistance() < 100) // nog aan te passen
  {
    x++;
    move_to_angle(edgeAngle1 + x);
  }
  float edgeAngle2 = position_step_1 - stepValue();
  delay(1000);
  float middleAngle = (edgeAngle1 + edgeAngle2) / 2;
  move_to_angle(middleAngle);
  delay(1000);
  Serial.println("middleangle:"  + String(middleAngle));
  if(position_step_1 >= endAngle)
  {
    return middleAngleDistance();
  }
  float middleDistance = getDistance();
  Serial.println("middledistance:" + String(middleDistance));
  float distanceToCart = 20; // nog aan te passen
  float distanceToSensor = 15; // nog aan te passen
  float distanceToMiddleZone = 5; // nog aan te passen
  float totalDistance = middleDistance + distanceToMiddleZone + distanceToSensor;
  Serial.println("totalDistance:" + String(totalDistance));
  float angleBack = 2*atan((totalDistance-sqrt((totalDistance+distanceToCart)*(totalDistance-distanceToCart)))/distanceToCart);
  totalDistance = sqrt((totalDistance+distanceToCart)*(totalDistance-distanceToCart));
  angleBack = angleBack * 180 / PI;
  Serial.println("angleback:" + String(angleBack));
  Serial.println(position_step_1 - angleBack);
  move_to_angle(position_step_1 - angleBack);
  Serial.println(position_step_1);
  return totalDistance;
}

void move_to_distance(float distance)
{
  float step = stepValue();
  Serial.println(distance);
  bool inverted_step_2 = false;
  float distanceTurn = 100; // nog aan te passen
  float distanceDegree = distanceTurn / 360;  // misschien niet nodig
  float turns = distance / distanceTurn;   
  float angle = turns * 360;
  float totalAngle = angle - position_step_2;
  int stepNumber = totalAngle / step;
  position_step_2 = position_step_2 + (stepNumber * step);
  position_cart = position_step_2 * distanceDegree;  // misschien niet nodig
  if (totalAngle < 0)
  {
    invert_pin(dir_2);
    totalAngle = -totalAngle;
    inverted_step_2 = true;
  }
  stepNumber = totalAngle / step;
  for (int x = 0; x < stepNumber; x++)
  {
    digitalWrite(step_2, HIGH);
    delay(10);
    digitalWrite(step_2, LOW);
    delay(10);
  }
    if (inverted_step_2)
  {
    invert_pin(dir_2);
    inverted_step_2 = false;
  }
  Serial.println(position_step_2);
  Serial.println(position_cart);
}

void setup()
{
  for (int i = 2; i < 54; i++)
    if (i != wght_data && i != dis_echo)
      pinMode(i, OUTPUT);
    else
      pinMode(i, INPUT);

  digitalWrite(mode_1a, HIGH);
  digitalWrite(mode_1b, HIGH);
  digitalWrite(dir_1, HIGH);
  digitalWrite(dir_2, HIGH);

  // start serial connection with raspberry pi and open connection for app
  Serial.begin(9600);
  delay(1000);
  move_to_distance(middleAngleDistance());
  delay(1000);
  // move_to_angle(100);
  // move_to_angle(80);


  // scale.begin(wght_data, wght_clk);
  // scale.set_scale(419); // MOET NOG GECALIBREERD WORDEN
  // Serial.println("Make sure the scale is empty!");
  // delay(3000);
  // scale.tare();

  // open connection to app
}

void loop()
{
  // Serial.println("  Distance  |  Weight  \n   " + String(getDistance()) + "cm      " + String(round(scale.get_units(1) * 10000.0) / 10000.0, 4) + "kg\n");

  // move_to_angle(100);

  // Serial.println("  Distance  |  Weight  \n   " + String(getDistance()) + "cm      " + String(round(scale.get_units(1) * 10000.0) / 10000.0, 4) + "kg\n");

  // move_to_angle(-60);

  // Serial.println("  Distance  |  Weight  \n   " + String(getDistance()) + "cm      " + String(round(scale.get_units(1) * 10000.0) / 10000.0, 4) + "kg\n");

  // move_to_angle(0);

  // set all pins low so code stays consistent if repeated
  //  for(int i = 2; i < 54; i++)
  //    if(i != wght_data && i != dis_echo)
  //      digitalWrite(i, LOW);
  
  //  (motion detection op raspberry pi -- play music)

  // wait for connection through app
  // once app has launched the arduino sends a signal to the pi to set the in_use variable to TRUE so that the customer doesn't get blasted with music every second
  // Serial.write("in use");
  // button in app to start calibration for volume/color with camera
  // tower rotating stepper turns facing the first container and arduino sends ready signal to pi
  // camera takes photo and determines volume and color
  // repeat last 2 actions, 3 times for every container
  // send all info to the arduino
  // calculate mass in every container with yash formula and set percentage relative to a full container
  // arduino displays findings and asks for confirmation, if not correct -> manual input through app
  // input order through app
  // extraction
  // turn tower towards first container of requested color
  // extend screw with big servo into the container
  // receive input from scale
  // turn screw to get beans
  // display collected weight in app
  // display percentage left in each container
  // stop when weight >= request
  // turn screw in reverse to empty screw
  // digitalWrite(dc_dir, HIGH);
  // return screw to retracted position
  // repeat for every color if order has more than 1 type of beans

  // delivery
  // scan area for delivery container
  // tower-rotating stepper turns until the distance sensor detects a substantial change in distance
  // rotate towards middle of delivery container
  // turn as close to each side and store rotation angle
  // turn to the average angle
  // measure distance towards delivery container
  // send cart the measured distance + constant distance towards delivery container (calibration needed)
  // open hatch to drop beans into delivery container with micro stepper
  // reverse motion to close hatch
  // return everything to home position
  // show percentage and mass left in each container
  // send in_use = false to pi
  // pi plays a goodbye sound :)
}