/*
  Author :  Simeon Simeonov
  Name :  Bolid  Arduino project
  Ver:  Dancho micro optimizing
*/
#include <Arduino.h>
#include <PWM.h>

#include "pitches.h"
#include <stdio.h> // for function sprintf

#include <SoftwareSerial.h>
//--------------------- PIN definitions -----------

#define RIGHT_EDGE  A2      // A0
#define RIGHT_POS   A3      // A1
#define CENTERR_POS A4      // A2
#define CENTERL_POS A5      // A2
#define LEFT_POS    A6      // A3
#define LEFT_EDGE   A7      // A4

#define LEFT_PWM    9
#define RIGHT_PWM   10
#define LEFT_DIR    5
#define RIGHT_DIR   6

#define PWM_FREQ      50000


//-------------- Beep ----------
#define BEEP        13

//--------------  Buttons -----------
#define BUTT1     7
#define BUTT2     11


int bluetoothTx = 1;                                  // TX-O пин от bluetooth на пин D2
int bluetoothRx = 2;                                  // RX-I пин от bluetooth на пин D3

byte PS_16 = (1 << ADPS2);
//byte PS_32 = (1 << ADPS2) | (1 << ADPS0);
//byte PS_64 = (1 << ADPS2) | (1 << ADPS1);
byte PS_128 = (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);

//--------------------------------
int melody[] = { NOTE_C4, NOTE_G3, NOTE_G3, NOTE_A3, NOTE_G3, 0, NOTE_B3, NOTE_C4 };
// note durations: 4 = quarter note, 8 = eighth note, etc.:
int noteDurations[] = { 4, 8, 8, 4, 4, 4, 4, 4 };
SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);   // създаване на bluetooth


void setup() {

  Serial.begin(9600);                                           // серийна комуникация
  bluetooth.begin(9600);                              // Стартиране на bluetooth
  pinMode(RIGHT_EDGE, INPUT);

  pinMode(LEFT_DIR, OUTPUT);
  pinMode(RIGHT_DIR, OUTPUT);

  // --- Изходи  ---
//  pinMode(GREEN_LED, OUTPUT);
  //pinMode(RED_LED, OUTPUT);
  pinMode(BEEP, OUTPUT);

  // --- Входове ---
  pinMode(BUTT1, INPUT);
  pinMode(BUTT2, INPUT);

  //---- PWM инициализация за управление на моторите
  InitTimersSafe();
  if ( SetPinFrequencySafe(LEFT_PWM, PWM_FREQ))
  {
    pinMode(LEFT_PWM, OUTPUT);
  }
  if (SetPinFrequencySafe(RIGHT_PWM, PWM_FREQ))
  {
    pinMode(RIGHT_PWM, OUTPUT);
  }

  // set sample rate to XXXX X001 => 16 micros readings
  ADCSRA &= ~PS_128;
  ADCSRA |= PS_16;

  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);

  tone(BEEP, 2400, 400);
  delay(2000);
  noTone(BEEP);
  // Изчакване натискането на бутон
  while (digitalRead (BUTT1) == HIGH);



//  // ---  Изпълняване на мелодията ---
///*
  for (int thisNote = 0; thisNote < 8; thisNote++) {
    // to calculate the note duration, take one second
    // divided by the note type. //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
    int noteDuration = 1000 / noteDurations[thisNote];
    tone(BEEP, melody[thisNote], noteDuration);
    // to distinguish the notes, set a minimum time between them.
    // the note's duration + 30% seems to work well:
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    // stop the tone playing:
    noTone(BEEP);
  }
//*/

  // Изчакване натискането на бутон
//  while (digitalRead (BUTT1) == HIGH) {
//    read_position();
//    sprintf(tmp_str, "%d \t %d \t %d \t %d \t %d \t %d",
//      sensor_values[0], sensor_values[1], sensor_values[2],
//      sensor_values[3], sensor_values[4], sensor_values[5]);
//    Serial.println(tmp_str);
//    delay(100);
  }

//}

void loop()
{


  char inp_char;
  // Има ли получен символ от сериен канал 4 ( Bluetooth модула)
  if (bluetooth.available()>0) {

    inp_char = bluetooth.read();
    Serial.write(inp_char);                                    // Ехо на получения символ на серийния монитор
    int FORWARD_SPEED = 20;
        if (inp_char == '0'){
      FORWARD_SPEED=0;}
    else if (inp_char == '1'){
      FORWARD_SPEED=60;}
    else if (inp_char == '2'){
      FORWARD_SPEED=60;}
    else if (inp_char == '3'){
      FORWARD_SPEED=60;}
    else if (inp_char == '4'){
      FORWARD_SPEED=120;}
    else if (inp_char == '5') {
      FORWARD_SPEED=120;}
    else if (inp_char == '6') {
      FORWARD_SPEED=120;}
    else if (inp_char == '7') {
      FORWARD_SPEED =180;}
    else if (inp_char == '8') {
      FORWARD_SPEED = 180;}
    else if (inp_char == '9') {
      FORWARD_SPEED == 255;}

  // Реакция на робота според получения символ
    switch (inp_char)
    {
    case  'F':                                                 // движение напред
      left_motor_speed(FORWARD_SPEED);
      right_motor_speed(FORWARD_SPEED);
      delay(100);
      break;
    case  'G':                                                 // движение напред и наляво / Диагонал
      left_motor_speed(FORWARD_SPEED);
      right_motor_speed(FORWARD_SPEED*0.75);
      delay(100);
      break;
    case  'I':                                                 // движение напред и надясно / Диагонал
      left_motor_speed(FORWARD_SPEED*0.75);
      right_motor_speed(FORWARD_SPEED);
      delay(100);
      break;
    case  'B':                                                 // движение назад
      left_motor_speed(-FORWARD_SPEED);
      right_motor_speed(-FORWARD_SPEED);
      delay(100);
      break;
    case  'H':                                                 // движение назад и наляво / Диагонал
      left_motor_speed(50);
      right_motor_speed(50);
      delay(100);
      break;
    case  'J':                                                 // движение назад и надясно / Диагонал
      left_motor_speed(-FORWARD_SPEED);
      right_motor_speed(-FORWARD_SPEED*0.75);
      delay(100);
      break;
    case  'R':                                                 // завой надясно
      left_motor_speed(80);
      right_motor_speed(0);
      delay(100);
      break;
    case  'L':                                                 // завой наляво
      left_motor_speed(0);
      right_motor_speed(80);
      delay(100);
      break;
    case  'S':                                                 // спиране
      left_motor_speed(-10);
      right_motor_speed(-10);
      delay(50);
      left_motor_speed(0);
      right_motor_speed(0);
      delay(100);
      break;
    }  // END Switch
  }  // END IF
}  //END LOOP
void left_motor_speed(signed int motor_speed) {
  if (motor_speed == 0)
    motor_speed = 1;

  if (motor_speed > 0) {
//    digitalWrite(LEFT_DIR, HIGH);
    PORTD  |= B00100000; // set LEFT_DIR on HIGH
  }
  else {
//    digitalWrite(LEFT_DIR, LOW);
    PORTD  &= B11011111; // set LEFT_DIR on LOW
  }
  motor_speed = abs(motor_speed);
  if (motor_speed > 255)
    motor_speed = 255;

  pwmWrite(LEFT_PWM, motor_speed);
}

//---------------------------------------
void right_motor_speed(signed int motor_speed) {
  if (motor_speed == 0)
    motor_speed = 1;

  if (motor_speed > 0) {
//    digitalWrite(RIGHT_DIR, HIGH);
    PORTD |= B01000000; // set RIGHT_DIR on HIGH
  }
  else {
//    digitalWrite(RIGHT_DIR, LOW);
    PORTD &= B10111111; // set RIGHT_DIR on LOW
  }
  motor_speed = abs(motor_speed);
  if (motor_speed > 255)
    motor_speed = 255;

  pwmWrite(RIGHT_PWM, motor_speed);
}