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 "time.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;
    cmd.ts = time(NULL);

    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 1
    // 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 = PHOTO_OPEN_POWER;
        strcpy(cmdPreset.serfile, param.serfile);
        cmdPreset.baud = param.baud;
        cmdPreset.addr = param.addr;
    }
#endif
    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 1
    if (cmdPreset.cmdidx != 0)
    {
        m_cmds.push_back(cmdPreset);
    }
#endif
    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;
    int closecmd=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;
    time_t auto_delay_time = 0;
    time_t auto_wait_time = WAIT_TIME_AUTO_CLOSE;
    time_t photo_move_preset_time = 0;
    int iwaitime = 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) || (PTZS_PHOTO_SELF_TESTING == state))
            {
                // 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) || (PTZS_PHOTO_SELF_TESTING == state))
            {
                time_t timeout = time(NULL) - selfTestingStartTime;
                if(timeout < 0)
                    selfTestingStartTime = time(NULL);
                if (timeout >= selfTestingWaitTime)
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "超时(%u秒)未收到云台自检结束应答，状态改为空闲！", (uint32_t)timeout);
                    state = PTZS_IDLE;
                    m_sem.release();
                    continue;
                }
                else
                {
                    //if(timeout >= CAMERA_SELF_TEST_TIME)
                    {
                        if (timeout == 1 || ((timeout % 10) == 0))
                        {
                            XYLOG(XYLOG_SEVERITY_INFO, "开始查询云台自检状态！timeout=%u秒", (uint32_t)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=%u秒", (uint32_t)timeout);
                                state = PTZS_IDLE;
                                m_sem.release();
                                continue;
                            }
                        }
                    }
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                m_sem.release();
                continue;
            }
            if(0 == start_delay_time)
            {
                if(0 == iwaitime)
                {
                    auto_delay_time = time(NULL);
                    iwaitime += 1;
                    m_sem.release();
                    continue;
                }
                else
                {
                    if(time(NULL) - auto_delay_time < 0)
                    {
                        auto_delay_time = time(NULL);
                    }
                    if(time(NULL) - auto_delay_time >= auto_wait_time)
                    {
                        iwaitime = 0;
                        XYLOG(XYLOG_SEVERITY_INFO, "摄像机自动上电延时时间超过%u秒！准备关闭摄像机！", (uint32_t)auto_wait_time);
                    }
                    else
                    {
                        m_sem.release();
                        continue;
                    }
                }
            }
            else
            {
                if(time(NULL) - start_delay_time < 0)
                {/* 防止等待关机期间，其他线程发生对时，改变了系统时间，导致长时间不会关摄像机电源*/
                    start_delay_time = time(NULL);
                }
                if(time(NULL) - start_delay_time >= close_delay_time)
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "摄像机空闲时间超过%u秒！准备关闭摄像机！", (uint32_t)close_delay_time);
                }
                else
                {
                    m_sem.release();
                    continue;
                }
            }
            if (state == PTZS_POWER_OFF)
            {
                closecmd = 0;
                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();
                closecmd = 0;
                state = PTZS_POWER_OFF;
                XYLOG(XYLOG_SEVERITY_INFO, "自动触发关闭云台电源！state=%d", state);
            }
            start_delay_time = 0;
            continue;
        }

        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_PHOTO_SELF_TESTING;
                        XYLOG(XYLOG_SEVERITY_INFO, "1、收到拍照指令，摄像机从关机状态改为自检状态！");

                        m_locker.lock();
                        m_cmds.insert(m_cmds.begin(), cmd);
                        m_locker.unlock();
                        m_sem.release();
                        continue;
                    }
                }
                if(cmd.photoParams->mPhotoInfo.scheduleTime == 0)
                {
                    if(1 == closecmd)
                    {
                        XYLOG(XYLOG_SEVERITY_INFO, "3、收到手动拍照指令，但同时后续收到关机指令，等待拍完照片再关机。state=%d！", state);
                    }
                    else
                    {
                        start_delay_time = time(NULL);
                        XYLOG(XYLOG_SEVERITY_INFO, "3、收到手动拍照指令，state=%d！", state);
                    }
                }
                else
                    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);
#if 0
                    if(START_ONCE_SELF == cmd.preset)
                    {
                        selfTestingStartTime = time(NULL);
                        selfTestingWaitTime = CAMERA_SELF_TEST_TIME;
                        state = PTZS_SELF_TESTING;
                        m_sem.release();
                        XYLOG(XYLOG_SEVERITY_INFO, "拍照调用200号预置点指令，摄像机启动一次性自检从拍照状态改为自检状态！取消拍照动作！设置的自检等待时间%u秒", (uint32_t)selfTestingWaitTime);
                        break;
                    }
#endif
                    PTZ_preset_start_time = time(NULL);
                    if(START_ONCE_SELF == cmd.preset)
                        PTZ_preset_wait_time = CAMERA_SELF_TEST_TIME;
                    else
                        PTZ_preset_wait_time = MOVE_PRESET_WAIT_TIME;
                    XYLOG(XYLOG_SEVERITY_INFO, "摄像机拍照前开始调用预置点%u！state=%d", (uint32_t)cmd.preset, state);
                    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, "摄像机拍照前调用预置点%u，收到移动结束应答！移动时长=%d秒 state=%d", (uint32_t)cmd.preset, (uint32_t)(time(NULL)-PTZ_preset_start_time), state);
                                break;
                            }
                        }
                        if(time(NULL) - PTZ_preset_start_time < 0)
                        {/* 防止等待关机期间，其他线程发生对时，改变了系统时间，导致长时间等待摄像机到达预置点*/
                            PTZ_preset_start_time = time(NULL);
                        }
                        if(time(NULL) - PTZ_preset_start_time >= PTZ_preset_wait_time)
                        {
                            XYLOG(XYLOG_SEVERITY_INFO, "摄像机拍照前调用预置点%u，摄像机在%u秒内未收到调用预置点结束应答！state=%d", (uint32_t)cmd.preset, (uint32_t)PTZ_preset_wait_time, state);
                            break;
                        }
                        std::this_thread::sleep_for(std::chrono::milliseconds(10));
                        photo_move_preset_time = time(NULL);
                    }
                }
                m_pPhoneDevice->TakePhotoWithNetCamera(cmd.photoParams->mPhotoInfo, cmd.photoParams->mPath, cmd.photoParams->mOsds, powerCtrl);
                state = PTZS_IDLE;
            }

                break;
            case PHOTO_OPEN_POWER:
                if (state == PTZS_POWER_OFF)
                {
                    if (!powerCtrl)
                    {
                        powerCtrl = std::make_shared<PlzCameraPowerCtrl>(0);
                        selfTestingStartTime = time(NULL);
                        selfTestingWaitTime = CAMERA_SELF_TEST_TIME;
                        state = PTZS_PHOTO_SELF_TESTING;
                        m_sem.release();
                        XYLOG(XYLOG_SEVERITY_INFO, "收到拍照指令开机，摄像机从关机状态改为自检状态！设置的自检等待时间%u秒", (uint32_t)selfTestingWaitTime);
                    }
                }
                else
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "收到拍照指令开机，摄像机处于state=%d！", state);
                }
                break;
            case OPEN_TOTAL:
                if (state == PTZS_POWER_OFF)
                {
                    if (!powerCtrl)
                    {
                        powerCtrl = std::make_shared<PlzCameraPowerCtrl>(0);
                        selfTestingStartTime = time(NULL);
                        selfTestingWaitTime = CAMERA_SELF_TEST_TIME;
                        state = PTZS_SELF_TESTING;
                        m_sem.release();
                        XYLOG(XYLOG_SEVERITY_INFO, "收到手动开机指令，摄像机从关机状态改为自检状态！设置的自检等待时间%u秒", (uint32_t)selfTestingWaitTime);
                    }
                }
                else
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "收到手动开机指令，摄像机处于state=%d！", state);
                }
                closecmd = 0;
                start_delay_time = time(NULL);
                XYLOG(XYLOG_SEVERITY_INFO, "收到手动打开摄像机指令，刷新关机计时初始值，state=%d", state);
                break;
            case CLOSE_TOTAL:
                if (state == PTZS_POWER_OFF)
                {
                    closecmd = 0;
                    XYLOG(XYLOG_SEVERITY_INFO, "收到关机指令，摄像机本来就处于关机状态！");
                    // Do Nothing
                }
                else if(PTZS_PHOTO_SELF_TESTING == state)
                {
                    closecmd = 1;
                    XYLOG(XYLOG_SEVERITY_INFO, "在拍照自检过程中收到关机指令，取消延时关机，转到自动关机处理！state=%d", state);
                }
                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;
                    }
                    closecmd = 0;
                    powerCtrl.reset();
                    state = PTZS_POWER_OFF;
                    XYLOG(XYLOG_SEVERITY_INFO, "关闭云台电源！state=%d", state);
                }
                start_delay_time = 0;
                break;
            default:
            {
                if (state == PTZS_POWER_OFF)
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "收到手动控制摄像机指令，摄像机处于关机状态,无法执行！");
                    CameraPhotoCmd(cmd.ts, cmd.channel, cmd.cmdidx, 0, cmd.preset, cmd.serfile, cmd.baud, cmd.addr);
                    break;
                }
                start_delay_time = time(NULL);
                XYLOG(XYLOG_SEVERITY_INFO, "收到手动控制摄像机指令，刷新关机计时初始值，state=%d", state);
                if(cmd.ts <= photo_move_preset_time)
                {
                    XYLOG(XYLOG_SEVERITY_INFO, "丢弃拍照调预置点期间收到的控制云台指令，指令时间" FMT_TIME_T "，拍照时间" FMT_TIME_T "！state=%d", cmd.ts, photo_move_preset_time, state);
                }
                else
                {
                    if((MOVE_PRESETNO == cmd.cmdidx) && (START_ONCE_SELF == cmd.preset))
                    {
                        selfTestingStartTime = time(NULL);
                        selfTestingWaitTime = CAMERA_SELF_TEST_TIME;
                        state = PTZS_SELF_TESTING;
                        m_sem.release();
                        XYLOG(XYLOG_SEVERITY_INFO, "收到调用200号预置点指令，摄像机启动一次性自检从当前状态改为自检状态！设置的自检等待时间%u秒", (uint32_t)selfTestingWaitTime);
                    }
                    CameraPhotoCmd(cmd.ts, cmd.channel, cmd.cmdidx, 0, cmd.preset, cmd.serfile, cmd.baud, cmd.addr);
                }
            }
                break;
        }
    }
}