// Baseline V4L2/CAN + 0920 TensorRT/NPP, with a narrow ctypes ABI.
#include "v4l2_capture.h"
#include "can_comm.h"
#include "detector.hpp"
#include "uyvy_converter.hpp"
#include <linux/videodev2.h>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <cstring>
#include <mutex>
#include <thread>

namespace {
using Clock=std::chrono::steady_clock;
double nowMs(){return std::chrono::duration<double,std::milli>(Clock::now().time_since_epoch()).count();}
thread_local std::string error;
std::mutex canMutex;
int positions[4]={-1,-1,-1,-1};
double canTime=0;
void receiveCan(int64_t, uint32_t id, const uint8_t* data, uint32_t len){
    if(id!=0x391||len<4)return;
    std::lock_guard<std::mutex> lock(canMutex);
    // Baseline comments specify bytes 1.0..4.7 (not overlapping bits 0,1,2,3).
    for(int i=0;i<4;++i)positions[i]=data[i];
    canTime=nowMs();
}
struct Engine {
    fangjia::TrtLogger logger;
    std::unique_ptr<cms::Detector> detector;
    std::unique_ptr<fangjia::RtmwEngine> pose;
    cudaStream_t stream{};
    int width,height,maxPeople;
    Engine(const char* dir,int w,int h,int n,float confidence):width(w),height(h),maxPeople(n){
        detector=std::make_unique<cms::Detector>(std::filesystem::path(dir)/"yolo11s.engine",logger,confidence,n);
        pose=std::make_unique<fangjia::RtmwEngine>(std::filesystem::path(dir)/"rtmw_l_384.engine",logger,w,h,n);
        CUDA_CHECK(cudaStreamCreateWithFlags(&stream,cudaStreamNonBlocking));
        try {pose->warmup(stream);detector->infer(cv::Mat(h,w,CV_8UC3,cv::Scalar()),stream);}catch(...){cudaStreamDestroy(stream);throw;}
    }
    ~Engine(){if(stream)cudaStreamDestroy(stream);}
};
struct Camera {
    V4l2Capture capture;
    std::unique_ptr<cms::UyvyConverter> converter;
    std::thread worker;
    std::mutex mutex;
    std::condition_variable ready;
    std::atomic<bool> stop{false};
    std::shared_ptr<const cms::BgrImage> latest;
    uint64_t generation=0, consumed=0, sequence=0;
    double timestamp=0,driverTimestamp=0;
    int width,height;
    bool canEnabled=false;
    std::string failure;
    Camera(const char* device,int w,int h,int fps,bool can):width(w),height(h){
        if(!capture.open(device)||!capture.setFormat(w,h,V4L2_PIX_FMT_UYVY))throw std::runtime_error("Camera open/format failed");
        if(capture.width()!=uint32_t(w)||capture.height()!=uint32_t(h))throw std::runtime_error("Camera negotiated unexpected dimensions");
        capture.setFps(fps);
        if(!capture.initMmap(4)||!capture.startStreaming())throw std::runtime_error("Camera streaming failed");
        converter=std::make_unique<cms::UyvyConverter>(w,h);
        if(can){CanComm::set_msg_callback(receiveCan);canEnabled=CanComm::can_start();}
        try {worker=std::thread([this]{loop();});}catch(...){if(canEnabled)CanComm::can_stop();throw;}
    }
    ~Camera(){stop=true;ready.notify_all();if(worker.joinable())worker.join();capture.close();if(canEnabled)CanComm::can_stop();}
    void loop(){
        try {
            int misses=0;
            while(!stop){
                V4l2Capture::Frame frame;
                if(!capture.getFrame(frame,1000)){if(++misses>=5)throw std::runtime_error("Camera timed out for 5 seconds");continue;}
                double received=nowMs();
                misses=0;
                auto stride=frame.bytes_per_line?frame.bytes_per_line:width*2;
                if(frame.size<size_t(stride)*height){capture.releaseFrame(frame);continue;}
                std::shared_ptr<const cms::BgrImage> image;
                try {image=converter->convert(frame.data,stride);}catch(...){capture.releaseFrame(frame);throw;}
                auto seq=frame.sequence;double driverTs=frame.timestamp_us/1000.0;
                capture.releaseFrame(frame);
                {std::lock_guard<std::mutex> lock(mutex);latest=std::move(image);sequence=seq;timestamp=received;driverTimestamp=driverTs;++generation;}
                ready.notify_one();
            }
        }catch(std::exception const&e){std::lock_guard<std::mutex> lock(mutex);failure=e.what();stop=true;ready.notify_all();}
    }
};
}
extern "C" {
const char* vap_error(){return error.c_str();}
void* vap_engine(const char* dir,int w,int h,int n,float confidence){try {return new Engine(dir,w,h,n,confidence);}catch(std::exception const&e){error=e.what();return nullptr;}}
void vap_engine_close(void* p){delete static_cast<Engine*>(p);}
int vap_detect(void* p,unsigned char* pixels,int w,int h,float* out){try {
    auto& e=*static_cast<Engine*>(p);auto boxes=e.detector->infer(cv::Mat(h,w,CV_8UC3,pixels),e.stream);
    for(size_t i=0;i<boxes.size();++i){auto& b=boxes[i];float v[]={b.x1,b.y1,b.x2,b.y2,b.score};std::memcpy(out+i*5,v,sizeof(v));}
    return boxes.size();
}catch(std::exception const&e){error=e.what();return -1;}}
int vap_pose(void* p,unsigned char* pixels,int w,int h,const float* boxes,int count,float* out){try {
    auto& e=*static_cast<Engine*>(p);
    if(count<0||count>e.maxPeople||w>e.width||h>e.height)throw std::runtime_error("Pose input exceeds configured capacity");
    std::vector<fangjia::Box> input;for(int i=0;i<count;++i)input.push_back({boxes[i*4],boxes[i*4+1],boxes[i*4+2],boxes[i*4+3],1,0});
    auto poses=e.pose->infer(cv::Mat(h,w,CV_8UC3,pixels),input,e.stream);
    for(size_t i=0;i<poses.size();++i)for(int k=0;k<133;++k){auto& pt=poses[i].keypoints[k];auto* v=out+(i*133+k)*3;v[0]=pt.x;v[1]=pt.y;v[2]=pt.score;}
    return poses.size();
}catch(std::exception const&e){error=e.what();return -1;}}
void* vap_camera(const char* device,int w,int h,int fps,int can){try{return new Camera(device,w,h,fps,can);}catch(std::exception const&e){error=e.what();return nullptr;}}
void vap_camera_close(void* p){delete static_cast<Camera*>(p);}
int vap_read(void* p,unsigned char* out,double* meta){try {
    auto& c=*static_cast<Camera*>(p);std::shared_ptr<const cms::BgrImage> image;
    {std::unique_lock<std::mutex> lock(c.mutex);
    if(!c.ready.wait_for(lock,std::chrono::seconds(6),[&]{return c.generation>c.consumed||c.stop;}))throw std::runtime_error("No camera frame");
    if(!c.failure.empty())throw std::runtime_error(c.failure);
    if(c.stop)return 0;
    image=c.latest;meta[0]=c.sequence;meta[1]=c.timestamp;meta[2]=c.generation-c.consumed-1;meta[3]=c.driverTimestamp;c.consumed=c.generation;}
    std::memcpy(out,image->pixels.data(),image->pixels.size());return 1;
}catch(std::exception const&e){error=e.what();return -1;}}
void vap_can(double* out){std::lock_guard<std::mutex> lock(canMutex);for(int i=0;i<4;++i)out[i]=positions[i];out[4]=canTime?nowMs()-canTime:-1;}
}