#include <xc.h>
#include <stdint.h>
#include <pic16f676.h>

// CONFIGURATION
#pragma config FOSC = INTRCIO   // Internal oscillator
#pragma config WDTE = OFF       // Watchdog timer off
#pragma config PWRTE = ON       // Power-up timer enabled
#pragma config MCLRE = OFF
#define _XTAL_FREQ 4000000

volatile uint8_t current_button_status = 1;
volatile uint8_t current_mode = 0;
volatile uint8_t servo_pos = 127; // 0=1ms, 255=2ms
volatile uint8_t pulse_state = 0; // 0=start pulse, 1=end pulse
volatile uint8_t isincreasing = 0;

void __interrupt() ISR(void) {
    INTCONbits.GIE = 0;
    if (PIR1bits.TMR1IF) {
        PIR1bits.TMR1IF = 0;    // Clear interrupt flag
        static uint16_t off_ticks; // Retains value between interrupts
        static uint16_t timer;
        if (pulse_state == 0) {
            // Calculate pulse parameters
            uint16_t on_ticks = 75 + ((uint16_t)servo_pos * 237) / 255; // Map to 0.5ms-2.5ms
            off_ticks = 2500 - on_ticks; // 2500 ticks = 20ms            
            RC5 = 1;            // Start pulse
            timer = 65535 - on_ticks;
            TMR1L = (unsigned char)(timer & 0xFF);
            TMR1H = (unsigned char)(timer >> 8);
            pulse_state = 1;
        } else {
            RC5 = 0;            // End pulse
            timer = 65535 - off_ticks;
            TMR1L = (unsigned char)(timer & 0xFF);
            TMR1H = (unsigned char)(timer >> 8);
            pulse_state = 0;
        }
    }
    INTCONbits.GIE = 1;
}

void setup() {
    
    // Set RA1 as digital input
    ANSEL = 0;
    CMCON = 0x07;
    TRISC = 0x00; 
    TRISAbits.TRISA1 = 1;    
    WPUAbits.WPUA1 = 1;        
    OPTION_REGbits.nRAPU = 0;
    
    // Configure Timer1
    T1CON = 0b00110000; // Timer1: prescaler 1:8, internal clock, OFF
    TMR1H = (65536 - 188) >> 8; // Initial 1.5ms pulse (high byte)
    TMR1L = (65536 - 188) & 0xFF; // Initial 1.5ms pulse (low byte)
    PIR1bits.TMR1IF = 0; // Clear interrupt flag
    PIE1bits.TMR1IE = 1; // Enable Timer1 interrupt
    INTCONbits.PEIE = 1; // Enable peripheral interrupts
    INTCONbits.GIE = 1;  // Enable global interrupts
    T1CONbits.TMR1ON = 1;
    
    // Configure ADC
    TRISAbits.TRISA2 = 0;    
    ANSELbits.ANS2 = 1;
    ADCON0 = 0b00001001;
    ADCON1 = 0;   
}

#define bool uint8_t
#define false 0
#define true 1

void UpdateLeds()
{
    RC0 = current_mode == 0;
    RC1 = current_mode == 1;
    RC2 = current_mode == 2;
}

void check_button() {
    if (!RA1 && current_button_status) {
        __delay_ms(20);             
        if(!RA1) {            
            current_mode = (current_mode + 1) % 3; // Cycle through modes        
            UpdateLeds();        
        }
    }
    current_button_status = RA1;
}

void smooth_servo(uint8_t target_pos) {
    // Move servo_pos towards target_pos gradually
    if (servo_pos < target_pos) {
        servo_pos++;
    } else if (servo_pos > target_pos) {
        servo_pos--;
    }
}

inline uint8_t read_adc() {
    ADCON0bits.GO = 1;
    while(ADCON0bits.GO_DONE); 
    return ADRESH;
}

void main() {
    setup();  // Initialize peripherals
    UpdateLeds();    
    while(1) {
        check_button();  // Check for button presses              
        switch(current_mode) {
            case 0:                
                smooth_servo(read_adc());
                __delay_us(100);
                break;
            case 1:
                //target_pos = 511;
                smooth_servo(127);
                __delay_us(100);
                break;
            case 2:
                if(isincreasing) {                                            
                    smooth_servo(servo_pos + 1);
                     __delay_ms(10);
                    if(servo_pos + 1 >= 254) isincreasing = 0;
                } else
                {
                    smooth_servo(servo_pos - 1);
                     __delay_ms(10);
                    if(servo_pos - 1  <= 0) isincreasing = 1;
                }
                break;
        }
    }
}