/*
 * Copyright (c) 2008-2009 Bill Whitacre http://rampancy.g0dsoft.com
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
*/

#include "contact.h"
#include "arbiter.h"

rat_contact *rat_contact_create(rat_block_allocator *bal)
{
	rat_contact *mem;
	if (bal)
		mem=(rat_contact *)rat_block_alloc(bal,sizeof(rat_contact));
	else
		mem=(rat_contact *)rat_alloc(sizeof(rat_contact));
	assert(mem!=NULL);
	memset(mem,0,sizeof(rat_contact));
	mem->bal=bal;
	return mem;
}

void rat_contact_destroy(rat_contact *con)
{
	if (con->bal)
		rat_block_dealloc(con->bal,(void *)con,sizeof(rat_contact));
	else
		rat_dealloc((void *)con);
}

rat_contact_array *rat_alloc_contact_array(rat_block_allocator *bal,unsigned int numcons)
{
	register unsigned int i;
	rat_contact_array *conarr;

	if (bal)
		conarr=(rat_contact_array *)rat_block_alloc(bal,sizeof(rat_contact_array));
	else
		conarr=(rat_contact_array *)rat_alloc(sizeof(rat_contact_array));
	memset(conarr,0,sizeof(rat_contact_array));

	conarr->bal=bal;
	conarr->numcons=numcons;
	if (!numcons) return conarr;

	if (bal)
		conarr->cons=(rat_contact **)rat_block_alloc(bal,numcons*sizeof(rat_contact *));
	else
		conarr->cons=(rat_contact **)rat_alloc(numcons*sizeof(rat_contact *));
	for (i=0; i<numcons; i++) conarr->cons[i]=rat_contact_create(bal);

	return conarr;
}

// note that to use this destructor you MUST have allocated
// the (rat_contact_array *) with rat_alloc_contact_array().
void rat_free_contact_array(rat_contact_array *cons)
{
	register unsigned int i;

	if (cons->numcons>0)
	{
		for (i=0; i<cons->numcons; i++)
			rat_contact_destroy(cons->cons[i]);

		if (cons->bal)
			rat_block_dealloc(cons->bal,(void *)cons->cons,cons->numcons*sizeof(rat_contact *));
		else
			rat_dealloc((void *)cons->cons);
	}

	if (cons->bal)
		rat_block_dealloc(cons->bal,(void *)cons,sizeof(rat_contact_array));
	else
		rat_dealloc((void *)cons);
}

rat_contact *rat_add_array_contact(rat_contact_array *cons)
{
	register unsigned int i;

	if (cons->numcons==0)
	{
		if (cons->bal)
			cons->cons=(rat_contact **)rat_block_alloc(cons->bal,sizeof(rat_contact *));
		else
			cons->cons=(rat_contact **)rat_alloc(sizeof(rat_contact *));
	}
	else
	{
		if (cons->bal)
			cons->cons=(rat_contact **)rat_block_realloc(cons->bal,cons->cons,sizeof(rat_contact *)*cons->numcons,sizeof(rat_contact *)*(cons->numcons+1));
		else
			cons->cons=(rat_contact **)rat_realloc(cons->cons,sizeof(rat_contact *)*(cons->numcons+1));
	}
	cons->cons[cons->numcons]=rat_contact_create(cons->bal);
	cons->numcons++;

	return cons->cons[cons->numcons-1];
}

int rat_contacts_match(rat_contact *a,rat_contact *b)
{
	if (a->hull_a!=b->hull_a) return 0;
	if (a->hull_b!=b->hull_b) return 0;
	if (a->idvala!=b->idvala) return 0;
	if (a->idvalb!=b->idvalb) return 0;
	return 1;
}

void rat_merge_contacts(rat_contact_array **thesecons,rat_contact_array *newcons)
{
	rat_block_allocator *bal=(*thesecons)->bal;
	rat_contact_array *oldcons=*thesecons;

	register unsigned int i,j;
	register unsigned int oldnumcons=oldcons->numcons;
	register unsigned int newnumcons=newcons->numcons;

	if (rat_arbiter_global_warm_starting_enabled())
	{
		for (i=0; i<oldnumcons; i++)
		{
			rat_contact *oldcon=rat_indexed_contact(oldcons,i);
			for (j=0; j<newnumcons; j++)
			{
				rat_contact *newcon=rat_indexed_contact(newcons,j);
				if (rat_contacts_match(oldcon,newcon))
				{
					newcon->norm_acc_j=oldcon->norm_acc_j;
					newcon->tan_acc_j=oldcon->tan_acc_j;
					break;
				}
			}
		}
	}

	rat_free_contact_array(oldcons);
	oldcons=rat_alloc_contact_array(bal,newcons->numcons);
	for (i=0; i<newcons->numcons; i++)
		memcpy(oldcons->cons[i],newcons->cons[i],sizeof(rat_contact)-sizeof(rat_block_allocator *));

	*thesecons=oldcons;
}

void rat_invert_contacts(rat_contact_array *cons)
{
	register unsigned int i;
	register unsigned int numcons=cons->numcons;

	for (i=0; i<numcons; i++)
	{
		rat_contact *con=rat_indexed_contact(cons,i);
		con->normal=vector2_negative(con->normal);
	}
}

unsigned int rat_count_array_contacts(rat_contact_array *cons)
{
	return cons->numcons;
}

rat_contact *rat_indexed_contact(rat_contact_array *cons,unsigned int idx)
{
	return cons->cons[idx];
}