#include "lab.h"
#include <string.h>
static int clamp(int x,int lo,int hi) {
    return x<lo?lo:(x>hi?hi:x);
}
int64_t now_ms(void) {
    return esp_timer_get_time()/1000;
}
void led_init(void) {
    gpio_config_t c= {
        .pin_bit_mask=1ULL<<LED,
        .mode=GPIO_MODE_OUTPUT
    };
    ESP_ERROR_CHECK(gpio_config(&c));
    ESP_ERROR_CHECK(gpio_set_level(LED,0));
}
void button_init(void) {
    gpio_config_t c= {
        .pin_bit_mask=1ULL<<BUTTON,
        .mode=GPIO_MODE_INPUT,
        .pull_up_en=GPIO_PULLUP_ENABLE
    };
    ESP_ERROR_CHECK(gpio_config(&c));
}
bool button_pressed(void) {
    static int previous=1,
    stable=1;
    static int64_t changed=0;
    int value=gpio_get_level(BUTTON);
    int64_t t=now_ms();
    if(value!=previous) {
        previous=value;
        changed=t;
    }
    if(value!=stable && t-changed>=30) {
        stable=value;
        return stable==0;
    }
    return false;
}
void pwm_init(void) {
    ledc_timer_config_t t= {
        .speed_mode=LEDC_LOW_SPEED_MODE,
        .duty_resolution=LEDC_TIMER_10_BIT,
        .timer_num=LEDC_TIMER_0,
        .freq_hz=5000,
        .clk_cfg=LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&t));
    ledc_channel_config_t c= {
        .gpio_num=LED,
        .speed_mode=LEDC_LOW_SPEED_MODE,
        .channel=LEDC_CHANNEL_0,
        .timer_sel=LEDC_TIMER_0,
        .duty=0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&c));
}
void pwm_set(int duty) {
    ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE,LEDC_CHANNEL_0,clamp(duty,0,1023)));
    ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE,LEDC_CHANNEL_0));
}
static adc_oneshot_unit_handle_t adc;
void adc_init(void) {
    adc_oneshot_unit_init_cfg_t u= {
        .unit_id=ADC_UNIT_1
    };
    ESP_ERROR_CHECK(adc_oneshot_new_unit(&u,&adc));
    adc_oneshot_chan_cfg_t c= {
        .atten=ADC_ATTEN_DB_12,
        .bitwidth=ADC_BITWIDTH_12
    };
    ESP_ERROR_CHECK(adc_oneshot_config_channel(adc,ADC_CHANNEL_0,&c));
    // GPIO1 on S3
}
int adc_read_raw(void) {
    int raw=0;
    ESP_ERROR_CHECK(adc_oneshot_read(adc,ADC_CHANNEL_0,&raw));
    return raw;
}
static i2c_master_bus_handle_t bus;
i2c_master_bus_handle_t bus_init(void) {
    if(bus)return bus;
    i2c_master_bus_config_t c= {
        .i2c_port=I2C_NUM_0,
        .sda_io_num=SDA,
        .scl_io_num=SCL,
        .clk_source=I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt=7
    };
    // Use module pull-ups to 3.3V or external 4.7k pull-ups. No reliance on weak internal pulls.
    ESP_ERROR_CHECK(i2c_new_master_bus(&c,&bus));
    return bus;
}
static i2c_master_dev_handle_t display;
static uint8_t frame[1024];
static void commands(const uint8_t *v,size_t n) {
    uint8_t p[40]= {
        0
    };
    assert(n<=39);
    memcpy(p+1,v,n);
    ESP_ERROR_CHECK(i2c_master_transmit(display,p,n+1,100));
}
void oled_init(void) {
    bus_init();
    int address=0x3c;
    esp_err_t e=i2c_master_probe(bus,address,100);
    if(e!=ESP_OK) {
        address=0x3d;
        ESP_ERROR_CHECK(i2c_master_probe(bus,address,100));
    }
    i2c_device_config_t d= {
        .dev_addr_length=I2C_ADDR_BIT_LEN_7,
        .device_address=address,
        .scl_speed_hz=100000
    };
    ESP_ERROR_CHECK(i2c_master_bus_add_device(bus,&d,&display));
    vTaskDelay(pdMS_TO_TICKS(100));
    const uint8_t init[]= {
        0xae,
        0xd5,
        0x80,
        0xa8,
        0x3f,
        0xd3,
        0x00,
        0x40,
        0x8d,
        0x14,
        0x20,
        0x00,
        0xa1,
        0xc8,
        0xda,
        0x12,
        0x81,
        0x7f,
        0xd9,
        0xf1,
        0xdb,
        0x40,
        0xa4,
        0xa6,
        0x2e,
        0xaf
    };
    commands(init,sizeof(init));
    oled_clear();
    oled_show();
}
void oled_clear(void) {
    memset(frame,0,sizeof(frame));
}
void oled_rect(int x,int y,int w,int h) {
    for(int j=y; j<y+h; j++)for(int i=x; i<x+w; i++) if(i>=0&&i<128&&j>=0&&j<64)frame[(j/8)*128+i]|=1U<<(j%8);
}
void oled_number(int x,int y,unsigned n,int digits) {
    // Original seven-segment glyphs; 12x22 pixels, 16-pixel advance.
    const uint8_t mask[10]= {
        0x3f,
        0x06,
        0x5b,
        0x4f,
        0x66,
        0x6d,
        0x7d,
        0x07,
        0x7f,
        0x6f
    };
    const int rect[7][4]= {
        {
            2,
            0,
            8,
            2
        },
        {
            10,
            2,
            2,
            8
        },
        {
            10,
            12,
            2,
            8
        },
        {
            2,
            20,
            8,
            2
        },
        {
            0,
            12,
            2,
            8
        },
        {
            0,
            2,
            2,
            8
        },
        {
            2,
            10,
            8,
            2
        }
    };
    for(int k=digits-1; k>=0; k--) {
        uint8_t m=mask[n%10];
        n/=10;
        for(int s=0; s<7; s++)if(m&(1<<s))oled_rect(x+k*16+rect[s][0],y+rect[s][1],rect[s][2],rect[s][3]);
    }
}
void oled_show(void) {
    const uint8_t pos[]= {
        0x21,
        0,
        127,
        0x22,
        0,
        7
    };
    commands(pos,sizeof(pos));
    for(int i=0; i<1024; i+=16) {
        uint8_t p[17];
        p[0]=0x40;
        memcpy(p+1,frame+i,16);
        ESP_ERROR_CHECK(i2c_master_transmit(display,p,sizeof(p),100));
    }
}
void motor_init(void) {
    ledc_timer_config_t t= {
        .speed_mode=LEDC_LOW_SPEED_MODE,
        .duty_resolution=LEDC_TIMER_10_BIT,
        .timer_num=LEDC_TIMER_1,
        .freq_hz=20000,
        .clk_cfg=LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&t));
    const int pins[]= {
        10,
        11,
        12,
        13
    };
    for(int i=0; i<4; i++) {
        ledc_channel_config_t c= {
            .gpio_num=pins[i],
            .speed_mode=LEDC_LOW_SPEED_MODE,
            .channel=(ledc_channel_t)(i+1),
            .timer_sel=LEDC_TIMER_1,
            .duty=0
        };
        ESP_ERROR_CHECK(ledc_channel_config(&c));
    }
}
static void motor_channel(int c,int duty) {
    ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE,(ledc_channel_t)c,duty));
    ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE,(ledc_channel_t)c));
}
static void motor_pair(int ch,int value) {
    value=clamp(value,-1023,1023);
    // Clear opposite input first. Application must stop before reversing direction.
    if(value>=0) {
        motor_channel(ch+1,0);
        motor_channel(ch,value);
    }
    else {
        motor_channel(ch,0);
        motor_channel(ch+1,-value);
    }
}
void motor_set(int left,int right) {
    motor_pair(1,left);
    motor_pair(3,right);
}