#include <assert.h>
#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>
#include <cglm/cglm.h>
#include <string.h>
#include <limits.h>

#include "cad/construction.h"
#include "cad/solve.h"
#include "cad/svg.h"

#define SETSIGN(b, v) ((v) * ((2 * (b)) - 1))

#define DEG(x) ((x) * M_PI / 180.0)

#define PP_DISTANCE(C1, C2, D) \
	{ \
		.type = CT_POINT_POINT_DISTANCE, \
		.v = D, \
		.c1 = C1, \
		.c2 = C2, \
	}

#define PL_DISTANCE(C1, C2, D) \
	{ \
		.type = CT_POINT_LINE_DISTANCE, \
		.v = D, \
		.c1 = C1, \
		.c2 = C2, \
	}

#define POINT_ON_LINE(C1, C2) \
	PL_DISTANCE(C1, C2, 0)

#define LL_ANGLE(C1, C2, D) \
	{ \
		.type = CT_LINE_LINE_ANGLE, \
		.v = D, \
		.c1 = C1, \
		.c2 = C2, \
	}

#define CEND() \
	{ \
		.type = CT_END, \
	}

void plot_point(struct point p) {
	printf("<circle cx=\"%f\" cy=\"%f\" r=\".4\" fill=\"black\" />\n", p.pos[0], -p.pos[1]);
}

void plot_line(struct line l) {
	char *style_str = "stroke=\"black\" stroke-width=\"0.2\"";
	if(fabs(l.norm[0]) < fabs(l.norm[1])) {
		double minx = -50;
		double maxx =  50;

		double miny = -(l.norm[0] * minx + l.C) / l.norm[1];
		double maxy = -(l.norm[0] * maxx + l.C) / l.norm[1];
		printf("<line %s x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" />\n", style_str, minx, -miny, maxx, -maxy);
	} else {
		double miny = -50;
		double maxy =  50;

		double minx = -(l.norm[1] * miny + l.C) / l.norm[0];
		double maxx = -(l.norm[1] * maxy + l.C) / l.norm[0];
		printf("<line %s x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" />\n", style_str, minx, -miny, maxx, -maxy);
	}

	double d0 = glm_vec2_norm2(l.norm);

	vec2 p0 = {0, 0};
	glm_vec2_mulsubs(l.norm, l.C, p0);
	glm_vec2_divs(p0, d0, p0);

	vec2 p1;
	glm_vec2_add(p0, l.norm, p1);

	printf("<line x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" stroke=\"black\" stroke-width=\".2\" />\n", p0[0], -p0[1], p1[0], -p1[1]);
}

void plot_circle(struct circle c) {
	printf("<circle cx=\"%f\" cy=\"%f\" r=\"%f\" fill=\"none\" stroke=\"black\" stroke-width=\".1\" />\n", c.center[0], -c.center[1], c.radius);
}

void plot_generic(struct element e) {
	switch(e.type) {
		case ETYPE_VALUE:
			break;
		case ETYPE_CIRCLE:
			plot_circle(e.circle);
			break;
		case ETYPE_POINT:
			plot_point(e.point);
			break;
		case ETYPE_LINE:
			plot_line(e.line);
			break;
	}
}

struct smooth_line {
	struct component l1;
	struct component l2;

	struct component corner;
	struct component corner_center;
	struct component end;
	struct component start;

	struct component perp1;
	struct component perp2;

	struct component corner_start;
	struct component corner_end;
};

struct smooth_line init_smooth_line() {
	return  (struct smooth_line){
		.l1 = {.type = COM_LINE},
		.l2 = {.type = COM_LINE},
		.corner = {.type = COM_POINT},
		.corner_center = {.type = COM_POINT},
		.end = {.type = COM_POINT},
		.start = {.type = COM_POINT},

		.perp1 = {.type = COM_LINE},
		.perp2 = {.type = COM_LINE},

		.corner_start = {.type = COM_POINT},
		.corner_end = {.type = COM_POINT},
	};
}

struct box {
	struct component corner[4];
	struct component side[4];
};

struct box init_box() {
	return (struct box){
		.corner = {
			{.type = COM_POINT},
			{.type = COM_POINT},
			{.type = COM_POINT},
			{.type = COM_POINT},
		},
		.side = {
			{.type = COM_LINE},
			{.type = COM_LINE},
			{.type = COM_LINE},
			{.type = COM_LINE},
		},
	};
}

struct mid {
	struct component l1;
	struct component l2;

	struct component x1;

	struct component p1;

	struct component p;
};

struct mid init_mid() {
	return (struct mid) {
		.l1 = {.type = COM_LINE},
		.l2 = {.type = COM_LINE},

		.x1 = {.type = COM_POINT},

		.p1 = {.type = COM_LINE},

		.p = {.type = COM_POINT},
	};
}

int main(int argc, char *argv[]) {
	struct constraints constraints = {};
	struct topology topo = {};

	struct component components[] = {
		{.type = COM_POINT},
		{.type = COM_POINT},
		{.type = COM_POINT},
		{.type = COM_LINE},
		{.type = COM_LINE},
		{.type = COM_POINT},
	};

	add_fragment(&topo, (struct topology_elem[]){
		TOPO_MOVETO(&components[1]),
		TOPO_LINETO(&components[5]),
		TOPO_LINETO(&components[0]),
		TOPO_LINETO(&components[2]),
		TOPO_LINETO(&components[1]),

		TOPO_END(),
	});

	add_constraint(&constraints, (struct constraint[]){
		PP_DISTANCE(&components[0], &components[1], 13),
		PP_DISTANCE(&components[1], &components[2], 7),
		PP_DISTANCE(&components[2], &components[0], 7),

		POINT_ON_LINE(&components[0], &components[3]),
		POINT_ON_LINE(&components[1], &components[3]),

		LL_ANGLE(&components[3], &components[4], M_PI/1.7),

		POINT_ON_LINE(&components[4], &components[1]),
		POINT_ON_LINE(&components[4], &components[5]),

		PP_DISTANCE(&components[0], &components[5], 12.8),
		CEND(),
	});

	struct smooth_line line = init_smooth_line();

	add_fragment(&topo, (struct topology_elem[]){
		TOPO_MOVETO(&line.start),
		TOPO_LINETO(&line.corner_start),
		TOPO_ARCTO(&line.corner_center, &line.corner_end),
		TOPO_LINETO(&line.end),

		TOPO_END(),
	});

	alias_point(&constraints, &line.start, &components[5]);

	add_constraint(&constraints, (struct constraint[]){
		LL_ANGLE(&components[3], &line.l1, DEG(90)),

		POINT_ON_LINE(&line.start, &line.l1),

		LL_ANGLE(&line.l2, &line.l1, DEG(90)),
		POINT_ON_LINE(&line.corner, &line.l1),
		POINT_ON_LINE(&line.corner, &line.l2),

		POINT_ON_LINE(&line.end, &line.l2),
		PL_DISTANCE(&line.end, &line.l1, 10),
		PL_DISTANCE(&line.end, &components[4], 5),

		PL_DISTANCE(&line.corner_center, &line.l1, 2),
		PL_DISTANCE(&line.corner_center, &line.l2, -2),

		LL_ANGLE(&line.l1, &line.perp1, DEG(90)),
		POINT_ON_LINE(&line.corner_center, &line.perp1),
		POINT_ON_LINE(&line.corner_start, &line.perp1),
		POINT_ON_LINE(&line.corner_start, &line.l1),

		LL_ANGLE(&line.l2, &line.perp2, DEG(90)),
		POINT_ON_LINE(&line.corner_center, &line.perp2),
		POINT_ON_LINE(&line.corner_end, &line.perp2),
		POINT_ON_LINE(&line.corner_end, &line.l2),

		CEND(),
	});

	struct box box = init_box();
	add_fragment(&topo, (struct topology_elem[]){
		TOPO_MOVETO(&box.corner[0]),
		TOPO_LINETO(&box.corner[1]),
		TOPO_LINETO(&box.corner[2]),
		TOPO_LINETO(&box.corner[3]),
		TOPO_LINETO(&box.corner[0]),

		TOPO_END(),
	});

	add_constraint(&constraints, (struct constraint[]){
		POINT_ON_LINE(&box.corner[0], &box.side[0]),
		POINT_ON_LINE(&box.corner[1], &box.side[0]),

		POINT_ON_LINE(&box.corner[1], &box.side[1]),
		POINT_ON_LINE(&box.corner[2], &box.side[1]),

		POINT_ON_LINE(&box.corner[2], &box.side[2]),
		POINT_ON_LINE(&box.corner[3], &box.side[2]),

		POINT_ON_LINE(&box.corner[0], &box.side[3]),
		POINT_ON_LINE(&box.corner[3], &box.side[3]),

		LL_ANGLE(&box.side[3], &box.side[0], DEG(80)),
		LL_ANGLE(&box.side[1], &box.side[2], DEG(100)),
		LL_ANGLE(&box.side[1], &box.side[3], DEG(180)),
		LL_ANGLE(&components[3], &box.side[1], DEG(90)),

		PL_DISTANCE(&box.side[1], &box.corner[0], 20),

		PP_DISTANCE(&line.corner_end, &box.corner[0], 9.5),
		PP_DISTANCE(&line.corner_start, &box.corner[0], 10),
		CEND(),
	});

	struct mid box_enter = init_mid();

	add_constraint(&constraints, (struct constraint[]){
		LL_ANGLE(&box.side[3], &box_enter.l1, DEG(-30)),
		POINT_ON_LINE(&box.corner[0], &box_enter.l1),
		LL_ANGLE(&box.side[3], &box_enter.l2, DEG(30)),
		POINT_ON_LINE(&box.corner[3], &box_enter.l2),

		POINT_ON_LINE(&box_enter.l1, &box_enter.x1),
		POINT_ON_LINE(&box_enter.l2, &box_enter.x1),

		LL_ANGLE(&box.side[3], &box_enter.p1, DEG(90)),

		POINT_ON_LINE(&box_enter.x1, &box_enter.p1),
		POINT_ON_LINE(&box_enter.p, &box.side[3]),
		POINT_ON_LINE(&box_enter.p1, &box_enter.p),

		PP_DISTANCE(&box_enter.p, &box.corner[0], 5),
		CEND(),
	});

	struct drawing drawing = {};
	solve_constraints(&constraints, &drawing);

	// Copy over all the parameter values to a new array
	// @PERF: Maybe we should just store them in a separate array to start with
	double *params = malloc(sizeof(double) * (constraints.length));
	for(size_t i = 0; i < constraints.length; i++) {
		params[i] = constraints.elements[i].v;
	}

	execute_drawing(&drawing, params);

	begin_drawing();
	// for(struct command *current = drawing.root; current != NULL && current != drawing.error; current = current->next) {
	// 	if(current->hidden) continue;
	// 	plot_generic(current->result);
	// }

	// if(drawing.error != NULL) {
	// 	plot_generic(*drawing.error->arg1);
	// 	plot_generic(*drawing.error->arg2);
	// 	fprintf(stderr, "Solver error detected. Drawing will be incomplete\n");
	// }

	draw_topology(&topo);
	draw_constraints(&constraints);
	end_drawing();
}