//https://docs.arduino.cc/hacking/hardware/PinMapping168
#include <Wire.h>                
#include <Adafruit_GFX.h>        
#include <Adafruit_SSD1306.h>    //Adafruit library

// These values are taken from wokwi website
// https://docs.wokwi.com/parts/wokwi-photoresistor-sensor
const float GAMMA = 0.7;
const float RL10 = 50;

// variables to read distance
volatile uint8_t CTC_count = 0;
volatile uint8_t timer_flag = 0;

// Define OLED display
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

uint16_t angle = 2999;
float diff;
float lux1;
float lux2;
int ldrVal1;
int ldrVal2;
float distance;

// Interrupt Service Routine for Sensor Reading
ISR(ADC_vect)   {
    if (ADCSRA &= ~(1 << ADSC)) {
        switch(ADMUX){
            case 0x40: { 
                ldrVal1 = ADC;
                ADMUX = 0x41;
                break;} 
            case 0x41: {
                ldrVal2= ADC;
                ADMUX = 0x40; 
                break;}}
                    }
    ADCSRA |= (1 << ADSC);
}

// Interrupt Service Routine of Timer1 PWM for Servo direction Adjust
ISR(TIMER1_COMPA_vect)
{
     OCR1A = angle;
}

// Interrupt Service Routine for Distance Calculation using Timer0
ISR(TIMER0_COMPA_vect)
{
    CTC_count++; // Increment Count
    timer_flag = 1; // Set the timer flag
}

// Function to calculate brightness
float ldr2brightness(int ldr)
{
    // These values are taken from wokwi website
// https://docs.wokwi.com/parts/wokwi-photoresistor-sensor
float voltage = ldr / 1024. * 5;
float resistance = 2000 * voltage / (1 - voltage / 5);
float lux = pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1 / GAMMA));
return lux;
}


void setup() {
    // To set OLED for display
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.display();
  delay(2000);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  
  // Pins for Ultrasonic Sensor
  DDRD |= (1 << PD5);
  DDRD &= ~(1 << PD6);
  // Pin for Switch ON OFF
  DDRB &= ~(1 << PB5);

  

  
  // Setup of Analog to Digital Converter
  // prescalar 128
  ADCSRA = (1 << ADEN) | (1 << ADIE) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); 
  ADCSRA |= (1 << ADSC); // Conversion start
  ADMUX = 0x40; 
  
  // Timer 1 operation for PWM to control servo - Mode 1 1 1 0
  TCCR1A = (1 << COM1A1); 
  TCCR0B = (1 << CS01) | (1 << CS00); // Set timer prescaler to 64
  TIMSK0 = (1 << OCIE0A); // Enable timer overflow interrupt
  OCR0A = 254;
  // Timer1 Setup for PWM generation for Servo
  TCCR1A = (1 << COM1A1) | (1 << WGM11); 
  TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); 
  ICR1 = 20000; 
  OCR1A = 2999;
  TIMSK1 = (1 << OCIE1A);
  DDRB |= (1 << PB1);
}

// Function to Get distance of obstacel
float getDistance(void){
        PORTD &= ~(1 << PD5); 
        _delay_us(2);
        PORTD |= (1 << PD5);
        _delay_us(10);
        PORTD &= ~(1 << PD5);

        // Wait for the echo
        while (!(PIND & (1 << PD6)));
        TCNT0 = 0; // Reset timer to 0
        timer_flag = 0;
        CTC_count = 0;
        

        while (PIND & (1 << PD6)); // wait echo to go low
        // Combine the CTC count and timer count to get distance
        uint16_t timer_count = (CTC_count << 8) | TCNT0; 
        float distance = ((float)timer_count * 0.068); 
        return distance;
}
void loop() {
    // Check if Switch is enabled
    if(PINB & (1 << PB5))
    {
        // Read lux values
   lux1 = ldr2brightness(ldrVal1);
   lux2 = ldr2brightness(ldrVal2);
   distance = getDistance();
    // Check if obstacle is close stop otherwise move
   if(distance > 50)
   {
   if(lux1 > lux2)
   {
   diff = lux1 - lux2;
   if(diff > 100)
   {
       display.setCursor(0, 0);
       display.clearDisplay();
       display.println("Right Turn");
       
   angle += 100;
   }
   }
   else
   {
   diff = lux2 - lux1;
   if(diff > 100)
   {
       display.setCursor(0, 0);
       display.clearDisplay();
       display.println("Left Turn");
   angle -= 100;
   }
   }

   if (diff < 100)
   {
       display.setCursor(0, 0);
       display.clearDisplay();
       display.println("Forward");
       angle = 2999;
   }

   if (angle < 999) {
        angle = 999;
      } else if (angle > 4999) {
        angle = 4999;
      }
   }
   else
   {
       display.setCursor(0, 0);
       display.clearDisplay();
       display.println("Stop");
   }
      display.display();

    }
    else
    {
      display.setCursor(0, 0);
       display.clearDisplay();
       display.println("OFF");  
       display.display();
    }
      
        

}