#define TIMER1_TOP 320 /* 10-bit PWM */
#define TIMER1_BOTTOM 0 /* 10-bit PWM */
#define Servo_TOP 301 /* 10-bit PWM */
#define Servo_BOTTOM 68 /* 10-bit PWM */
#define Servo_Delay 10
#define BIT1_MASK 0x02 // 0000 0010 --> Pin 0
#define PORTB_MASK 0x25
#define DDRB_MASK 0x24
#define PINB_MASK 0x23

// Switch Pin Definitions
#define SWITCH1_MASK 0x01 // A0 (PC0)
#define SWITCH2_MASK 0x02 // A1 (PC1)

// Register Pointers
unsigned char *SREG_ESP;
unsigned char *portDDRB;
unsigned char *portPinB;
unsigned char *portB;
unsigned char *TCCR1A_ESP;
unsigned char *TCCR1B_ESP;
unsigned char *TCNT1H_ESP;
unsigned char *TCNT1L_ESP;
unsigned char *OCR1AH_ESP;
unsigned char *OCR1AL_ESP;
unsigned char *TIMSK1_ESP;

unsigned char *DDRC_ESP = (unsigned char *)0x27;  // Data Direction Register for Port C
unsigned char *PORTC_ESP = (unsigned char *)0x28; // Data Register for Port C
unsigned char *PINC_ESP = (unsigned char *)0x26; // Input Pins Register for Port C

// Global Variables
volatile bool servo_active = true;     // Initially, the servo is active
volatile bool switches_on_state = false; // Tracks if switches are ON
volatile uint16_t duty_cycle = 184;    // Default: 90 degrees
volatile uint8_t direction = 0;        // 0 = UP, 1 = DOWN

// Enum for Servo Movement Direction
enum { UP, DOWN };

ISR(TIMER1_OVF_vect) {
    if (servo_active) {
        // Update duty cycle if servo is active
        switch (direction) {
        case UP:
            if (++duty_cycle >= Servo_TOP + Servo_Delay) {
                direction = DOWN; // Switch direction to DOWN
            }
            break;
        case DOWN:
            if (--duty_cycle <= Servo_BOTTOM - Servo_Delay) {
                direction = UP; // Switch direction to UP
            }
            break;
        }
    }

    // Update OCR1A with the current duty cycle
    *OCR1AH_ESP = (duty_cycle >> 8) & 0xFF; // High byte
    *OCR1AL_ESP = duty_cycle & 0xFF;        // Low byte
}

void setup() {
    // Serial Library Initialization
    Serial.begin(9600); // Open the serial port at 9600 bps

    // Register Pointer Initialization
    SREG_ESP = (unsigned char *)0x5F;
    TCCR1A_ESP = (unsigned char *)0x80;
    TCCR1B_ESP = (unsigned char *)0x81;
    TCNT1H_ESP = (unsigned char *)0x85;
    TCNT1L_ESP = (unsigned char *)0x84;
    OCR1AH_ESP = (unsigned char *)0x89;
    OCR1AL_ESP = (unsigned char *)0x88;
    TIMSK1_ESP = (unsigned char *)0x6F;
    portDDRB = (unsigned char *)DDRB_MASK;
    portB = (unsigned char *)PORTB_MASK;

    // Configure Timer1 for Phase Correct PWM, 10-bit
    *TCCR1A_ESP = 0x82; // COM1A1 = 1 (Clear on Compare Match), WGM11:10 = 1:1
    *TCCR1B_ESP = 0x1A; // WGM13:12 = 0:0, CS11:10 = 1:1 (Prescaler clk/64)

    // Set Duty Cycle (Default: 90 degrees)
    duty_cycle = 184; // Between Servo_BOTTOM (68) and Servo_TOP (301)
    *OCR1AH_ESP = (duty_cycle >> 8) & 0xFF; // High byte
    *OCR1AL_ESP = duty_cycle & 0xFF;        // Low byte

    // Enable Timer1 Overflow Interrupt
    *TIMSK1_ESP = 0x01; // Enable Overflow Interrupt (TOIE1)

    // Configure OC1A (PB1) as Output
    *portDDRB |= BIT1_MASK; // Set PB1 as output

    // Configure Switches on A0 (PC0) and A1 (PC1)
    *DDRC_ESP &= ~(SWITCH1_MASK | SWITCH2_MASK); // Set PC0 and PC1 as inputs
    *PORTC_ESP |= (SWITCH1_MASK | SWITCH2_MASK); // Enable pull-up resistors

    // Enable Global Interrupts
    *SREG_ESP = 0x80; // Set I-bit in SREG
}

int main(void) {
    setup();

    while (1) {
        // Check if both switches are in the ON state (LOW)
        if (!(*PINC_ESP & SWITCH1_MASK) && !(*PINC_ESP & SWITCH2_MASK)) {
            if (!switches_on_state) { // Only toggle if switches weren't already ON
                switches_on_state = true; // Mark switches as ON
                servo_active = !servo_active; // Toggle servo state
            }
        } else {
            switches_on_state = false; // Reset switches_on_state if switches are OFF
        }
    }

    return 0;
}