/*
 * ============================================================================
 *  Bare-Metal SG90 Servo Controller via UART
 *  Target: ATmega328P @ 16 MHz (Arduino Uno R3)
 * ============================================================================
 *
 *  WHAT IT DOES
 *  ------------
 *  A serial-controlled servo. Type an angle (0-180) into the serial
 *  monitor and press Enter; the servo on pin 9 moves to that position.
 *  Invalid input gets a friendly error message. The current angle is
 *  echoed back after each successful move.
 *
 *  Features:
 *    - Hardware Timer1 generates the 50 Hz servo PWM signal
 *      (precise, jitter-free, no CPU involvement after setup)
 *    - Interrupt-driven UART receive into a ring buffer, so input is
 *      never dropped even during long operations
 *    - Line-buffered parser: type freely, press Enter to commit
 *    - Backspace support for editing
 *
 *  HOW IT WORKS
 *  ------------
 *  Timer1 runs in Fast PWM mode 14 (TOP = ICR1) with prescaler 8,
 *  giving a tick rate of 2 MHz (0.5 us per tick). ICR1 = 40000 yields
 *  a 20 ms period (50 Hz). OCR1A controls the pulse width:
 *      2000 ticks = 1.0 ms = 0 degrees
 *      3000 ticks = 1.5 ms = 90 degrees
 *      4000 ticks = 2.0 ms = 180 degrees
 *  The hardware drives OC1A (pin 9) automatically -- no CPU work to
 *  hold position.
 *
 *  USART0 receives at 9600 baud. RX Complete fires USART_RX_vect on
 *  every byte; the ISR drops it into a 32-byte ring buffer. The main
 *  loop drains the buffer, builds a line, and parses it on Enter.
 *
 *  WIRING (SG90 servo -> Arduino Uno)
 *  ----------------------------------
 *      Brown  (GND)    -> GND
 *      Red    (VCC)    -> 5V
 *      Orange (Signal) -> D9 (PB1, OC1A)
 *
 *  USAGE
 *  -----
 *  Open the serial monitor at 9600 baud. Type an angle (e.g. "90") and
 *  press Enter. The servo moves; the controller reports the new angle.
 *
 * ============================================================================
 */

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

// ============================================================
// Servo (Timer1 Fast PWM on OC1A / pin 9)
// ============================================================

/* FOR SG90 Part
#define SERVO_MIN 2000   // ~1.0 ms pulse  -> 0 degrees
#define SERVO_MAX 4000   // ~2.0 ms pulse  -> 180 degrees
*/

/*Adjustment for WOKWI */
#define SERVO_MIN 1300
#define SERVO_MAX 4470

void servo_init(void) {
    DDRB |= (1 << DDB1);  // PB1 (pin 9) as output

    // Fast PWM, mode 14: WGM13:0 = 1110, TOP = ICR1
    // Non-inverting on OC1A: clear on compare match, set at BOTTOM
    TCCR1A = (1 << COM1A1) | (1 << WGM11);
    TCCR1B = (1 << WGM13)  | (1 << WGM12) | (1 << CS11);  // prescaler 8

    ICR1 = 40000;        // 20 ms period (50 Hz)
    OCR1A = 3000;        // start centered (90 degrees)
}

void servo_write_angle(uint8_t angle) {
    if (angle > 180) angle = 180;
    OCR1A = SERVO_MIN + ((uint32_t)(SERVO_MAX - SERVO_MIN) * angle) / 180;
}

// ============================================================
// UART with interrupt-driven RX ring buffer
// ============================================================

#define BAUD 9600
#define UBRR_VALUE ((F_CPU / (16UL * BAUD)) - 1)

#define RX_BUF_SIZE 32  // power of 2
static volatile uint8_t rx_buf[RX_BUF_SIZE];
static volatile uint8_t rx_head = 0;
static volatile uint8_t rx_tail = 0;

void uart_init(void) {
    UBRR0H = (uint8_t)(UBRR_VALUE >> 8);
    UBRR0L = (uint8_t)(UBRR_VALUE);
    UCSR0B = (1 << RXEN0) | (1 << TXEN0) | (1 << RXCIE0);
    UCSR0C = (1 << UCSZ01) | (1 << UCSZ00);
}

ISR(USART_RX_vect) {
    uint8_t c = UDR0;
    uint8_t next = (rx_head + 1) & (RX_BUF_SIZE - 1);
    if (next != rx_tail) {
        rx_buf[rx_head] = c;
        rx_head = next;
    }
}

void uart_tx(char c) {
    while (!(UCSR0A & (1 << UDRE0)));
    UDR0 = c;
}

void uart_print(const char *s) {
    while (*s) uart_tx(*s++);
}

void uart_print_u8(uint8_t v) {
    char buf[4];
    uint8_t i = 0;
    if (v == 0) { uart_tx('0'); return; }
    while (v) { buf[i++] = '0' + (v % 10); v /= 10; }
    while (i--) uart_tx(buf[i]);
}

uint8_t uart_available(void) {
    return rx_head != rx_tail;
}

char uart_rx(void) {
    while (rx_head == rx_tail);
    char c = rx_buf[rx_tail];
    rx_tail = (rx_tail + 1) & (RX_BUF_SIZE - 1);
    return c;
}

// ============================================================
// Line-buffered command parser
// ============================================================

#define LINE_BUF 8
static char line[LINE_BUF];
static uint8_t line_len = 0;

void process_line(void) {
    line[line_len] = '\0';

    if (line_len == 0) {
        uart_print("> ");
        return;
    }

    // Parse digits into an integer
    uint16_t angle = 0;
    uint8_t valid = 1;
    for (uint8_t i = 0; i < line_len; i++) {
        if (line[i] < '0' || line[i] > '9') { valid = 0; break; }
        angle = angle * 10 + (line[i] - '0');
    }

    if (!valid) {
        uart_print("Error: not a number\r\n> ");
    } else if (angle > 180) {
        uart_print("Error: out of range (0-180)\r\n> ");
    } else {
        servo_write_angle((uint8_t)angle);
        uart_print("Servo at ");
        uart_print_u8((uint8_t)angle);
        uart_print(" degrees\r\n> ");
    }

    line_len = 0;
}

void handle_char(char c) {
    if (c == '\r' || c == '\n') {
        uart_tx('\r'); uart_tx('\n');
        process_line();
    } else if (c == 0x7F || c == 0x08) {
        if (line_len > 0) {
            line_len--;
            // Erase visually in terminal
            uart_tx(0x08); uart_tx(' '); uart_tx(0x08);
        }
    } else if (c >= '0' && c <= '9') {
        if (line_len < LINE_BUF - 1) {
            line[line_len++] = c;
            uart_tx(c);
        }
    }
    // Ignore everything else
}

// ============================================================
// Main
// ============================================================

int main(void) {
    servo_init();
    uart_init();

    sei();

    _delay_ms(50);  // give the serial monitor time to attach

    uart_print("\r\n=== Bare-Metal Servo Controller ===\r\n");
    uart_print("Type angle 0-180 and press Enter\r\n> ");

    while (1) {
        while (uart_available()) {
            handle_char(uart_rx());
        }
    }
}