/*
 * Main application using modular libraries
 */

#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
#include <zephyr/usb/usb_device.h>
#include <zephyr/drivers/uart.h>

#include "led.h"
#include "button.h"
#include "actuator.h"
#include "battery_adc.h"
#include "temperature.h"
#include "timer_count.h" // your custom measurement driver

const struct device *const dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_console));

int main(void)
{
    int ret;

    /* Init USB */
    if (usb_enable(NULL))
    {
        printk("USB init failed\n");
        return 0;
    }

    //actuator_init(ACTUATOR_MODE_MOTOR);

    //actuator_servo_set_angle(100);
    //actuator_motor_set_speed(-50); // -100..100 %
    /* Init modules */
    led_init();
    button_init();
    ret = battery_adc_init();
    if (ret < 0)
    {
        printk("Battery ADC init failed (%d)\n", ret);
    }
    ret = temperature_init();
    if (ret < 0)
    {
        printk("Temperature init failed (%d)\n", ret);
    }

    configure_measurement();
    make_measurement();

    /* Start blinking LED0 every 1s */
    led_start_blink(0, 1000);

    int old_val = 1;
    int out_en = 1;

    while (1)
    {
        /* --- Battery measurement --- */
        int32_t batt_mv;
        ret = battery_measure_mv(&batt_mv);
        if (ret == 0)
        {
            printk("Battery: %d mV ", batt_mv);
        }
        else
        {
            printk("Battery measurement failed (%d) ", ret);
        }

        /* --- Temperature measurement --- */
        int32_t temp_c;
        ret = temperature_get_celsius(&temp_c);
        if (ret == 0)
        {
            printk("Temperature: %d C ", temp_c);
        }
        else
        {
            printk("Temperature read failed (%d) ", ret);
        }

        int32_t humidity_meas = read_measurement();
        if (humidity_meas >= 0)
        {
            printk("humidity_raw: %d\n", humidity_meas);
            make_measurement();
        }

        /* --- Button handling --- */
        int val = button_read();
        if (val < 0)
        {
            printk("Error reading button\n");
        }
        else if (val == 0)
        {
            if (old_val != val)
            {
                out_en = !out_en;
                led_toggle(1); // toggle LED1 on press
                old_val = val;

                actuator_enable(1);
                if(out_en){
                    actuator_motor_set_speed(35); // -100..100 %
                }
                else{
                    actuator_motor_set_speed(-35); // -100..100 %
                }
                k_sleep(K_MSEC(300));
                actuator_motor_set_speed(0); // -100..100 %
                actuator_enable(0);
            }
        }
        else
        {
            if (old_val != val)
            {
                old_val = val;
            }
            printk("Button released\n");
        }

        k_sleep(K_MSEC(100));
    }
}