#include "arpa/inet.h"
#include "boost/filesystem.hpp"
#include "boost/thread/mutex.hpp"
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.h"
#include "opencv2/opencv.hpp"
#include "ros/ros.h"
#include "sys/socket.h"
#include "time.h"

using namespace std;
using namespace cv;

#define font CV_FONT_HERSHEY_SIMPLEX
#define color Scalar(0, 255, 0)

//-----Parameter
string capture_directory;
string lidar_ip;
int video_width;
int video_height;
//-----Timer
ros::Timer tim_15hz;
ros::Timer tim_30hz;
//-----Subscriber
image_transport::Subscriber sub_frame_bgr[3];
//-----Mutex
boost::mutex mutex_frame_bgr[3];

//Keperluan koneksi UDP
int sockfd;
struct sockaddr_in client_addr;
struct sockaddr_in dummy_addr;
socklen_t dummy_socklen;
//Panjang data dan buffer UDP
uint8_t tx_len, tx_buffer[512];
uint8_t rx_len, rx_buffer[512];
//Container frame
Mat frame_bgr[3];
//Status algoritma
uint8_t capture_ready = 0;
uint64_t capture_number = 0;
//Struktur data LiDAR
typedef struct
{
    uint32_t VC;
    uint32_t SN;
    uint8_t Class;
    uint8_t nAxleGroup;
    uint8_t AxleGroup[5];
    uint8_t Type;
} vehicle;
typedef struct
{
    uint32_t STAT;
    uint32_t NSN;
    uint32_t nVeh;
    vector<vehicle> Vehicle;
} lidar;

//-----Protype
bool checkFile(string filePath);
bool checkDirectory(string filePath);
void createDirectory(string filePath);
string date();
string time();

void cllbck_tim_15hz(const ros::TimerEvent &event)
{
    *(uint32_t *)&tx_buffer[0] = 0xDDCCBBAA;
    *(uint32_t *)&tx_buffer[4] = 0x00000001;
    *(uint32_t *)&tx_buffer[8] = 0x00000000;
    tx_len = sendto(sockfd, tx_buffer, 12, MSG_DONTWAIT, (struct sockaddr *)&client_addr, sizeof(client_addr));
}

void cllbck_tim_30hz(const ros::TimerEvent &event)
{
    static Mat canvas;

    static vehicle last_Vehicle;

    static uint8_t lidarPassingStatus = 0;
    static uint8_t prev_lidarPassingStatus = 0;

    string datetime_entry;
    string datetime_exit;

    rx_len = recvfrom(sockfd, rx_buffer, 512, MSG_DONTWAIT, (struct sockaddr *)&dummy_addr, &dummy_socklen);

    if (capture_ready == 0x07)
    {
        if (*(uint32_t *)&rx_buffer[0] == 0xDDCCBBAA)
        {
            prev_lidarPassingStatus = lidarPassingStatus;

            lidar Lidar;
            memcpy(&Lidar.STAT, rx_buffer + 4, 4);
            memcpy(&Lidar.NSN, rx_buffer + 8, 4);
            memcpy(&Lidar.nVeh, rx_buffer + 12, 4);
            for (int i = 0; i < Lidar.nVeh; i++)
            {
                vehicle Vehicle;
                memcpy(&Vehicle.VC, rx_buffer + i * 8 + 16, 4);
                memcpy(&Vehicle.SN, rx_buffer + i * 8 + 20, 4);
                Vehicle.Class = (Vehicle.VC & 0x0000000F) >> 0;
                Vehicle.nAxleGroup = (Vehicle.VC & 0x000000F0) >> 4;
                Vehicle.AxleGroup[0] = (Vehicle.VC & 0x00000F00) >> 8;
                Vehicle.AxleGroup[1] = (Vehicle.VC & 0x0000F000) >> 12;
                Vehicle.AxleGroup[2] = (Vehicle.VC & 0x000F0000) >> 16;
                Vehicle.AxleGroup[3] = (Vehicle.VC & 0x00F00000) >> 20;
                Vehicle.AxleGroup[4] = (Vehicle.VC & 0x0F000000) >> 24;
                Vehicle.Type = (Vehicle.VC & 0xF0000000) >> 28;
                Lidar.Vehicle.push_back(Vehicle);

                last_Vehicle = Vehicle;
            }

            lidarPassingStatus = (Lidar.STAT & 0x00000001) >> 0;
        }

        if (lidarPassingStatus == 1 && prev_lidarPassingStatus == 0)
        {
            //Mengahapus buffer canvas sebelumnya
            canvas = Mat::zeros(Size(3 * video_width, 2 * video_height), CV_8UC3);

            //Memasang gambar saat rising edge lidar
            for (int i = 0; i < 3; i++)
                canvas(Rect(i * video_width, 0 * video_height, video_height, video_height)) = frame_bgr[i];

            //Mencatat tanggal dan jam kendaraan masuk LiDAR
            datetime_entry = date() + " " + time();
        }

        if (lidarPassingStatus == 0 && prev_lidarPassingStatus == 1)
        {
            //Memasang gambar saat falling edge lidar
            for (int i = 0; i < 3; i++)
                canvas(Rect(i * video_width, 1 * video_height, video_height, video_height)) = frame_bgr[i];

            //Mencatat tanggal dan jam kendaraan keluar LiDAR
            datetime_exit = date() + " " + time();

            putText(canvas, format("Image #{0:05d}", capture_number), Point(20, 20), font, 0.5, color, 1);
            putText(canvas, format("Golongan {0:01d}", last_Vehicle.Class), Point(20, 55), font, 1, color, 2);
            putText(canvas, format("-> {0}", datetime_entry), Point(20, 80), font, 0.5, color, 1);
            putText(canvas, format("<- {0}", datetime_exit), Point(20, 100), font, 0.5, color, 1);

            //Membuat direktori baru jika belum tersedia
            if (checkDirectory(capture_directory + '/' + date()) == false)
                createDirectory(capture_directory + '/' + date());

            //Mem-parsing nama file gambar
            char filename[128];
            sprintf(filename, "%s/%s/%06d.jpg", capture_directory.c_str(), date().c_str(), capture_number++);
        }
    }
}

void cllbck_sub_frame_bgr_cctv0(const sensor_msgs::ImageConstPtr &msg)
{
    mutex_frame_bgr[0].lock();
    frame_bgr[0] = cv_bridge::toCvCopy(msg, "bgr8")->image;
    mutex_frame_bgr[0].unlock();

    capture_ready |= 0x01;
}

void cllbck_sub_frame_bgr_cctv1(const sensor_msgs::ImageConstPtr &msg)
{
    mutex_frame_bgr[1].lock();
    frame_bgr[1] = cv_bridge::toCvCopy(msg, "bgr8")->image;
    mutex_frame_bgr[1].unlock();

    capture_ready |= 0x02;
}

void cllbck_sub_frame_bgr_cctv2(const sensor_msgs::ImageConstPtr &msg)
{
    mutex_frame_bgr[2].lock();
    frame_bgr[2] = cv_bridge::toCvCopy(msg, "bgr8")->image;
    mutex_frame_bgr[2].unlock();

    capture_ready |= 0x04;
}

bool checkFile(std::string filePath)
{
    boost::filesystem::path pathObj(filePath);
    if (boost::filesystem::exists(pathObj) && boost::filesystem::is_regular_file(pathObj))
        return true;
    return false;
}

bool checkDirectory(std::string filePath)
{
    boost::filesystem::path pathObj(filePath);
    if (boost::filesystem::exists(pathObj) && boost::filesystem::is_directory(pathObj))
        return true;
    return false;
}

void createDirectory(string filePath)
{
    boost::filesystem::create_directory(filePath);
}

string date()
{
    time_t Time = time(NULL);
    struct tm Tm = *localtime(&Time);

    char buffer[11];
    sprintf(buffer, "%04d-%02d-%02d", Tm.tm_year + 1900, Tm.tm_mon + 1, Tm.tm_mday);
    return buffer;
}

string time()
{
    time_t Time = time(NULL);
    struct tm Tm = *localtime(&Time);

    char buffer[11];
    sprintf(buffer, "%02d-%02d-%02d", Tm.tm_hour, Tm.tm_min, Tm.tm_sec);
    return buffer;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lidar");

    ros::NodeHandle NH;
    image_transport::ImageTransport IT(NH);
    ros::MultiThreadedSpinner MTS;

    //-----Parameter
    NH.getParam("/capture_directory", capture_directory);
    NH.getParam("/lidar_ip", lidar_ip);
    NH.getParam("/video/width", video_width);
    NH.getParam("/video/height", video_height);
    //-----Timer
    tim_15hz = NH.createTimer(ros::Duration(0.067), cllbck_tim_15hz);
    tim_30hz = NH.createTimer(ros::Duration(0.033), cllbck_tim_30hz);
    //-----Subscriber
    sub_frame_bgr[0] = IT.subscribe("cctv0/frame_bgr", 1, cllbck_sub_frame_bgr_cctv0);
    sub_frame_bgr[1] = IT.subscribe("cctv1/frame_bgr", 1, cllbck_sub_frame_bgr_cctv1);
    sub_frame_bgr[2] = IT.subscribe("cctv2/frame_bgr", 1, cllbck_sub_frame_bgr_cctv2);

    //Menghentikan timer sebelum membuka koneksi UDP
    tim_15hz.stop();
    tim_30hz.stop();
    //Inisialisasi koneksi UDP ke LiDAR
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    client_addr.sin_family = AF_INET;
    client_addr.sin_addr.s_addr = inet_addr(lidar_ip.c_str());
    client_addr.sin_port = htons(3018);
    //Memulai timer setelah membuka koneksi UDP
    tim_15hz.start();
    tim_30hz.start();

    ROS_WARN("%s", lidar_ip.c_str());
    ROS_WARN("%d", sockfd);

    MTS.spin();
}