//
// Created by iflyt on 2025/3/13.
//

#include "laser_control.h"

#include <cmath>
#include <stm32f4xx_hal.h>
#include "tim_pwm.h"
#include <base/zbasic.h>

LaserControl::LaserControl() {
}

LaserControl::~LaserControl() {
}

void LaserControl::init(const bool isFlip) {
    this->initGPIO();
    this->isFlip_ = isFlip;
    this->powerOff();
}

void LaserControl::powerOn() {
    isPowerOn_ = true;
    HAL_GPIO_WritePin(LASER_EN_PORT, LASER_EN_PIN, GPIO_PIN_RESET);
}

void LaserControl::powerOff() {
    isPowerOn_ = false;
    this->dutyCycle_ = calculateDutyCycle(MIN_POWER);
    this->setDutyCycle(dutyCycle_);
    HAL_GPIO_WritePin(LASER_EN_PORT, LASER_EN_PIN, GPIO_PIN_SET);
}

void LaserControl::setPower(double power) {
    power_ = power;
    const int32_t abs_voltage = fabs(power);
    this->dutyCycle_ = calculateDutyCycle(abs_voltage);
    this->setDutyCycle(dutyCycle_);
    ZLOGI("[LASER]", "SET DUTY %d", this->dutyCycle_);
    this->powerOn();
}

void LaserControl::initGPIO() {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // 使能 GPIO 时钟
    __HAL_RCC_GPIOE_CLK_ENABLE();

    // 配置使能引脚
    GPIO_InitStruct.Pin = LASER_EN_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(LASER_EN_PORT, &GPIO_InitStruct);
}

void LaserControl::setDutyCycle(uint32_t dutyCycle) const {
    if(dutyCycle == minVDutyCycle) {
        bsp_SetTIMOutPWM(LASER_PWM_PORT, LASER_PWM_PIN, LASER_TIMER_HANDLE, LASER_TIMER_CHANNEL, 0, 0);
    }
    else if (dutyCycle == maxVDutyCycle){
        bsp_SetTIMOutPWM(LASER_PWM_PORT, LASER_PWM_PIN, LASER_TIMER_HANDLE, LASER_TIMER_CHANNEL, 0, 10000);
    }
    else {
        bsp_SetTIMOutPWM(LASER_PWM_PORT, LASER_PWM_PIN, LASER_TIMER_HANDLE, LASER_TIMER_CHANNEL, standardFrequency_, dutyCycle);
    }
}

int LaserControl::calculateDutyCycle(int voltage) const {
    if (voltage < MIN_POWER) {
        voltage = MIN_POWER;
    } else if (voltage > MAX_POWER) {
        voltage = MAX_POWER;
    }

    if (isFlip_) {
        // 反转关系
        return maxVDutyCycle - (voltage - MIN_POWER) * (maxVDutyCycle - minVDutyCycle) / (MAX_POWER - MIN_POWER);
    } else {
        // 正常关系
        return minVDutyCycle + (voltage  - MIN_POWER) * (maxVDutyCycle - minVDutyCycle) / (MAX_POWER - MIN_POWER);
    }
}