#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Encoder.h>

// Define pins for LED and potentiometers
const int led1Pin = 2;
const int led2Pin = 3;
const int potPin1 = A0; // Potentiometer 1 for on time
const int potPin2 = A1; // Potentiometer 2 for off sm
const int potPin3 = A3; // Potentiometer 2 for off km

// Define pins for encoder
const int encPinA = 4;  // Encoder pin A
const int encPinB = 5;  // Encoder pin B

// Variables for on and off times (in milliseconds)
volatile uint8_t time1 = 24; // Default 1 second on time
volatile uint8_t time2 = 24; // Default 1 second off time

volatile uint8_t phase = 0; // Phase: 0=LED1 on, 1=off, 2=LED2 on, 3=off

volatile uint16_t msCounter = 0;

float cm = 6.0;
float kmh = 2.0;

// Encoder
Encoder myEnc(encPinA, encPinB);
long encoderPosition  = 0;
long prevEncoderPosition = -999;

// SSD1306 Display
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1 // Reset pin not used
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  // Initialize LED pins as outputs
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  // Initialize potentiometer pins as input
  pinMode(potPin1, INPUT);
  pinMode(potPin2, INPUT);
  pinMode(potPin3, INPUT);

  // Set initial LED states
  digitalWrite(led1Pin, LOW);
  digitalWrite(led2Pin, LOW);

  // Initialize Timer1 for a 1 ms (1000 Hz) interrupt
  noInterrupts(); // Disable all interrupts
  TCCR1A = 0;     // Set entire TCCR1A register to 0
  TCCR1B = 0;     // Same for TCCR1B
  TCNT1 = 0;      // Initialize counter value to 0
  OCR1A = 15999;  // Compare match register for 1 ms increments (16MHz/64/1000 - 1)
  TCCR1B |= (1 << WGM12);   // Turn on CTC mode
  TCCR1B |= (1 << CS11) | (1 << CS10); // Set CS11 and CS10 bits for 64 prescaler
  TIMSK1 |= (1 << OCIE1A);  // Enable timer compare interrupt
  interrupts(); // Enable all interrupts

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for SSD1306
    Serial.println(F("SSD1309 allocation failed"));
    for (;;);
  }
  display.display();
  delay(1000);
  display.clearDisplay();

  // Initialize encoder position
  myEnc.write(0);
}

ISR(TIMER1_COMPA_vect) {
  // Increment counter
  msCounter++;

  // Handle phase changes
  if (phase == 0 && msCounter >= time1) {
    // Phase 0: LED1 on, switch to off
    digitalWrite(led1Pin, LOW);
    phase = 1;
    msCounter = 0;
  } else if (phase == 1 && msCounter >= time2 / 2) {
    // Phase 1: Half of off period, switch to LED2 on
    digitalWrite(led2Pin, HIGH);
    phase = 2;
    msCounter = 0;
  } else if (phase == 2 && msCounter >= time1) {
    // Phase 2: LED2 on, switch to off
    digitalWrite(led2Pin, LOW);
    phase = 3;
    msCounter = 0;
  } else if (phase == 3 && msCounter >= time2 / 2) {
    // Phase 3: Half of off period, switch to LED1 on
    digitalWrite(led1Pin, HIGH);
    phase = 0;
    msCounter = 0;
  }
}

void loop() {
  // Read the potentiometer values (0 - 1023)
  int potValue1 = analogRead(potPin1);
  int potValue2 = analogRead(potPin2);
  int potValue3 = analogRead(potPin3);

  // Map the potentiometer values to useful ranges for on and off times
  time1 = map(potValue1, 0, 1023, 8, 60);  // On time (100 - 2000 ms)
  // time2 = map(potValue2, 0, 1023, 1, 20);  // Off time (100 - 2000 ms)
  float cm = map(potValue2, 0, 1023, 5, 16);  // Off time (100 - 2000 ms)
  float kmh = map(potValue3, 0, 1023, 1, 8);  // Off time (100 - 2000 ms)


  // Read encoder position
  encoderPosition = myEnc.read();
  if (prevEncoderPosition != encoderPosition) {
    prevEncoderPosition = encoderPosition;
    // Modify some functionality based on encoder, this is just a placeholder
    // You can add meaningful functionality here.
  }
 time2 = ((cm / (kmh / (36 / 2))) - time1);
 
  // Display the timing values on the OLED display
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("On:  ");
  display.print(time1);
  display.print(" ms");
  display.setCursor(0, 10);
  display.print("Off: ");
  display.print(time2);
  display.print(" ms");
  display.setCursor(0, 20);
  display.print("Sm:  ");
  display.print(cm);
  display.setCursor(0, 30);
  display.print("Kmh: ");
  display.print(kmh);
  display.display();

  // Small delay to prevent too frequent updates
  delay(100);
}