/*
 *  Copyright (C) 2022  贺龙宇
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
#include "FYT2021.h"

#ifdef PC_DEBUG

#endif

int FYT2021()
{
    try{
    /* SerialPort */
#ifndef PC_DEBUG

#ifdef NUC
        system("echo nuc | sudo -S chmod 777 /dev/ttyUSB*");
#else
        system("echo dji | sudo -S chmod 777 /dev/ttyS0");
#ifdef MANIFOLD_G
	    system("echo dji | sudo -S chmod 777 /dev/ttyTHS2");
#endif
#endif

#if defined(NUC) || defined(NX)
        SerialPort serial_port("/dev/ttyUART");
#elif defined MANIFOLD_C   
        /*查看妙算上的线接在哪，uart0为/dev/ttyS0 uart1(只有G板有)为/dev/ttyTHS2*/
        SerialPort serial_port("/dev/ttyS0");
#else
        SerialPort serial_port("/dev/ttyTHS2");
#endif
        if(!serial_port.Init()){
            FYTLog->writeLog("Serial init failed",LOG_ERROR);
            return 1;
        }
        /*串口V2.0 取消与下位机握手*/
        //OpenDevice： 与下位机进行握手 
        //if(!selectDev->OpenDevice(serial_port,SELECTDEV_MCU)){
        //    FYTLog->writeLog("SerialPort Open Failed!",LOG_ERROR);
        //    return 1;
        //}
        
        Protocol protocol(serial_port);

        //接受下位机数据
        std::thread recieve(&Protocol::receiveData,&protocol);

        /*如果没收到下位机的数据，则阻塞直到第一次收到数据*/
        while(!serial_first_receive);
    
    
    

#endif


#ifdef PC_DEBUG
    mcu_data.color=hand_data.enemy_color;
#endif


#ifndef SERIAL_ONLY
        /* Video source */
        cameraParam.LoadParam();
#ifdef READ_FROM_VIDEO
        /*读取视频文件*/
        cv::VideoCapture *capture=new cv::VideoCapture("/home/dworry/video/Blue_Dim.avi");
#else
        FightingCapture* capture;
        capture = new FightingDahengCapture();
        printf("Camera name: %s\n", cameraParam.camera_name.c_str());
        if (capture && !capture->init()) {
            printf("Video source initialization failed.\n");
            FYTLog->writeLog("Video source initialization failed.",LOG_ERROR);
            delete capture;
            return 1;
        }
        printf("camera init success\n");
#endif

#ifdef VIDEO_WRITER
        time_t currentTime = std::time(NULL);
        char chCurrentTime[64];
        std::string stCurrentTime;
        std::strftime(chCurrentTime, sizeof(chCurrentTime), "%Y%m%d%H%M%S", std::localtime(&currentTime));
        stCurrentTime = chCurrentTime;

        cv::VideoWriter video_writer(stCurrentTime + ".avi",cv::VideoWriter::fourcc('M','J','P','G'), 24,
         cv::Size(cameraParam.resolution_width, cameraParam.resolution_height), true);
#endif

        /* ArmorDetector and RuneDetector */
        armorParam.LoadParam();
        runeParam.LoadParam();
        ArmorDetector armor_detector_t;
        Detection armor_detector_cnn;
        RuneDetector rune_detector;
        cv::Mat src;
        cv::Point3f target;
        SolvePos solvePos;
        std::vector<cv::Rect> box;
        std::vector<int> id;
#endif

#ifndef SERIAL_ONLY
        while (true) {

        /*用于做掉帧处理，保持时间为0.4s*/
#ifdef MANIFOLD_G
            static int lost_cnt=12;
#elif defined NUC
            static int lost_cnt=16;
#elif defined MANIFOLD_C
            static int lost_cnt=12;
#elif defined NX
            static int lost_cnt=16;
#endif
            static cv::Point3f last_target={0,0,-1};
            if (capture->read(src)) {
#ifdef READ_FROM_VIDEO

                /*从某一帧开始播放*/
                static int start_index=0;
                static int count=0;
                count++;
                std::cout<<count<<std::endl;
                if(count<start_index) {
                    continue;
                }
                // cv::resize(src,src,cv::Size(640,480));
#endif
#ifdef DEBUG
    /*在DEBUG模式下，每0.5s重新读取一次参数*/
    #ifdef NUC
                int frame=100;
    #endif
    #ifdef MANIFOLD_C
                int frame=50;
    #endif
    #ifdef MANIFOLD_G
                int frame=15;
    #endif
    #ifdef NX
                int frame=20;
    #endif
                static int cnt=0;
                cnt++;
                if(cnt==frame){
                    armorParam.LoadParam();
                    runeParam.LoadParam();
                    cameraParam.LoadParam();
                    cnt=0;
                }
#endif 
                
                /*******计时起点**************/
                timespec t0, t_final;
                clock_gettime(CLOCK_MONOTONIC,&t0);
                /***************************/
                
                
#ifndef READ_FROM_VIDEO
                //曝光时间调整
                // if(!capture->AdjustExplosureTime(src)) continue;
                capture->AdjustExplosureTime(src);
#endif
                if(mcu_data.id==3 || mcu_data.id==4 || mcu_data.id==5){
                    if(mcu_data.state==State::ARMOR_STATE)/*装甲板识别模式*/
                    {
                        static cv::Point3f last_target={0,0,-1};
#ifdef TRADITION
                        if(!armor_detector_t.DetectArmor(src,box,id))
#else

		                if(!armor_detector_cnn.Detecting(src,box,id))
#endif
                        {
                            target = { 0, 0, -1 }; 
                        }
                        else{
                            
                            target=solvePos.GetTarget(src,box,id);
                        }
                        //如果target.z小于0，说明没检测到目标，进入掉帧处理状态
                        //如果cnt小于10，则一直发送上一时刻目标并令cnt++
                        //如果大于10，则进入下一个判断
                        if(target.z<0 && lost_cnt<10){
                            lost_cnt++;
                            target=last_target;
                        }
                        //如果target.z大于0，说明检测到目标，重置cnt，并令last_target=target
                        else if(target.z>0){
                            lost_cnt=0;
                            last_target=target;
                        }
                        //如果都不满足，即没检测到目标且cnt>10，则认为是目标丢失，直接发送0 0 -1
                        //(此时target值即为0 0 -1 因此直接发送target即可)
                        //如果此时是反陀螺模式，则提醒接下来反陀螺需要初始化，调用TargetLost函数
                        else{
                            solvePos.TargetLost();
                        }

                        // std::cout<<"distance:"<<target.z<<std::endl;
                        id.clear();
                        box.clear();
                    }
                    /*大符、小符同样算法。之后需要改动在设立额外分支*/
                    /*直接在Rune模块中区分大小符*/
                    else{ 


                        /*RuneDetector*/
                        if(!rune_detector.DetectRune(src,target))
                            target={0,0,-1};
                    
                    }
                }

                /*对于非步兵的兵种，只使用自瞄模式*/
                else{
                    static cv::Point3f last_target={0,0,-1};
#ifdef TRADITION
                    if(!armor_detector_t.DetectArmor(src,box,id))
#else

		            if(!armor_detector_cnn.Detecting(src,box,id))
#endif
                    {
                        target = { 0, 0, -1 }; 
                    }
                    else{
                        target=solvePos.GetTarget(src,box,id);
                    }
                    //如果target.z小于0，说明没检测到目标，进入掉帧处理状态
                    //如果cnt小于10，则一直发送上一时刻目标并令cnt++
                    //如果大于10，则进入下一个判断
                    if(target.z<0 && lost_cnt<10){
                        lost_cnt++;
                        target=last_target;
                    }
                    //如果target.z大于0，说明检测到目标，重置cnt，并令last_target=target
                    else if(target.z>0){
                        lost_cnt=0;
                        last_target=target;
                    }
                    //如果都不满足，即没检测到目标且cnt>10，则认为是目标丢失，直接发送0 0 -1
                    //(此时target值即为0 0 -1 因此直接发送target即可)
                    //如果此时是反陀螺模式，则提醒接下来反陀螺需要初始化，调用TargetLost函数
                    else{
                        solvePos.TargetLost();
                    }

                    // std::cout<<"distance:"<<target.z<<std::endl;
                    id.clear();
                    box.clear();
                }


                /******计时终点******************/
                clock_gettime(CLOCK_MONOTONIC,&t_final);
                float exe_time_ms=((t_final.tv_sec-t0.tv_sec)*1e9+t_final.tv_nsec-t0.tv_nsec)/1e6;
                /******************************/


                /*已调试*/
#ifdef MANIFOLD_G
                /*G板定帧为30帧*/
                if(exe_time_ms<33.33) usleep((33.33-exe_time_ms)*1e3);
#endif
#ifdef MANIFOLD_C
                /*C板定帧为100帧*/
                if(exe_time_ms<10) usleep((10-exe_time_ms)*1e3);
#endif
#ifdef NUC
                /*NUC rune定帧为100帧；armor 40 fps*/
                if(mcu_data.state!=State::ARMOR_STATE){
		            if(exe_time_ms<10) 
	                {
		                usleep((10-exe_time_ms)*1e3);
		            }
                }
		        else{
		            if(exe_time_ms<25){
		                usleep((25-exe_time_ms)*1e3);
		            }
                }

                
#endif
#ifdef NX
            if(exe_time_ms<25) usleep((25-exe_time_ms)*1e3);
#endif
/*默认为最差情况，即妙算G*/
#ifndef MANIFOLD_G
#ifndef MANIFOLD_C
#ifndef NUC
#ifndef NX
                 if(exe_time_ms<33.33) usleep((33.33-exe_time_ms)*1e3);
#endif
#endif
#endif
#endif
#ifdef CALC_TIME
                /*调试使用*/
                clock_gettime(CLOCK_MONOTONIC,&t_final);
                exe_time_ms=((t_final.tv_sec-t0.tv_sec)*1e9+t_final.tv_nsec-t0.tv_nsec)/1e6;
                std::cout<<exe_time_ms<<"ms"<<std::endl;
                /*******************************/
#endif
                
#ifndef PC_DEBUG
                protocol.sendTarget(target);
#endif

#ifdef PC_DEBUG
               printf("sendTarget:(%f,%f,%f)\n",target.x,target.y,target.z);
#endif


        
#ifdef DEBUG
                // cv::imshow("src", src);

#ifndef READ_FROM_VIDEO
                auto c=cv::waitKey(1);
#else 
                auto c=cv::waitKey(40);
#endif

                if(c=='a') cv::waitKey(0);
#endif
          
#ifdef VIDEO_WRITER
            video_writer.write(src);
#endif
            }//if(capture->read(src))
             else
                break;

        }//while(true)
        delete capture;
        return 0;
#endif

#ifdef SERIAL_ONLY
        cv::Point3f target={0,0,-1};
        while(true){
            protocol.sendTarget(target);
            usleep(10000);
        }
#endif
    }//FYT2021()
    catch(cv::Exception err){
        FYTLog->writeLog(err.msg,LOG_ERROR);
        return 1;
    }
    catch(...){
        return 1; 
    }
}

int read_video_for_cnn()
{
    cv::VideoCapture cap;
    cap.open("1.avi");
    
    if(!cap.isOpened()) return 0;
    std::string tmp_state ="ARMOR_STATE";
    cv::Mat frame;
    Detection armor_detector_cnn;
    std::vector<int> id;
    std::vector<cv::Rect> box;

    while(1)
    {
        cap>>frame;
        if(frame.empty()) break;
        armor_detector_cnn.Set_WH(frame.size().width, frame.size().height);
	if(tmp_state =="ARMOR_STATE") armor_detector_cnn.Detecting(frame, box, id);
	else armor_detector_cnn.Detecting(frame, box, id);
	
	id.clear();
	box.clear(); 
    }
    cap.release();
    return 0;
}

int main(int argc, char* argv[])
{
    if(argc==2)
    {
        read_video_for_cnn();
    }
    else
    {
        while (true)
        {
            FYT2021();
            printf("### Error, start again! ###\n\n");
            cv::waitKey(2000);
        }
    }   
    return 0;
}