#define _GNU_SOURCE
#include <stdlib.h>

/* a single printf */
#include <stdio.h>

#include "../../../headers/input/touch/touch_controller_L2.h"
#include "../../../headers/input/touch/touch_controller_L3.h"
#include "../../../headers/input/touch/touch_controller_L3_deltas_compute.h"



/*

	WHAT POINTER IS THE THIRD???

*/
static int cmp_uint64(const void *p1, const void *p2)
{
	uint64_t a = *(const uint64_t*) p1;
	uint64_t b = *(const uint64_t*) p2;
//printf("comparison between %ld and %ld\n", a, b);
	if( a < b ){
		return 1;
	}
	else if( a > b){
		return -1;
	}
	else{
		return 0;
	}
}


/* returns the sorted tracking_ids of a touch_coordinates array */
void touch_events_to_sorted_ids( uint64_t **sortedIds, touch_event **full, uint8_t full_count){
//printf("sorting an array of ids long %d\n", full_count);
	*sortedIds = malloc( full_count * sizeof( uint64_t));
//printf("populate\n");
	for( uint8_t i = 0; i < full_count; i++){
		// could crash here...
		( *sortedIds)[i] = full[i]->tracking_id;
	}
//printf("begin of actual sorting\n");
	/* this NULL arg could bite */
	/* qsort_r */
	qsort( sortedIds, full_count, sizeof( uint64_t), cmp_uint64);
}

void TC_L3_get_position_deltas( 
	touch_event **previous, uint8_t previous_count, 
	touch_event **current, uint8_t current_count, 
	touch_coordinates **deltas, uint8_t *count
){
	if( previous && current){
		uint64_t *sorted_previous_ids = NULL;
		uint64_t *sorted_current_ids = NULL;
		tracked_delta *id_deltas;
		/*	the number of total id_deltas, being a sum of sets, can be greater than the single sorted_ids list
			but the output count, being their intersections, cannot
		*/
		uint16_t id_deltas_count;
		
		printf("mannaggia all'abaco mostrami sti id\n");
		printf("previous\n");
		for( uint8_t i = 0; i < previous_count; i++){
			printf( "%ld\n", previous[i]->tracking_id);
		}

		printf("current\n");
		for( uint8_t i = 0; i < current_count; i++){
			printf( "%ld\n", current[i]->tracking_id);
		}
		
		printf("sort ids 1\n");

		touch_events_to_sorted_ids( &sorted_previous_ids, previous, previous_count);

		printf("sort ids 2\n");

		touch_events_to_sorted_ids( &sorted_current_ids, current, current_count);

		printf("sorted ids\n");

		printf("get id_deltas\n");
		touch_events_sorted_ids_deltas( 
			&id_deltas, &id_deltas_count, 
			sorted_previous_ids, previous_count, 
			sorted_current_ids, current_count
		);
		printf("got id_deltas\n");
		
		free( sorted_previous_ids);
		free( sorted_current_ids);

		uint8_t endcount = 0;

		for( uint16_t i = 0; i < id_deltas_count; i++){
			if( TRACKED_DELTA_UNCHANGED == id_deltas[i].state){
				endcount++;
			}
		}

		*count = endcount;

		*deltas = new_touch_coordinates_buffer( endcount);

		for( uint16_t i = 0; i < id_deltas_count; i++){
			tracked_delta current_id_delta = id_deltas[i];
			if( TRACKED_DELTA_UNCHANGED == current_id_delta.state){
				touch_event *prev_evt = previous[ current_id_delta.old_index];
				touch_event *next_evt = current[ current_id_delta.new_index];
				deltas[i]->x =  next_evt->position.x - prev_evt->position.x;
				deltas[i]->y =  next_evt->position.y - prev_evt->position.y;
			}
		}
		free( id_deltas);
	}
	else{
		printf( "previous or current are empty!\n");
		*deltas = NULL;
		*count = 0;
	}
}

float TC_L3_get_angle_delta( float previous_angle, float current_angle){
	float dAngle = 0;
	dAngle = current_angle - previous_angle;
	if( dAngle > 180){
		dAngle -= 360;
	}
	else if( dAngle < -180){
		dAngle += 360;
	}
	return dAngle;
}

touch_coordinates* TC_L3_get_midpoint_delta( touch_coordinates previous, touch_coordinates current){
	touch_coordinates* delta = malloc( sizeof( touch_coordinates));

	delta->x = current.x - previous.x;
	delta->y = current.y - previous.y;

	return delta;
}