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

#include <AFMotor.h>
//#include <NewPing.h>
#include "SR04.h"
#include "TimerOne.h"
#include <IRremote.h>

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

int pinCS = 53; // Pin to communicate with SD card

//Ustawienie timera
unsigned long startTime;
unsigned long followWallTime;
unsigned long followWall_frontTime;

//Ustawienie 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

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

//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 pilota
int RECV_PIN = 22;

IRrecv irrecv(RECV_PIN);
decode_results results;

//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 = 16;
const float Ki_wall = 0.003;
const float Kd_wall = 11;

const float Kp_front = 30;
const float Ki_front = 0;
const float Kd_front = 15;

const int offset = 12; // offset distance
const int max_front_distance = 16; // max front distance

int Tp = 128; // target power

float error_wall = 0;
float lastError_wall = 0;
int correction_wall = 0;
float p_wall = 0;
float i_wall = 0;
float d_wall = 0;

int Wall = 0;
int Front = 0;

float error_front = 0;
float lastError_front = 0;
float correction_front = 0;
float p_front = 0;
float i_front = 0;
float d_front = 0;

int leftMotorSpeed = 0;
int rightMotorSpeed = 0;

int front_status = 0;

//int rangeFrontObstacle = 0;
//int rangeWallObstacle = 0;
//int rangeFront = 0;
//int rangeWall = 0;
//int speedSet = 0;

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

int speedSet = 0;



void setup() {

  Serial.begin(9600);

  //SD setting
  pinMode(53, OUTPUT);
  delay(5000);

//Ustawienie timera
  Timer1.initialize(1000000); // setting timer to 1 sec
//zwieksz counter1 gdy pin predkosciomieza jest na Hig
  attachInterrupt(digitalPinToInterrupt (motor_A), ISR_count_A, RISING);
//zwieksz counter2 gdy pin predkosciomieza jest na High
  attachInterrupt(digitalPinToInterrupt (motor_B), ISR_count_B, RISING);
  Timer1.attachInterrupt(ISR_timerone);

  if (SD.begin(SS))
  {
    Serial.println("SD card is ready to use.");
  } else
  {
    Serial.println("SD card initialization failed");
    return;
  }

  myFile = SD.open("DaneMapy.txt", FILE_WRITE);

  if(myFile){
    myFile.print("Lewy silnik cm/s, Prawy silnik cm/s");
  }
  myFile.close();
}

void loop() {

  // put your main code here, to run repeatedly:
beginning:

  //scheck obstacle in front
  front_status = GET_FRONT_STATUS();

  if (front_status == OBSTACLE) {

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

    rightTurnBegin = 1;
    straightLineBegin = 0;

    goto beginning;
  }

  followWall();

}

void followWall() {
  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;

  //if error less than 10, robot drives correctly

  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;

    leftTurnBegin = 0;
    straightLineBegin = 1;
  } else {

	//if error greater than 10, robot has to turn right

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

    //PID for left turn
    int speed = 2.5 * error_wall + 8 * d_wall;

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

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

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

    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()
{

  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 > 160 && correction_front > 0) {
    correction_front = 160;
  }

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

  rightMotorSpeed  = 95 + correction_front;
  leftMotorSpeed = 95 - correction_front;

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

  lastError_front = error_front;
}


//Rpm to cm/s
int RMPtoCmS(float RPM){
  int result;
  float radius = (wheeldiameter / 2);
  float metersPerSecond = (radius * 0.001) * RPM * 0.10472; //speed m/s
  float cmPerSec = metersPerSecond * 100; // m/s to cm/s

  float f_result = cmPerSec;
  result = (int) f_result;

  return result;
}

//(Interupt Service Routines ISR)

void ISR_count_A(){
  counter_A++;
}

void ISR_count_B(){
  counter_B++;
}

//TimerOne ISR
void ISR_timerone(){
  Timer1.detachInterrupt(); //Stop timer

  Serial.print("Motor1 Speed: ");
  float rotation_A = (counter_A / diskslots) * 60.00; //(RPM) for motor1
  Serial.print(RMPtoCmS(rotation_A));
  Serial.print(" Cm/s - ");
  counter_A = 0;

  Serial.print("Motor2 Speed: ");
  float rotation_B = (counter_B / diskslots) * 60.00; //(RPM) for motor2
  Serial.print(RMPtoCmS(rotation_B));
  Serial.println(" Cm/s");
  counter_B = 0;

  float average_speed = (RMPtoCmS(rotation_A) + RMPtoCmS(rotation_B)) /2;

  myFile = SD.open("DaneMapy.txt", FILE_WRITE);
  if (myFile) {
  Serial.println("Writing to file...");
    myFile.print("Motor1: ");
    myFile.print(RMPtoCmS(rotation_A));
    myFile.print("Motor2: ");
    myFile.print(RMPtoCmS(rotation_B));
    myFile.print(" average_speed: ");
    myFile.print(average_speed);
    myFile.close();
   // delay(500);
   Serial.println("Done.");
  }

  Timer1.attachInterrupt(ISR_timerone);
}

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;
}