// Basic Bluetooth sketch HC-05_02
// Connect the HC-05 module and communicate using the serial monitor
//
// The HC-05 defaults to commincation mode when first powered on.
// The default baud rate for communication mode is 9600
//

#include <SoftwareSerial.h>
SoftwareSerial BTserial(2, 3); // RX | TX
// Connect the HC-05 TX to Arduino pin 2 RX.
// Connect the HC-05 RX to Arduino pin 3 TX through a voltage divider.
//
class BluetoothDataReader {
  public:
    String angle;
    String type;

  public:
    BluetoothDataReader() {
    }

    void read_bluetooth_data_and_parse_wanted_values(String myString) {
      int symbol_index = myString.indexOf("_");
      type = myString.substring(0, symbol_index);
      angle = myString.substring(symbol_index + 1, myString.length());
    }

    String getAngle() {
      return this->angle;
    }
    String getType() {
      return this->type;
    }
};

char c = ' ';
String wholeMessage = "";
BluetoothDataReader* bdr;

//------------------
#define STEPPER_PIN_1 8
#define STEPPER_PIN_2 9
#define STEPPER_PIN_3 10
#define STEPPER_PIN_4 11
int step_number = 0;


//Rotation motor constant variables
#define MAX_ANGLE 180
#define MIN_ANGLE 0
#define BLIND_LENGTH 150
#define ANGLE_PER_ROTATION_PERCENTAGE  22.5
#define STEPS_PER_FULL_ROTATION  2100

int current_angle = 0;
int target_angle = 0;
//------------------

void setup()
{
  Serial.begin(9600);
  BTserial.begin(9600);
  Serial.println("Arduino is ready");
  bdr = new BluetoothDataReader();

  //rotation setup
  pinMode(STEPPER_PIN_1, OUTPUT);
  pinMode(STEPPER_PIN_2, OUTPUT);
  pinMode(STEPPER_PIN_3, OUTPUT);
  pinMode(STEPPER_PIN_4, OUTPUT);
}
void loop()
{
  //  if (BTserial.available()) {
  //  String msg_bt = parse_msg_from_bluetooth();
  String msg_bt = "r_45";
  bdr->read_bluetooth_data_and_parse_wanted_values(msg_bt);
  wholeMessage = "";
  Serial.print("angle:");
  Serial.println(bdr->getAngle());
  Serial.print("type:");
  Serial.println(bdr->getType());

  if (bdr->getType().equals("r")) {
    target_angle = bdr->getAngle().toInt();
    Serial.print("target:");
    Serial.println(target_angle);


    if (target_angle < MIN_ANGLE) {
      target_angle = MIN_ANGLE;
    }
    if (target_angle > MAX_ANGLE) {
      target_angle = MAX_ANGLE;
    }
    if (target_angle != 0) {
      int rotations = calculateRotations();
      bool direc = calculateRotationDirection();
      Serial.print("rotations:");
      Serial.println(rotations);
      Serial.print("direc:");
      Serial.println(direc);
      for (int i = 0; i < rotations; i++) {

        for (int j = 0; j < STEPS_PER_FULL_ROTATION; j++) {
          OneStep(direc);
          delay(2);
        }
      }
      current_angle = target_angle;
      target_angle = 0;
      delay(10000);
    }
  }
  if (bdr->getType().equals("m")) {
  }
  //  }
  delay(100);
}

String parse_msg_from_bluetooth() {
  while (BTserial.available()) {
    c = BTserial.read();
    if (c != '{' && c != '}') {
      wholeMessage += c;
    }
    if (c == '}') {
      c = '\0';
      return wholeMessage;
    }
  }
}

int calculateRotations() {
  int rotations_to_do = current_angle - target_angle;
  return abs(rotations_to_do) / ANGLE_PER_ROTATION_PERCENTAGE;
}

bool calculateRotationDirection() {
  return current_angle - target_angle < 0 ? true : false;
}

void OneStep(bool dir) {
  if (dir) {
    switch (step_number) {
      case 0:
        digitalWrite(STEPPER_PIN_1, HIGH);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, LOW);
        break;
      case 1:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, HIGH);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, LOW);
        break;
      case 2:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, HIGH);
        digitalWrite(STEPPER_PIN_4, LOW);
        break;
      case 3:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, HIGH);
        break;
    }
  } else {
    switch (step_number) {
      case 0:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, HIGH);
        break;
      case 1:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, HIGH);
        digitalWrite(STEPPER_PIN_4, LOW);
        break;
      case 2:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, HIGH);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, LOW);
        break;
      case 3:
        digitalWrite(STEPPER_PIN_1, HIGH);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, LOW);


    }
  }
  step_number++;
  if (step_number > 3) {
    step_number = 0;
  }
}
// Keep reading from Arduino Serial Monitor and send to HC-05
//  if (Serial.available())
//  {
//    c =  Serial.read();
//    BTserial.write(c);
//  }