#include <SPI.h>
#include <SD.h>

#include <AFMotor.h>
#include "SR04.h"

//Ustawienia karty SD
File myFile;
const int8_t DISABLE_CHIP_SELECT = 10;

int pinCS = 53; // Pin komunikacji z karta SD

//zmienne dla predkosciomierza
const byte motor_A = 20; //Motor1 interrupt pin
const byte motor_B = 21; //Motor2 interrupt pin

volatile int counter_A = 0; //puls counter variables
volatile int counter_B = 0;

float diskslots = 20.00; // encoders slots

float average_speed;
const float wheeldiameter = 65.10; // wheel diameter (mm)

//Zmienne dla millis
unsigned long startMillis;
unsigned long currentMillis;
unsigned long speedMillis;
const unsigned long period = 100;

bool timeToWrite = false;

// This is a structure to hold data.
struct robotLog
{
  uint8_t Direction; // 0 - prosto 1 - skret
};

typedef struct robotLog RobotLog;

// Now create the buffer. The size can be changed
const unsigned int numOfLogsToBuffer = 10;
RobotLog sdBuffer[numOfLogsToBuffer];

// index to indicate the current position in the buffer.
unsigned int logBufferIndex = 0;


//Ustawienie sensorow
int LEFT_SENSOR_TRIG = A2;
int LEFT_SENSOR_ECHO = A3;
int FRONT_SENSOR_TRIG = A0;
int FRONT_SENSOR_ECHO = A1;

SR04 sr04_left = SR04(LEFT_SENSOR_ECHO, LEFT_SENSOR_TRIG);
SR04 sr04_front = SR04(FRONT_SENSOR_ECHO, FRONT_SENSOR_TRIG);

//Ustawienie silnikow
AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(3, MOTOR34_1KHZ);

int NO_OBSTACLE = 0;
int OBSTACLE = 1;

//Zmienne PID
const float Kp_wall = 8; // stala uzyta do skretu
const float Ki_wall = 0.003; // stala calki uzyta do poprawy bledu
const float Kd_wall = 3; // stala rozniczki uzyta do poprawy predkosci poprawy bledu

const float Kp_front = 30; // stala uzyta do skretu
const float Ki_front = 0; // stala calki uzyta do poprawy bledu
const float Kd_front = 15; // stala rozniczki uzyta do poprawy predkosci poprawy bledu

const int offset = 13; // docelowa odleglosc
const int max_front_distance = 20; // maksymalna odleglosc z przodu

int Tp = 128; // docelowa predkosc - target power

float error_wall = 0;
float lastError_wall = 0;
int correction_wall = 0;
float p_wall = 0;  //proporcja
float i_wall = 0;  //calka
float d_wall = 0;  //rozniczka

int Wall = 0;
int Front = 0;

float error_front = 0;
float lastError_front = 0;
float correction_front = 0;
float p_front = 0;  //proporcja
float i_front = 0;  //calka
float d_front = 0;  //rozniczka

int leftMotorSpeed = 0;
int rightMotorSpeed = 0;

int robot_status = 0; // 0 - jazda prosto; 1 - skret w lewo; 2 - przeszkoda
int front_status = 0;

//0 - true; 1 - false
int rightTurnBegin = 0;
int leftTurnBegin = 0;
int straightLineBegin = 0;

int speedSet = 0;

void setup() {
  Serial.begin(9600);
  //Ustawienie czytnika kart SD
  pinMode(53, OUTPUT);

  if (!SD.begin(pinCS))
  {
    Serial.println("SD card initialization failed");
    return;
  } else
  {
    Serial.println("SD card ready");
  }

  SD.remove("DaneMapy.txt");
  myFile = SD.open("DaneMapy.txt", O_CREAT | O_APPEND | O_WRITE);
  myFile.close();
}

void loop() {
  currentMillis = millis();

  timerMillis();
  if (timeToWrite == true) {
    if (leftMotorSpeed != 0 && rightMotorSpeed != 0) {
      storeData();
    }
  } else {
    //starts robot
    checking_status();
  }
}

void checking_status() {
  front_status = GET_FRONT_STATUS();

  if (front_status == OBSTACLE) {

    if (leftTurnBegin == 0) {
      zero_wall();
    }
    followWall_front();

    // Zmieniam flage skretu na false do nastepnego czasu az natrafi na przeszkode
    rightTurnBegin = 1;
    straightLineBegin = 0;

  }
  if (front_status != OBSTACLE) {
    followWall();
  }
}

void followWall() {
  //Kiedy robot zaczyna jechac prosto (dopoki nie ma przeszkody) zmieniam flage na true
  rightTurnBegin = 0;

  Wall = sr04_left.Distance();
  error_wall = Wall - offset;
  i_wall = (i_wall + error_wall);
  d_wall = (error_wall - lastError_wall);

  correction_wall = Kp_wall * error_wall + Ki_wall * i_wall + Kd_wall * d_wall;

  //jezeli error_wall jest mniejszy od 10, oznacza to ze robot jedzie obok sciany poprawnie
  if (error_wall < 10) {

    if (straightLineBegin == 0) {
      zero_wall();
    }

    if (correction_wall > 127 && correction_wall > 0) {
      correction_wall = 127;
    }

    if (correction_wall < -127 && correction_wall < 0) {
      correction_wall = -127;
    }

    rightMotorSpeed  = Tp + correction_wall;
    leftMotorSpeed = Tp - correction_wall;
    robot_status = 0;
    leftTurnBegin = 0;
    straightLineBegin = 1;
  } else {

    //jezeli error_wall jest wiekszy od 10, oznacza to ze robot musi skrecic w lewo

    //zeruje wartosci na poczatku skretu w lewo
    if (leftTurnBegin == 0) {
      zero_wall();
      zero_front();
    }

    //PID do skretu w lewo
    int speed = 2.5 * error_wall + 7 * d_wall;

    if (speed > 127 && speed > 0) {
      speed = 127;
    }

    if (speed < -127 && speed < 0) {
      speed = -127;
    }

    rightMotorSpeed  = Tp + (speed);
    leftMotorSpeed = Tp - (speed);
    robot_status = 1;

    leftTurnBegin = 1;
    straightLineBegin = 0;
  }

  motor1.setSpeed(rightMotorSpeed);
  motor1.run(FORWARD);
  motor2.setSpeed(leftMotorSpeed);
  motor2.run(FORWARD);

  lastError_wall = error_wall;
  zero_front();
}

void zero_wall(void) {
  i_wall = 0;
  d_wall = 0;
  lastError_wall = 0;
}

void zero_front(void) {
  i_front = 0;
  d_front = 0;
  lastError_front = 0;
}

void followWall_front()
{
  //tu umiec zczytywanie z sensora

  Front = sr04_front.Distance();
  error_front = (Front - max_front_distance);
  i_front = (i_front + error_front);
  d_front = (error_front - lastError_front);

  correction_front = Kp_front * error_front + Ki_front * i_front + Kd_front * d_front;


  if (correction_front > 127 && correction_front > 0) {
    correction_front = 127;
  }

  if (correction_front < -127 && correction_front < 0) {
    correction_front = -127;
  }

  rightMotorSpeed  = Tp + correction_front;
  leftMotorSpeed = Tp - correction_front;
  robot_status = 2;

  motor1.setSpeed(rightMotorSpeed);
  motor1.run(FORWARD);
  motor2.setSpeed(leftMotorSpeed);
  motor2.run(FORWARD);

  lastError_front = error_front;
}

void timerMillis() {
  if (currentMillis - startMillis >= period) {
    timeToWrite = true;
    startMillis = currentMillis;
  }
}

void storeData() {
  sdBuffer[logBufferIndex].Direction = robot_status;

  logBufferIndex++;
  if (logBufferIndex == 10) {
    motor1.setSpeed(0);
    motor1.run(FORWARD);
    motor2.setSpeed(0);
    motor2.run(FORWARD);
    myFile = SD.open("DaneMapy.txt", O_CREAT | O_APPEND | O_WRITE);
    for (int i = 0; i < 10; i++) {
      if (myFile) {
        myFile.println(sdBuffer[i].Direction);
      }
    }
    myFile.close();
    logBufferIndex = 0;
  }
  timeToWrite = false;
}

int mediana(int n, int x[]) {
  int temp;
  int i, j;
  for (i = 0; i < n - 1; i++) {
    for (j = i + 1; j < n; j++) {
      if (x[j] < x[i]) {
        temp = x[i];
        x[i] = x[j];
        x[j] = temp;
      }
    }
  }
  return x[n / 2];
}
int GET_LEFT_STATUS(void) {
  if (sr04_left.Distance() < offset)
    return OBSTACLE;
  else
    return NO_OBSTACLE;
}

int GET_FRONT_STATUS(void) {
  if (sr04_front.Distance() < max_front_distance)
    return OBSTACLE;
  else
    return NO_OBSTACLE;
}