#include "vertix.h"
#include <stdlib.h>
#include "pose_body.h"

Polygon workspace;

void dump_polygon_to_file(char *filename) {
    FILE *fp = fopen(filename, "w+");
    if (fp == NULL)
    {
        printf("failed to open fp\n");
        return;
    }

    char s[100];
    snprintf(s, 100, "rect: [");
    fwrite(s, strlen(s), 1, fp);
    for (int i = 0; i < workspace.numVertix; i++)
    {
        if (i != workspace.numVertix - 1)
        {
            snprintf(s, 100, "(%f, %f), ", workspace.vertix[i].x, workspace.vertix[i].y);
        }
        else
        {
            snprintf(s, 100, "(%f, %f)", workspace.vertix[i].x, workspace.vertix[i].y);
        }
        fwrite(s, strlen(s), 1, fp);
    }
    const char *suffix = "]\n";
    fwrite(suffix, strlen(suffix), 1, fp);
    fclose(fp);

}

void init_polygon(float *datas, int point_size)
{
    if (point_size == 0)
    {
        workspace.numVertix = 0;
#ifdef DUMP_POLYGON_INFO
        char filename[128];
        snprintf(filename, 128, "/usr/data/camera/%s/dump_rect.txt", CameraID);
        dump_polygon_to_file(filename);
#endif
        return;
    }
    if (point_size % 2 != 0)
    {
        printf("point size error");
        exit(0);
    }
    workspace.numVertix = point_size / 2;
    workspace.vertix = (Point *)malloc(sizeof(Point) * point_size);
    for (int i = 0; i < point_size / 2; i++)
    {
        workspace.vertix[i] = Point{
            x : datas[i * 2],
            y : datas[i * 2 + 1],
        };
    }

#ifdef DUMP_POLYGON_INFO
    char filename[128];
    snprintf(filename, 128, "/usr/data/camera/%s/dump_rect.txt", CameraID);
    dump_polygon_to_file(filename);
#endif
}

void dump_box_info(bool is_in, int left, int top, int right, int bottom)
{
    char filename[128];
    snprintf(filename, 128, "/usr/data/camera/%s/dump_rect.txt", CameraID);
    FILE *fp = fopen(filename, "a+");
    if (fp == NULL)
    {
        printf("failed to open fp\n");
        return;
    }

    float y = (top + bottom) / 2.0;
    float x = (left + right) / 2.0;
    char s[] = {0};
    if (is_in)
    {
        snprintf(s, 1024, "in: [%d, %d, %d, %d](%f, %f)\n",
                 left, top, right, bottom, x, y);
    }
    else
    {
        snprintf(s, 1024, "not in: [%d, %d, %d, %d](%f, %f)\n",
                 left, top, right, bottom, x, y);
    }
    fwrite(s, strlen(s), 1, fp);
    fclose(fp);
}

bool is_point_in_workspace(int left, int top, int right, int bottom)
{

    float y = (top + bottom) / 2.0;
    float x = (left + right) / 2.0;
    int n = workspace.numVertix;
    if (n == 0)
    {
        // all area
#ifdef DUMP_POLYGON_INFO
        dump_box_info(true, left, top, right, bottom);
#endif
        printf("all workspace\n");
        return true;
    }

    int i, j;
    bool inside = false;

    for (i = 0, j = n - 1; i < n; j = i++)
    {
        if ((workspace.vertix[i].y > y) != (workspace.vertix[j].y > y))
        {
            // this point is between the Point[j] and Point[i] in y axis
            float deltax = (workspace.vertix[j].x - workspace.vertix[i].x);
            float deltay = (workspace.vertix[j].y - workspace.vertix[i].y);
            float ratio = deltax / deltay;

            if (x < (ratio * (y - workspace.vertix[i].y) + workspace.vertix[i].x))
            {
                inside = !inside;
            }
        }
    }

#ifdef DUMP_POLYGON_INFO
    dump_box_info(inside, left, top, right, bottom);
#endif
    return inside;
}