TermApp/app/src/main/cpp/PtzController.cpp

//
// Created by Matthew on 2025/3/5.
//

#include "PtzController.h"
#include "SensorsProtocol.h"
#include "GPIOControl.h"
#include "PhoneDevice.h"
#include <memory>

PtzController::PtzController(CPhoneDevice* pPhoneDevice) : m_pPhoneDevice(pPhoneDevice)
{
    m_exit = false;
}

void PtzController::Startup()
{
    m_thread = std::thread(PtzThreadProc, this);
}

void PtzController::PtzThreadProc(PtzController* pThis)
{
    pThis->PtzProc();
}

void PtzController::AddCommand(uint8_t channel, int cmdidx, uint8_t bImageSize, uint8_t preset, const char *serfile, uint32_t baud, int addr)
{
    SERIAL_CMD cmd = { 0 };
    cmd.channel = channel;
    cmd.preset = preset;
    cmd.cmdidx = cmdidx;
    cmd.bImageSize = bImageSize;
    strcpy(cmd.serfile, serfile);
    cmd.baud = baud;
    cmd.addr = addr;

    m_locker.lock();
    m_cmds.push_back(cmd);
    m_locker.unlock();

    m_sem.release();
}

void PtzController::AddPhotoCommand(IDevice::PHOTO_INFO& photoInfo, const std::string& path, const std::vector<IDevice::OSD_INFO>& osds)
{
    IDevice::SerialsPhotoParam param = { "", 0, 0 };
    m_pPhoneDevice->GetPhotoSerialsParamCb(param);

    SERIAL_CMD cmdPreset = { 0 };
    time_t ts = time(NULL);
    // if (photoInfo.preset != 0 && photoInfo.preset != 0xFF)
    {
        cmdPreset.ts = photoInfo.selfTestingTime;
        cmdPreset.delayTime = photoInfo.closeDelayTime;
        cmdPreset.channel = photoInfo.channel;
        cmdPreset.channel = photoInfo.preset;
        cmdPreset.cmdidx = OPEN_TOTAL;
        strcpy(cmdPreset.serfile, param.serfile);
        cmdPreset.baud = param.baud;
        cmdPreset.addr = param.addr;
    }
    SERIAL_CMD cmd = { 0 };
    cmd.ts = ts;
    cmd.delayTime = photoInfo.closeDelayTime;
    cmd.channel = photoInfo.channel;
    cmd.preset = photoInfo.preset;
    cmd.cmdidx = Take_Photo;
    cmd.bImageSize = photoInfo.resolution;

    strcpy(cmd.serfile, param.serfile);
    cmd.baud = param.baud;
    cmd.addr = param.addr;

    PtzPhotoParams* ppp = new PtzPhotoParams(photoInfo, path, osds);
    cmd.photoParams.reset(ppp);
    // cmd.delayTime;
    // uint8_t bImageSize;
    // char serfile[128];
    // uint32_t baud;
    // int addr;
    m_locker.lock();
    if (cmdPreset.cmdidx != 0)
    {
        m_cmds.push_back(cmdPreset);
    }
    m_cmds.push_back(cmd);
    m_locker.unlock();

    m_sem.release();
    m_sem.release();
}

void PtzController::ExitAndWait()
{
    m_exit = true;
    m_sem.release();

    if (m_thread.joinable())
    {
        m_thread.join();
    }
}

void PtzController::PtzProc()
{
    PROC_PTZ_STATE state = PTZS_POWER_OFF;
    SERIAL_CMD cmd;
    PTZ_STATE ptz_state;
    bool hasCmd = false;
    int i=0;

    std::shared_ptr<PowerControl> powerCtrl;
    time_t selfTestingStartTime = 0;
    time_t selfTestingWaitTime = 0;
    time_t PTZ_preset_start_time = 0;
    time_t PTZ_preset_wait_time = 0;
    time_t close_delay_time = CAMERA_CLOSE_DELAYTIME;
    time_t start_delay_time = 0;

    while(true)
    {
        m_sem.acquire();

        if (m_exit)
        {
            break;
        }

        hasCmd = false;

        m_locker.lock();
        for (auto it = m_cmds.begin(); it != m_cmds.end(); ++it)
        {
            if (state == PTZS_SELF_TESTING)
            {
                // find first non-taking-photo cmd
                if (it->cmdidx != Take_Photo)
                {
                    cmd = *it;
                    m_cmds.erase(it);
                    hasCmd = true;
                    break;
                }
            }
            else
            {
                cmd = *it;
                m_cmds.erase(it);
                hasCmd = true;
                break;
            }
        }
        m_locker.unlock();

        if (!hasCmd)
        {
            if (state == PTZS_SELF_TESTING)
            {
                time_t timeout = time(NULL) - selfTestingStartTime;
                if (timeout >= selfTestingWaitTime)
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "超时(%d秒)未收到云台自检结束应答，状态改为空闲！", timeout);
                    state = PTZS_IDLE;
                    m_sem.release();
                    continue;
                }
                else
                {
                    if(timeout >= CAMERA_SELF_TEST_TIME)
                    {
                        //XYLOG(XYLOG_SEVERITY_INFO, "开始查询云台自检状态！timeout=%d秒", timeout);
                        if(0 == QueryPtzState(&ptz_state, QUERY_PTZ_STATE, cmd.serfile, cmd.baud, cmd.addr))
                        {
                            if(0 == ptz_state.ptz_status)
                            {
                                XYLOG(XYLOG_SEVERITY_INFO, "收到云台自检结束应答，状态改为空闲！timeout=%d秒", timeout);
                                state = PTZS_IDLE;
                                m_sem.release();
                                continue;
                            }
                        }
                    }
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                m_sem.release();
                continue;
            }
            if(time(NULL) - start_delay_time >= close_delay_time)
            {
                XYLOG(XYLOG_SEVERITY_INFO, "摄像机空闲时间超过%d秒！关闭摄像机！", close_delay_time);
                powerCtrl.reset();
            }
            continue;
        }

        start_delay_time = time(NULL);
        switch (cmd.cmdidx)
        {
            case Take_Photo:
            {
                if (state == PTZS_POWER_OFF)
                {
                    if (!powerCtrl)
                    {
                        //powerCtrl = std::make_shared<PlzCameraPowerCtrl>(cmd.photoParams->mPhotoInfo.closeDelayTime);
                        powerCtrl = std::make_shared<PlzCameraPowerCtrl>(0);
                        selfTestingStartTime = time(NULL);
                        selfTestingWaitTime = cmd.photoParams->mPhotoInfo.selfTestingTime;
                        state = PTZS_SELF_TESTING;
                        XYLOG(XYLOG_SEVERITY_INFO, "1、收到拍照指令，摄像机从关机状态改为自检状态！");

                        m_locker.lock();
                        m_cmds.insert(m_cmds.begin(), cmd);
                        m_locker.unlock();
                        m_sem.release();
                        continue;
                    }
                }
                XYLOG(XYLOG_SEVERITY_INFO, "2、收到拍照指令，state=%d！", state);

                state = PTZS_TAKING_PHOTO;
                if (cmd.preset != 0 && cmd.preset != 0xFF)
                {
                    CameraPhotoCmd(0, cmd.channel, MOVE_PRESETNO, 0, cmd.preset, cmd.serfile, cmd.baud, cmd.addr);

                    XYLOG(XYLOG_SEVERITY_INFO, "摄像机拍照前开始调用预置点！state=%d", state);
                    PTZ_preset_start_time = time(NULL);
                    PTZ_preset_wait_time = MOVE_PRESET_WAIT_TIME;
                    for(;;)
                    {
                        if(0 == QueryPtzState(&ptz_state, QUERY_PTZ_STATE, cmd.serfile, cmd.baud, cmd.addr))
                        {
                            if(0 == ptz_state.ptz_status)
                            {
                                XYLOG(XYLOG_SEVERITY_INFO, "摄像机拍照前调用预置点，收到移动结束应答！移动时长=%d秒 state=%d", time(NULL)-PTZ_preset_start_time, state);
                                break;
                            }
                        }
                        if(time(NULL) - PTZ_preset_start_time >= PTZ_preset_wait_time)
                        {
                            XYLOG(XYLOG_SEVERITY_INFO, "摄像机拍照前调用预置点，摄像机在%d秒内未收到调用预置点结束应答！state=%d", PTZ_preset_wait_time, state);
                            break;
                        }
                        std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    }
                }
                m_pPhoneDevice->TakePhotoWithNetCamera(cmd.photoParams->mPhotoInfo, cmd.photoParams->mPath, cmd.photoParams->mOsds, powerCtrl);
                state = PTZS_IDLE;
            }

                break;
            case OPEN_TOTAL:
                if (state == PTZS_POWER_OFF)
                {
                    if (!powerCtrl)
                    {
                        powerCtrl = std::make_shared<PlzCameraPowerCtrl>(0);
                        selfTestingStartTime = time(NULL);
                        selfTestingWaitTime = 150;
                        state = PTZS_SELF_TESTING;
                        m_sem.release();
                        XYLOG(XYLOG_SEVERITY_INFO, "收到开机指令，摄像机从关机状态改为自检状态！设置的自检等待时间%d秒", selfTestingWaitTime);
                    }
                }
                else
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "收到开机指令，摄像机处于state=%d！", state);
                }
                break;
            case CLOSE_TOTAL:
                if (state == PTZS_POWER_OFF)
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "收到关机指令，摄像机本来就处于关机状态！");
                    // Do Nothing
                }
                else
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "收到关机指令，通知云台准备关机！state=%d", state);
                    for(i=0; i<3; i++)
                    {
                        if(0 == QueryPtzState(&ptz_state, NOTIFY_PTZ_CLOSE, cmd.serfile, cmd.baud, cmd.addr))
                            break;
                    }
                    powerCtrl.reset();
                    state = PTZS_POWER_OFF;
                    XYLOG(XYLOG_SEVERITY_INFO, "关闭云台电源！state=%d", state);
                }
                break;
            default:
            {
                CameraPhotoCmd(cmd.ts, cmd.channel, cmd.cmdidx, 0, cmd.preset, cmd.serfile, cmd.baud, cmd.addr);

            }
                break;
        }
    }


}