#include <stdio.h>

ADC_HandleTypeDef hadc1;
TIM_HandleTypeDef htim2; // Timer untuk servo
TIM_HandleTypeDef htim3; // Timer untuk input capture ultrasonic
UART_HandleTypeDef huart2;

#define TRIG_Pin GPIO_PIN_0
#define TRIG_Port GPIOA

// Ganti pin di sini sesuai mapping
uint16_t echoPins[6] = {GPIO_PIN_1, GPIO_PIN_2, GPIO_PIN_3, GPIO_PIN_4, GPIO_PIN_5, GPIO_PIN_6};
GPIO_TypeDef* echoPorts[6] = {GPIOA, GPIOA, GPIOA, GPIOA, GPIOA, GPIOA};

uint32_t adcChannels[6] = {ADC_CHANNEL_7, ADC_CHANNEL_8, ADC_CHANNEL_9, ADC_CHANNEL_10, ADC_CHANNEL_11, ADC_CHANNEL_12};

uint32_t servoChannels[6] = {TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4, TIM_CHANNEL_1, TIM_CHANNEL_2}; // Sesuaikan dengan Timer
TIM_HandleTypeDef* servoTimers[6] = {&htim2, &htim2, &htim2, &htim2, &htim3, &htim3}; // Ganti sesuai setup kamu

int threshold_moisture = 500;
int threshold_level = 20;

void triggerUltrasonic() {
    HAL_GPIO_WritePin(TRIG_Port, TRIG_Pin, GPIO_PIN_SET);
    HAL_Delay(1);
    HAL_GPIO_WritePin(TRIG_Port, TRIG_Pin, GPIO_PIN_RESET);
}

uint32_t measureDistance(GPIO_TypeDef* echoPort, uint16_t echoPin) {
    uint32_t start = 0, end = 0;

    // Wait for echo HIGH
    while (HAL_GPIO_ReadPin(echoPort, echoPin) == GPIO_PIN_RESET);
    start = __HAL_TIM_GET_COUNTER(&htim3);

    // Wait for echo LOW
    while (HAL_GPIO_ReadPin(echoPort, echoPin) == GPIO_PIN_SET);
    end = __HAL_TIM_GET_COUNTER(&htim3);

    uint32_t diff = (end > start) ? (end - start) : (0xFFFF - start + end);
    float distance_cm = diff * 0.034 / 2;
    return distance_cm;
}

uint16_t readSoil(uint32_t channel) {
    ADC_ChannelConfTypeDef sConfig = {0};
    sConfig.Channel = channel;
    sConfig.Rank = ADC_REGULAR_RANK_1;
    sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);

    HAL_ADC_Start(&hadc1);
    HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);
    return HAL_ADC_GetValue(&hadc1);
}

void setServoAngle(TIM_HandleTypeDef* htim, uint32_t channel, int angle) {
    // 0 deg = 0.5ms, 180 deg = 2.5ms
    uint32_t pulse_length = ((angle * (2500 - 500)) / 180) + 500;
    __HAL_TIM_SET_COMPARE(htim, channel, pulse_length);
}

int __io_putchar(int ch) {
    HAL_UART_Transmit(&huart2, (uint8_t*)&ch, 1, HAL_MAX_DELAY);
    return ch;
}

int main(void) {
    HAL_Init();
    SystemClock_Config();

    MX_GPIO_Init();
    MX_ADC1_Init();
    MX_TIM2_Init();
    MX_TIM3_Init();
    MX_USART2_UART_Init();

    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); // Lanjutkan untuk semua servoChannels
    HAL_TIM_Base_Start(&htim3); // Timer untuk mengukur durasi echo

    printf("Mulai sistem...\r\n");

    while (1) {
        for (int i = 0; i < 6; i++) {
            uint16_t soil = readSoil(adcChannels[i]);
            triggerUltrasonic();
            HAL_Delay(1);
            float distance = measureDistance(echoPorts[i], echoPins[i]);

            printf("Sensor %d - Soil: %d | Distance: %.2f cm | Servo: ", i + 1, soil, distance);

            if (distance > threshold_level) {
                setServoAngle(servoTimers[i], servoChannels[i], 0);
                printf("Tutup (Ultrasonic)\r\n");
            } else {
                if (soil < threshold_moisture) {
                    setServoAngle(servoTimers[i], servoChannels[i], 90);
                    printf("Buka (Kering)\r\n");
                } else {
                    setServoAngle(servoTimers[i], servoChannels[i], 0);
                    printf("Tutup (Lembap)\r\n");
                }
            }
            HAL_Delay(300);
        }
        printf("-------------\r\n");
        HAL_Delay(1000);
    }
}