/*
 * semestralny_projekt.c
 * zadanie č. 74: Path following robot
 *  Author: Andrea Kuňáková
 */ 

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#define IR_PIN_1 PB0 // S1 - uplne na pravo (pin D8)
#define IR_PIN_2 PD4 // S2 (pin D4)
#define IR_PIN_3 PD5 // S3 (pin D5)
#define IR_PIN_4 PD6 // S4 (pin D6)
#define IR_PIN_5 PD7 // S5 (pin D7)
#define IR_PIN_6 PB3 // S6 (pin D11)

#define SERVO_PIN_1 PB1 // lave servo pripojene na pin D9
#define SERVO_PIN_2 PB2 // prave servo pripojene na pin D10

#define TRIGGER_PIN PD2 // (pin D2)
#define ECHO_PIN PD3  // (pin D3)

#define OBSTACLE_DISTANCE_THRESHOLD 3 // vzdialenost prekazky od robota pri ktorej sa jej zacne vyhybat

volatile uint16_t sensorValues[6];

void servo_init() 
{
  // Konfiguracia Timer1 pre Fast PWM mode, 8-bit
  TCCR1A |= (1 << WGM10) | (1 << WGM11) | (1 << COM1A1); 
  TCCR1A |= (1 << WGM20) | (1 << WGM21) | (1 << COM1B1); 
  TCCR1B |= (1 << CS11); // Prescaler of 8

  // Nastavenie OC1A/PB1 a OC1B/PB2 na output
  DDRB |= (1 << SERVO_PIN_1) | (1 << SERVO_PIN_2);
}

void servo_rotate(uint8_t servo, uint8_t angle) 
{
  // Mapovanie uhla (od 0 do 180 stupnov) na PWM hodnotu (od 0 do 255)
  uint8_t pwm_value = (angle * 255) / 360;
  // Nastavenie PWM hodnoty na specificke servo
  if (servo == 1) 
  {
    OCR1A = -pwm_value; // Servo 1
  } 
  else if (servo == 2) 
  {
    OCR1B = pwm_value; // Servo 2
  }
}

void setup() 
{
  servo_init();

  // Nastavenie IR pins na inputs
  DDRD &= ~((1 << IR_PIN_2) | (1 << IR_PIN_3) | (1 << IR_PIN_4) | (1 << IR_PIN_5) | (1 << ECHO_PIN));
  DDRB &= ~((1 << IR_PIN_1) | (1 << IR_PIN_6));
  DDRD |= (1 << TRIGGER_PIN);
  // Povolenie prerusenia pre IR piny
  PCICR |= (1 << PCIE2) | (1 << PCIE0); // Povolenie PCIE2 pre piny od PCINT16 do PCINT23 a PCIE0 pre piny od PCINT0 do PCINT7
  PCMSK2 |= (1 << PCINT20) | (1 << PCINT21) | (1 << PCINT22) | (1 << PCINT23); // Povolenie preruseni pre PD4 az PD7
  PCMSK0 |= (1 << PCINT0) | (1 << PCINT3); // Povolenie preruseni pre PB0 a PB3
  sei();
}

ISR(PCINT2_vect) 
{
  // ISR pre PCINT2 (IR piny od 1 po 6)
  sensorValues[0] = digitalRead(IR_PIN_1);
  sensorValues[1] = digitalRead(IR_PIN_2);
  sensorValues[2] = digitalRead(IR_PIN_3);
  sensorValues[3] = digitalRead(IR_PIN_4);
  sensorValues[4] = digitalRead(IR_PIN_5);
  sensorValues[5] = digitalRead(IR_PIN_6);
}

void loop() 
{
  // pohyb
  if (sensorValues[2] == LOW && sensorValues[3] == LOW) 
  { 
    // dopredu
    servo_rotate(1, 90);
    servo_rotate(2, 90);
    
  }
  if(sensorValues[4] == LOW || sensorValues[3] == LOW)
  {
    // do lava
    servo_rotate(1, 10); 
    servo_rotate(2, 0); 
  }
  if(sensorValues[2] == LOW || sensorValues[1] == LOW)
  {
    // do prava
    servo_rotate(1, 0); 
    servo_rotate(2, 10); 
  }
  if(sensorValues[4] == LOW || sensorValues[5] == LOW)
  {
    // do prava
    servo_rotate(1, 10); 
    servo_rotate(2, 0); 
  }
  if(sensorValues[0] == LOW || sensorValues[1] == LOW)
  {
    // do prava
    servo_rotate(1, 0); 
    servo_rotate(2, 10); 
  }

  while (check_obstacle() == HIGH) 
  {
    servo_rotate(2, 179); 
    servo_rotate(1,179);  
  }

   delay(10);
 }

  
// Funkcia na kontrolu prekazok pomocou HC-SR04
bool check_obstacle() 
{
  // Generate trigger pulse for HC-SR04
  PORTD |= (1 << TRIGGER_PIN);
  _delay_us(10);
  PORTD &= ~(1 << TRIGGER_PIN);

  // Zmeranie sirky ozveny z echo pinu 
  uint32_t pulse_width = 0;
  while (!(PIND & (1 << ECHO_PIN))); // Cakanie na nabeznu hranu
  TCNT1 = 0; // Resetovanie casovaca
  while (PIND & (1 << ECHO_PIN)) 
  { // Cakanie na dobeznu hranu
    if (TCNT1 > 23200) // Maximalna povolena sirka impulzu (400cm)
      return false; // Ziadna prekazka nebola zaznamenana
  }
  pulse_width = TCNT1; // Ziskaj sirku impulzu

  // Premena sirky impulzu na vzdialenost v cm
  uint16_t distance = pulse_width / 58;

  // Kontrola ci prekazka je aspom 3cm od robota
  if (distance >= OBSTACLE_DISTANCE_THRESHOLD)
    return false; // V prahovej vzdialenosti nebola zistena ziadna prekazka

  return true; // Zistena prekazka
}