/*
 * ============================================================================
 * MPPT SOLAR CHARGE CONTROLLER - ATtiny85
 * ============================================================================
 * Versión: 2.0 Corregida
 * Frecuencia PWM: 22 kHz
 * Compilador: AVR-GCC / Arduino IDE
 * 
 * PINOUT ATtiny85 (Sin conflictos):
 * Pin 1 (PB5/ADC0):  Voltaje Batería
 * Pin 2 (PB3):       OLED SCL (I2C Software)
 * Pin 3 (PB4/ADC2):  Voltaje Panel Solar
 * Pin 4 (GND):       Ground
 * Pin 5 (PB0):       OLED SDA (I2C Software)  
 * Pin 6 (PB1/OC1A):  PWM Salida Buck
 * Pin 7 (PB2/ADC1):  Corriente ACS712
 * Pin 8 (VCC):       5V
 * ============================================================================
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <stdint.h>
#include <stdlib.h>

/* ============================================================================
 * CONFIGURACIÓN DEL SISTEMA
 * ============================================================================ */
#define F_CPU           8000000UL

/* Canales ADC - CORREGIDO: Sin conflicto de pines */
#define ADC_BATTERY     0       // PB5/ADC0 - Voltaje Batería
#define ADC_CURRENT     1       // PB2/ADC1 - Corriente ACS712
#define ADC_PANEL       2       // PB4/ADC2 - Voltaje Panel

/* Pines I2C Software - CORREGIDO: PB0 y PB3, no interfieren con ADC */
#define OLED_SDA        PB0     // Pin 5 - SDA I2C
#define OLED_SCL        PB3     // Pin 2 - SCL I2C

/* Pin PWM */
#define PWM_PIN         PB1     // Pin 6 - OC1A

/* ============================================================================
 * PARÁMETROS DEL SISTEMA
 * ============================================================================ */
#define VREF_MV         5000
#define ADC_MAX         1023

/* Escalado de voltaje (ajustar según divisor resistivo real) */
#define VBAT_SCALE_NUM  1304    // Numerador factor batería
#define VBAT_SCALE_DEN  1000    // Denominador
#define VPANEL_SCALE_NUM 1304   // Numerador factor panel
#define VPANEL_SCALE_DEN 1000   // Denominador

/* ACS712-20A: 100mV/A, punto cero a 2.5V */
#define I_SCALE_MA      49      // mA por paso ADC (49mA/paso)
#define ADC_ZERO        512     // Punto cero (2.5V en 5V/1024)

/* PWM 22 kHz */
#define PWM_TOP         181     // Para 22kHz con prescaler 2
#define MAX_DUTY        175     // ~96% máximo
#define MIN_DUTY        15      // ~8% mínimo

/* Algoritmo MPPT */
#define MPPT_STEP       2
#define MPPT_DEADBAND   100     // mW
#define MPPT_INTERVAL_MS 1000

/* Límites de carga */
#define V_FLOAT         1280    // mV - Voltaje flotación
#define V_MAX           1600    // mV - Máximo absoluto
#define V_MIN           1000    // mV - Mínimo batería
#define I_MAX_MA        1500    // mA - Corriente máxima

/* Filtro ADC */
#define FILTER_SAMPLES  16

/* ============================================================================
 * VARIABLES GLOBALES - DECLARADAS CORRECTAMENTE
 * ============================================================================ */
static volatile uint16_t v_battery_mv = 0;
static volatile uint16_t v_panel_mv = 0;
static volatile int16_t current_ma = 0;
static volatile uint16_t power_mw = 0;
static uint8_t duty_cycle = 90;

static volatile uint8_t mppt_update_flag = 0;
static volatile uint8_t display_update_flag = 0;

static uint16_t last_power = 0;
static int8_t mppt_direction = 1;

static char disp_buffer[24];

/* Filtros */
static uint16_t filter_bat = 0;
static uint16_t filter_panel = 0;
static int16_t filter_current = 0;

/* ============================================================================
 * STRINGS EN PROGMEM
 * ============================================================================ */
static const char str_bat[] PROGMEM = "BAT:";
static const char str_sol[] PROGMEM = "SOL:";
static const char str_cur[] PROGMEM = "I:";
static const char str_pow[] PROGMEM = "P:";
static const char str_v[] PROGMEM = "V";
static const char str_a[] PROGMEM = "A";
static const char str_w[] PROGMEM = "W";

/* ============================================================================
 * PROTOTIPOS DE FUNCIONES
 * ============================================================================ */
static void init_system(void);
static void init_adc(void);
static void init_pwm(void);
static void init_timer(void);
static void init_wdt(void);

static uint16_t read_adc(uint8_t channel);
static void read_sensors(void);
static void mppt_algorithm(void);
static void charge_control(void);

static void update_pwm(uint8_t duty);
static uint8_t slew_rate_limit(uint8_t current, uint8_t target);

static void i2c_init(void);
static void i2c_start(void);
static void i2c_stop(void);
static void i2c_write(uint8_t data);

static void oled_init(void);
static void oled_cmd(uint8_t cmd);
static void oled_data(uint8_t data);
static void oled_pos(uint8_t page, uint8_t col);
static void oled_clear(void);
static void oled_char(uint8_t c);
static void oled_string(const char* str);
static void oled_string_p(const char* pstr);

static void format_number(uint16_t num, uint8_t div, char* out);
static void update_display(void);

/* ============================================================================
 * INICIALIZACIÓN
 * ============================================================================ */
static void init_system(void) {
    // Configurar pines
    DDRB = (1 << PWM_PIN) | (1 << OLED_SDA) | (1 << OLED_SCL);
    PORTB = (1 << OLED_SDA) | (1 << OLED_SCL);
    
    cli();
    init_wdt();
    init_adc();
    init_pwm();
    init_timer();
    sei();
    
    _delay_ms(100);
    i2c_init();
    oled_init();
    oled_clear();
}

static void init_wdt(void) {
    wdt_reset();
    MCUSR &= ~(1 << WDRF);
    WDTCR = (1 << WDCE) | (1 << WDE);
    WDTCR = (1 << WDP2) | (1 << WDP1) | (1 << WDE); // 1 segundo
}

static void init_adc(void) {
    ADMUX = (1 << REFS0); // VCC como referencia
    ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // Prescaler 128
    DIDR0 = (1 << ADC0D) | (1 << ADC1D) | (1 << ADC2D);
}

static void init_pwm(void) {
    // Timer1: Fast PWM, prescaler 2, TOP en OCR1C
    PLLCSR = 0;
    TCCR1 = (1 << PWM1A) | (1 << COM1A1) | (1 << CS11);
    OCR1C = PWM_TOP;
    OCR1A = duty_cycle;
    GTCCR = 0;
}

static void init_timer(void) {
    // Timer0: CTC, 1ms tick
    TCCR0A = (1 << WGM01);
    TCCR0B = (1 << CS01) | (1 << CS00); // Prescaler 64
    OCR0A = 125; // 1ms @ 8MHz
    TIMSK |= (1 << OCIE0A);
}

/* ============================================================================
 * ADC Y SENSORES
 * ============================================================================ */
static uint16_t read_adc(uint8_t channel) {
    ADMUX = (ADMUX & 0xF0) | (channel & 0x03);
    _delay_us(20);
    ADCSRA |= (1 << ADSC);
    while (ADCSRA & (1 << ADSC));
    return ADC;
}

static void read_sensors(void) {
    uint32_t accum;
    uint16_t reading;
    int16_t current_raw;
    
    // Leer batería con promedio de 16 muestras
    accum = 0;
    for (uint8_t i = 0; i < FILTER_SAMPLES; i++) {
        accum += read_adc(ADC_BATTERY);
    }
    reading = accum / FILTER_SAMPLES;
    v_battery_mv = ((uint32_t)reading * VREF_MV * VBAT_SCALE_NUM) / 
                   (ADC_MAX * VBAT_SCALE_DEN);
    
    // Suavizado exponencial
    if (filter_bat == 0) filter_bat = v_battery_mv;
    filter_bat = (filter_bat * 3 + v_battery_mv) / 4;
    v_battery_mv = filter_bat;
    
    // Leer panel
    accum = 0;
    for (uint8_t i = 0; i < FILTER_SAMPLES; i++) {
        accum += read_adc(ADC_PANEL);
    }
    reading = accum / FILTER_SAMPLES;
    v_panel_mv = ((uint32_t)reading * VREF_MV * VPANEL_SCALE_NUM) / 
                 (ADC_MAX * VPANEL_SCALE_DEN);
    
    if (filter_panel == 0) filter_panel = v_panel_mv;
    filter_panel = (filter_panel * 3 + v_panel_mv) / 4;
    v_panel_mv = filter_panel;
    
    // Leer corriente
    accum = 0;
    for (uint8_t i = 0; i < FILTER_SAMPLES; i++) {
        accum += read_adc(ADC_CURRENT);
    }
    reading = accum / FILTER_SAMPLES;
    current_raw = (int16_t)reading - ADC_ZERO;
    current_ma = current_raw * I_SCALE_MA;
    
    if (filter_current == 0) filter_current = current_ma;
    filter_current = (filter_current * 3 + current_ma) / 4;
    current_ma = filter_current;
    
    // Calcular potencia
    if (current_ma > 0) {
        power_mw = ((uint32_t)v_battery_mv * (uint32_t)current_ma) / 1000;
    } else {
        power_mw = 0;
    }
}

/* ============================================================================
 * CONTROL PWM CON LIMITACIÓN DE SLEW RATE
 * ============================================================================ */
static void update_pwm(uint8_t duty) {
    if (duty > MAX_DUTY) duty = MAX_DUTY;
    if (duty < MIN_DUTY) duty = MIN_DUTY;
    OCR1A = duty;
}

static uint8_t slew_rate_limit(uint8_t current, uint8_t target) {
    int16_t diff = (int16_t)target - (int16_t)current;
    if (diff > 5) diff = 5;
    if (diff < -5) diff = -5;
    return current + (uint8_t)diff;
}

/* ============================================================================
 * ALGORITMO MPPT (Perturb & Observe)
 * ============================================================================ */
static void mppt_algorithm(void) {
    int16_t power_delta = (int16_t)power_mw - (int16_t)last_power;
    uint8_t target_duty = duty_cycle;
    
    if (abs(power_delta) > MPPT_DEADBAND) {
        if (power_delta < 0) {
            mppt_direction = -mppt_direction;
        }
        
        if (mppt_direction > 0) {
            if (target_duty < MAX_DUTY) {
                target_duty += MPPT_STEP;
            }
        } else {
            if (target_duty > MIN_DUTY) {
                target_duty -= MPPT_STEP;
            }
        }
    }
    
    // Aplicar slew rate limit para suavidad
    duty_cycle = slew_rate_limit(duty_cycle, target_duty);
    last_power = power_mw;
}

/* ============================================================================
 * CONTROL DE CARGA Y PROTECCIONES
 * ============================================================================ */
static void charge_control(void) {
    uint8_t target = duty_cycle;
    
    // Protección sobre-voltaje
    if (v_battery_mv >= V_MAX) {
        target = MIN_DUTY;
    }
    // Protección bajo-voltaje batería
    else if (v_battery_mv < V_MIN) {
        target = MIN_DUTY;
    }
    // Protección sobre-corriente
    else if (current_ma > I_MAX_MA) {
        target = slew_rate_limit(duty_cycle, duty_cycle - 5);
    }
    // Modo flotación
    else if (v_battery_mv >= V_FLOAT) {
        target = slew_rate_limit(duty_cycle, duty_cycle - 1);
    }
    // Panel no genera suficiente
    else if (v_panel_mv < v_battery_mv + 500) {
        target = MIN_DUTY;
    }
    else {
        // Operación normal MPPT
        mppt_algorithm();
        return; // mppt_algorithm ya actualiza duty_cycle
    }
    
    duty_cycle = target;
    update_pwm(duty_cycle);
}

/* ============================================================================
 * I2C SOFTWARE
 * ============================================================================ */
#define SDA_HIGH()    (PORTB |= (1 << OLED_SDA))
#define SDA_LOW()     (PORTB &= ~(1 << OLED_SDA))
#define SCL_HIGH()    (PORTB |= (1 << OLED_SCL))
#define SCL_LOW()     (PORTB &= ~(1 << OLED_SCL))
#define SDA_READ()    (PINB & (1 << OLED_SDA))

static void i2c_init(void) {
    DDRB |= (1 << OLED_SDA) | (1 << OLED_SCL);
    PORTB |= (1 << OLED_SDA) | (1 << OLED_SCL);
}

static void i2c_start(void) {
    SDA_HIGH();
    SCL_HIGH();
    _delay_us(5);
    SDA_LOW();
    _delay_us(5);
    SCL_LOW();
}

static void i2c_stop(void) {
    SDA_LOW();
    _delay_us(3);
    SCL_HIGH();
    _delay_us(5);
    SDA_HIGH();
    _delay_us(5);
}

static void i2c_write(uint8_t data) {
    for (uint8_t i = 0; i < 8; i++) {
        if (data & 0x80) SDA_HIGH();
        else SDA_LOW();
        _delay_us(2);
        SCL_HIGH();
        _delay_us(3);
        SCL_LOW();
        _delay_us(2);
        data <<= 1;
    }
    SDA_HIGH();
    _delay_us(2);
    SCL_HIGH();
    _delay_us(3);
    SCL_LOW();
    _delay_us(2);
}

/* ============================================================================
 * OLED SSD1306
 * ============================================================================ */
#define OLED_ADDR   0x78

static void oled_cmd(uint8_t cmd) {
    i2c_start();
    i2c_write(OLED_ADDR);
    i2c_write(0x00);
    i2c_write(cmd);
    i2c_stop();
    _delay_us(10);
}

static void oled_data(uint8_t data) {
    i2c_start();
    i2c_write(OLED_ADDR);
    i2c_write(0x40);
    i2c_write(data);
    i2c_stop();
}

static void oled_pos(uint8_t page, uint8_t col) {
    oled_cmd(0xB0 + page);
    oled_cmd(col & 0x0F);
    oled_cmd(0x10 | (col >> 4));
}

static void oled_clear(void) {
    for (uint8_t p = 0; p < 8; p++) {
        oled_pos(p, 0);
        for (uint8_t c = 0; c < 128; c++) {
            oled_data(0x00);
        }
    }
}

static void oled_init(void) {
    _delay_ms(100);
    oled_cmd(0xAE);
    oled_cmd(0xD5); oled_cmd(0x80);
    oled_cmd(0xA8); oled_cmd(0x3F);
    oled_cmd(0xD3); oled_cmd(0x00);
    oled_cmd(0x40);
    oled_cmd(0x8D); oled_cmd(0x14);
    oled_cmd(0x20); oled_cmd(0x00);
    oled_cmd(0xA1);
    oled_cmd(0xC8);
    oled_cmd(0xDA); oled_cmd(0x12);
    oled_cmd(0x81); oled_cmd(0x7F);
    oled_cmd(0xD9); oled_cmd(0xF1);
    oled_cmd(0xDB); oled_cmd(0x40);
    oled_cmd(0xA4);
    oled_cmd(0xA6);
    oled_cmd(0xAF);
    _delay_ms(50);
}

/* Fuente 5x8 básica */
static const uint8_t font_5x8[96][5] PROGMEM = {
    {0x00,0x00,0x00,0x00,0x00}, // espacio
    {0x00,0x00,0x5F,0x00,0x00}, // !
    {0x00,0x07,0x00,0x07,0x00}, // "
    {0x14,0x7F,0x14,0x7F,0x14}, // #
    {0x24,0x2A,0x7F,0x2A,0x12}, // $
    {0x23,0x13,0x08,0x64,0x62}, // %
    {0x36,0x49,0x55,0x22,0x50}, // &
    {0x00,0x05,0x03,0x00,0x00}, // '
    {0x00,0x1C,0x22,0x41,0x00}, // (
    {0x00,0x41,0x22,0x1C,0x00}, // )
    {0x08,0x2A,0x1C,0x2A,0x08}, // *
    {0x08,0x08,0x3E,0x08,0x08}, // +
    {0x00,0x50,0x30,0x00,0x00}, // ,
    {0x08,0x08,0x08,0x08,0x08}, // -
    {0x00,0x60,0x60,0x00,0x00}, // .
    {0x20,0x10,0x08,0x04,0x02}, // /
    {0x3E,0x51,0x49,0x45,0x3E}, // 0
    {0x00,0x42,0x7F,0x40,0x00}, // 1
    {0x42,0x61,0x51,0x49,0x46}, // 2
    {0x21,0x41,0x45,0x4B,0x31}, // 3
    {0x18,0x14,0x12,0x7F,0x10}, // 4
    {0x27,0x45,0x45,0x45,0x39}, // 5
    {0x3C,0x4A,0x49,0x49,0x30}, // 6
    {0x01,0x71,0x09,0x05,0x03}, // 7
    {0x36,0x49,0x49,0x49,0x36}, // 8
    {0x06,0x49,0x49,0x29,0x1E}, // 9
    {0x00,0x36,0x36,0x00,0x00}, // :
    {0x00,0x56,0x36,0x00,0x00}, // ;
    {0x00,0x08,0x14,0x22,0x41}, // <
    {0x14,0x14,0x14,0x14,0x14}, // =
    {0x41,0x22,0x14,0x08,0x00}, // >
    {0x02,0x01,0x51,0x09,0x06}, // ?
    {0x32,0x49,0x79,0x41,0x3E}, // @
    {0x7E,0x11,0x11,0x11,0x7E}, // A
    {0x7F,0x49,0x49,0x49,0x36}, // B
    {0x3E,0x41,0x41,0x41,0x22}, // C
    {0x7F,0x41,0x41,0x22,0x1C}, // D
    {0x7F,0x49,0x49,0x49,0x41}, // E
    {0x7F,0x09,0x09,0x01,0x01}, // F
    {0x3E,0x41,0x41,0x51,0x32}, // G
    {0x7F,0x08,0x08,0x08,0x7F}, // H
    {0x00,0x41,0x7F,0x41,0x00}, // I
    {0x20,0x40,0x41,0x3F,0x01}, // J
    {0x7F,0x08,0x14,0x22,0x41}, // K
    {0x7F,0x40,0x40,0x40,0x40}, // L
    {0x7F,0x02,0x04,0x02,0x7F}, // M
    {0x7F,0x04,0x08,0x10,0x7F}, // N
    {0x3E,0x41,0x41,0x41,0x3E}, // O
    {0x7F,0x09,0x09,0x09,0x06}, // P
    {0x3E,0x41,0x51,0x21,0x5E}, // Q
    {0x7F,0x09,0x19,0x29,0x46}, // R
    {0x46,0x49,0x49,0x49,0x31}, // S
    {0x01,0x01,0x7F,0x01,0x01}, // T
    {0x3F,0x40,0x40,0x40,0x3F}, // U
    {0x1F,0x20,0x40,0x20,0x1F}, // V
    {0x7F,0x20,0x18,0x20,0x7F}, // W
    {0x63,0x14,0x08,0x14,0x63}, // X
    {0x03,0x04,0x78,0x04,0x03}, // Y
    {0x61,0x51,0x49,0x45,0x43}, // Z
    {0x00,0x00,0x7F,0x41,0x41}, // [
    {0x02,0x04,0x08,0x10,0x20}, // \
    {0x41,0x41,0x7F,0x00,0x00}, // ]
    {0x04,0x02,0x01,0x02,0x04}, // ^
    {0x40,0x40,0x40,0x40,0x40}, // _
    {0x00,0x01,0x02,0x04,0x00}, // `
    {0x20,0x54,0x54,0x54,0x78}, // a
    {0x7F,0x48,0x44,0x44,0x38}, // b
    {0x38,0x44,0x44,0x44,0x20}, // c
    {0x38,0x44,0x44,0x48,0x7F}, // d
    {0x38,0x54,0x54,0x54,0x18}, // e
    {0x08,0x7E,0x09,0x01,0x02}, // f
    {0x08,0x14,0x54,0x54,0x3C}, // g
    {0x7F,0x08,0x04,0x04,0x78}, // h
    {0x00,0x44,0x7D,0x40,0x00}, // i
    {0x20,0x40,0x44,0x3D,0x00}, // j
    {0x00,0x7F,0x10,0x28,0x44}, // k
    {0x00,0x41,0x7F,0x40,0x00}, // l
    {0x7C,0x04,0x18,0x04,0x78}, // m
    {0x7C,0x08,0x04,0x04,0x78}, // n
    {0x38,0x44,0x44,0x44,0x38}, // o
    {0x7C,0x14,0x14,0x14,0x08}, // p
    {0x08,0x14,0x14,0x18,0x7C}, // q
    {0x7C,0x08,0x04,0x04,0x08}, // r
    {0x48,0x54,0x54,0x54,0x20}, // s
    {0x04,0x3F,0x44,0x40,0x20}, // t
    {0x3C,0x40,0x40,0x20,0x7C}, // u
    {0x1C,0x20,0x40,0x20,0x1C}, // v
    {0x3C,0x40,0x30,0x40,0x3C}, // w
    {0x44,0x28,0x10,0x28,0x44}, // x
    {0x0C,0x50,0x50,0x50,0x3C}, // y
    {0x44,0x64,0x54,0x4C,0x44}  // z
};

static void oled_char(uint8_t c) {
    if (c < 32 || c > 127) c = 32;
    uint8_t idx = c - 32;
    for (uint8_t i = 0; i < 5; i++) {
        oled_data(pgm_read_byte(&font_5x8[idx][i]));
    }
    oled_data(0x00);
}

static void oled_string(const char* str) {
    while (*str) {
        oled_char(*str++);
    }
}

static void oled_string_p(const char* pstr) {
    char c;
    while ((c = pgm_read_byte(pstr++))) {
        oled_char(c);
    }
}

/* ============================================================================
 * FORMATEO DE NÚMEROS (sin sprintf)
 * ============================================================================ */
static void format_number(uint16_t num, uint8_t div, char* out) {
    uint16_t ent = num / div;
    uint16_t dec = (num % div) / (div / 100);
    
    // Parte entera
    if (ent >= 100) {
        *out++ = '0' + (ent / 100);
        ent %= 100;
        *out++ = '0' + (ent / 10);
    } else if (ent >= 10) {
        *out++ = '0' + (ent / 10);
    } else {
        *out++ = '0' + ent;
    }
    ent %= 10;
    *out++ = '0' + ent;
    
    // Decimal
    *out++ = '.';
    *out++ = '0' + (dec / 10);
    *out++ = '0' + (dec % 10);
    *out = '\0';
}

static void update_display(void) {
    // Línea 0: Voltaje batería
    oled_pos(0, 0);
    oled_string_p(str_bat);
    format_number(v_battery_mv, 1000, disp_buffer);
    oled_string(disp_buffer);
    oled_string_p(str_v);
    
    // Línea 2: Voltaje panel
    oled_pos(2, 0);
    oled_string_p(str_sol);
    format_number(v_panel_mv, 1000, disp_buffer);
    oled_string(disp_buffer);
    oled_string_p(str_v);
    
    // Línea 4: Corriente
    oled_pos(4, 0);
    oled_string_p(str_cur);
    if (current_ma < 0) {
        oled_char('-');
        format_number((uint16_t)(-current_ma), 1000, disp_buffer);
    } else {
        format_number((uint16_t)current_ma, 1000, disp_buffer);
    }
    oled_string(disp_buffer);
    oled_string_p(str_a);
    
    // Línea 6: Potencia y Duty
    oled_pos(6, 0);
    oled_string_p(str_pow);
    format_number(power_mw, 1000, disp_buffer);
    oled_string(disp_buffer);
    oled_string_p(str_w);
    oled_char(' ');
    uint8_t pct = ((uint16_t)duty_cycle * 100) / PWM_TOP;
    format_number((uint16_t)pct, 1, disp_buffer);
    oled_string(disp_buffer);
    oled_char('%');
}

/* ============================================================================
 * INTERRUPCIÓN TIMER (1ms)
 * ============================================================================ */
ISR(TIM0_COMPA_vect) {
    static uint16_t ms_count = 0;
    ms_count++;
    
    if (ms_count >= MPPT_INTERVAL_MS) {
        ms_count = 0;
        mppt_update_flag = 1;
        display_update_flag = 1;
    }
    
    // Reset watchdog
    wdt_reset();
}

/* ============================================================================
 * FUNCIÓN PRINCIPAL
 * ============================================================================ */
int main(void) {
    init_system();
    
    // Pantalla inicial
    oled_pos(3, 40);
    oled_string_p(PSTR("MPPT"));
    oled_pos(5, 30);
    oled_string_p(PSTR("V 2.0"));
    _delay_ms(1500);
    oled_clear();
    
    // Lectura inicial
    read_sensors();
    last_power = power_mw;
    
    while (1) {
        if (mppt_update_flag) {
            mppt_update_flag = 0;
            read_sensors();
            charge_control();
            update_pwm(duty_cycle);
        }
        
        if (display_update_flag) {
            display_update_flag = 0;
            update_display();
        }
        
        _delay_ms(1);
    }
    
    return 0;
}