#include <LedControl.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// 8x8 Matrix
#define DIN_PIN 11
#define CS_PIN  10
#define CLK_PIN 13

// L298N driver
#define IN1 8
#define IN2 7
#define IN3 5
#define IN4 4
#define ENA 6
#define ENB 3

#define SERVO 9

#define W  0
#define NW 45
#define N  90
#define NE 135
#define E  180

#define TRIG 14
#define ECHO 15

// Serial output constants
#define FORWARD             ">F<"   // Robot platform moving forward
#define RIGHT               ">R<"   // Platform rotates RIGHT in-place ~90 deg
#define LEFT                ">L<"   // Platform rotates LEFT in-place ~90 deg
#define SCAN_FRONT_CLEAR    ">SF0<"
#define SCAN_FRONT_OBSTACLE ">SF1<"
#define SCAN_RIGHT_CLEAR    ">SR0<"
#define SCAN_RIGHT_OBSTACLE ">SR1<"
#define SCAN_LEFT_CLEAR     ">SL0<"
#define SCAN_LEFT_OBSTACLE  ">SL1<"

// 8x8 Matrix images as 64-bit long integer
#define FRONT_0       0x18187e7e1818817e
#define FRONT_1       0x00007e7e0000817e
#define RIGHT_0       0x408c8cbfbf8c8c40
#define RIGHT_1       0x408080bfbf808040
#define LEFT_0        0x023131fdfd313102
#define LEFT_1        0x020101fdfd010102
#define SCANING_LEFT  0x0209259595250902
#define SCANING_FRONT 0x18002418423c817e
#define SCANING_RIGHT 0x4090a4a9a9a49040

#define NE_1          0x00003fbf80808078
#define NE_0          0x0c0c3fbf8c8c8078
#define NW_1          0x0000fcfd0101011e
#define NW_0          0x3030fcfd3131011e

#define FORWARD       0x0042663c5a663c18
// #define FORWARD2      0x42663c5a663c1800
#define R_LEFT        0xc0c0e4763f1f0604
// #define R_LEFT2       0xc0c0e87838780000
#define R_RIGHT       0x0303276efcf86020
// #define R_RIGHT2      0x0303171e1c1e0000


SoftwareSerial btSerial(16, 17);
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo;
LedControl display = LedControl(DIN_PIN, CLK_PIN, CS_PIN);

bool goingForward    = 0;
bool goingBack       = 0;
bool standing        = 1;
bool rotatingLeft    = 0;
bool rotatingRight   = 0;
bool scaning         = 0;
bool goodToGo        = 0;
bool nextStep        = 0;
bool servoIsRotating = 0;
bool servoHasRotated = 0;

int rotation_time     = 2000;
int driving_time      = 2000;
int servo_rotation    = 500;
int pause             = 500;
int obstacle_treshold = 30;

byte driving_speed  = 200;
byte rotation_speed = 200;

int movements_count = 0;
int rotations = 0;
unsigned long time;
unsigned long pause_start;
unsigned long movement_start;
unsigned long rotation_start;
unsigned long servo_start;

byte rotationAngles[] = {W, N, E};
byte scanResults[] = {3,3,3};
byte scaningStep = 0;
// 0 - scaning LEFT
// 1 - scaning FRONT
// 2 - scaning RIGHT
byte step = 1;
// 1 - scaning left
// 2 - scaning front
// 3 - scaning right
// 4 - movement
byte movements[5] = {0,0,0,0,0};
// 0 - no movement
// 1 - forward
// 2 - right
// 3 - left

void setup() {
  Serial.begin(9600);
  btSerial.begin(9600);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  servo.attach(SERVO);
  servo.write(N);

  lcd.init();
  lcd.backlight();

  display.clearDisplay(0);
  display.shutdown(0, false);
  display.setIntensity(0, 10);

  delay(1000);

  Serial.println(F("^_^"));
  for (int i = 0; i < 3; i++){
    Serial.println(rotationAngles[i]);
  }
}

void displayImage(uint64_t image) {
  for (int i = 0; i < 8; i++) {
    byte row = (image >> i * 8) & 0xFF;
    for (int j = 0; j < 8; j++) {
      display.setLed(0, i, j, bitRead(row, j));
    }
  }
}

void lcdWrite() {
  lcd.setCursor(0, 0);
  lcd.print(F("Mov.:    Rotat.:"));
  lcd.setCursor(0, 1);
  lcd.print(movements_count);
  lcd.setCursor(9, 1);
  lcd.print(rotations);
  //   0123456789ABCDEF
  // 0 Mov.:    Rotat.:
  // 1 1        1
}

int ultrasonic() {
  //Signala izsutisana no Trig pin
  digitalWrite(TRIG, LOW); // Pirms impulsa sutisanas, parliecinamies, ka Trig pin ir izslegts
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  //Atbildes impulsa salasisana no Sensora
  long ilgums = pulseIn(ECHO, HIGH);

  //Attaluma noteiksana
  //Izmantojam skanas atrumu 343 metri sekunde

  int attalums = (ilgums / 2) * 0.0343;

  return attalums;
}

void forward() {
  analogWrite(ENA, driving_speed);
  analogWrite(ENB, driving_speed);
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
  standing = 0;
}

void backward() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);

}

void motorsStop() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 0);

}

void left() {
  analogWrite(ENA, rotation_speed);
  analogWrite(ENB, rotation_speed);
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);

}

void right() {
  analogWrite(ENA, rotation_speed);
  analogWrite(ENB, rotation_speed);
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
}

void changeStep() {
  if (++step > 5) step = 0;
  nextStep = 0;
}

int scaningDirection() {
  int scaningResults[5];
  for (int i = 0; i < 5; i++){
    scaningResults[i] = ultrasonic();
    delay(50);
  }
  int smallest = scaningResults[0];
  for (int i = 1; i < 5; i++){
    smallest = min(smallest, scaningResults[i]);
  }
  return smallest;
}

void scaningProcess() {
  if (!servoIsRotating) {
    // Serial.print("servoIsRotating? "); Serial.println(servoIsRotating);
    if (servoHasRotated) {
      servoHasRotated = 0;
      int distance = scaningDirection();
      // Serial.print(F("Scaned dist: ")); Serial.println(distance);
      if (distance >= obstacle_treshold) {
        scanResults[scaningStep] = 0;
      } else {
        scanResults[scaningStep] = 1;
      }
      if (++scaningStep > 2) {
        scaningStep = 0;
        scaning = 0;
        changeStep();
      }
    } else {
      servo.write(rotationAngles[scaningStep]);
      // Serial.print("servo rotates to: "); Serial.println(rotationAngles[scaningStep]);
      servo_start = time;
      servoIsRotating = 1;
    }
  }
}

void displayMatrixInfo() {
  if (scaning) {
    switch (scaningStep) {
      case 0:
        if (scanResults[scaningStep] > 1) {
          displayImage(SCANING_LEFT);
        } else if (scanResults[scaningStep] == 0) {
          displayImage(LEFT_0);
        } else if (scanResults[scaningStep] == 1) {
          displayImage(LEFT_1);
        } 
        break;
      case 1:
        if (scanResults[scaningStep] > 1) {
          displayImage(SCANING_FRONT);
        } else if (scanResults[scaningStep] == 0) {
          displayImage(FRONT_0);
        } else if (scanResults[scaningStep] == 1) {
          displayImage(FRONT_1);
        }
        break;
      case 2:
        if (scanResults[scaningStep] > 1) {
          displayImage(SCANING_RIGHT);
        } else if (scanResults[scaningStep] == 0) {
          displayImage(RIGHT_0);
        } else if (scanResults[scaningStep] == 1) {
          displayImage(RIGHT_1);
        }
        break;
      default:
        break;
    }
  }

  if (goingForward)  { displayImage(FORWARD); }
  if (rotatingLeft)  { displayImage(R_LEFT); }
  if (rotatingRight) { displayImage(R_RIGHT); }
  
}

char previous_rotation = ' ';

void chooseMovement() {
  // if going STRAIGHT is not an option,
  // default rotation will be RIGHT, then FORWARD, then RIGHT again
  // if RIGHT is not available, but LEFT is clear -> turning LEFT, than
  // if RIGHT is not available -> turnin LEFT
  if (scanResults[1] == 0) {
    movements[0] = 1; // Front
    goingForward = 1;
  } else if (scanResults[2] == 0) {
    movements[0] = 2; // RIGHT
    movements[1] = 1; // FRONT
    movements[2] = 2; // RIGHT
  } 
  // else if () {

  // }
}

unsigned long debugTime;
unsigned long lcdRefreshTime;

bool first = 1;

void loop() {
  time = millis();

  // 1. Scaning LEFT, FRONT and RIGHT
  // 2. Choosing, where to go

  if (goodToGo) {
    switch (step) {
      case 1:
        scaning = 1;
        break;
      case 2:
        // scaning = 1;
        chooseMovement();
        break;
      default:
        break;
    }
  }

  if (Serial.available() > 0) {
    char input = Serial.read();
    if (input != 10 or input != 13){
    switch (input) {
      case 'r':
        displayImage(RIGHT_0);
        break;
      default:
        displayImage(0);
    }
    }  
  }

  if (nextStep) {
    changeStep();
  }

  if (scaning) {
    scaningProcess();
  }

  if (pause_start + pause <= time) {
    goodToGo = 1;
  }

  if (servoIsRotating && servo_start + servo_rotation <= time) {
    servoIsRotating = 0;
    servoHasRotated = 1;
    // Serial.println("Servo has stoped!");
  }

  if (debugTime + 500 <= time) {
    debugTime = time;
    printDebugInfo();
  }

  if (lcdRefreshTime + 1000 <= time) {
    lcdWrite();
    lcdRefreshTime = time;
  }

  displayMatrixInfo();
  // if (time > 10000) {exit(1);}
  // if (first) {
  //   // displayImage(FORWARD);
  //   displayImage(SCANING_RIGHT);
  //   first = 0;
  // }
}

void printDebugInfo() {
  // Serial.print(F("step: ")); Serial.print(step);
  // Serial.print(F(" scaning: ")); Serial.print(scaning);
  // Serial.print(F("\t")); Serial.println(time);
  Serial.print(F("L: ")); Serial.print(scanResults[0]);
  Serial.print(F("\tF: ")); Serial.print(scanResults[1]);
  Serial.print(F("\tR: ")); Serial.print(scanResults[2]);
  Serial.println();
}