#include "config.h"
#include "world.h"
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <sys/types.h>
#include <unistd.h>

#define MAX_MUTATIONS			5

// In percent
#define MAX_JOINT_DISPLACEMENT 	20
#define MAX_FREQ_DISPLACEMENT	25
#define MAX_AMPL_DISPLACEMENT	25
#define MAX_OFFS_DISPLACEMENT	50

#define MAX_SPRING_LENGTH		1.75
#define MAX_MUSCLE_AMPL			0.6
#define MAX_MUSCLE_FREQ			1
#define MAX_MUSCLE_OFFS			12.44

#define CHANCE_ADD_JOINT	   	50
#define CHANCE_ADD_SPRING		5
#define CHANCE_ADD_MUSCLE		5
#define CHANCE_SHIFT_JOINT		10
#define CHANCE_SHIFT_FREQ		10
#define CHANCE_SHIFT_AMPL		10
#define	CHANCE_SHIFT_OFFS		20
#define CHANCE_GRAFT			15
#define	CHANCE_GRAFT2			15
#define	CHANCE_REMOVE_SPRING	5
#define CHANCE_REMOVE_MUSCLE	5
#define CHANCE_MERGE_JOINTS		5
#define CHANCE_SPLICE_SPRING	5

#define CHANCE_SHIFT_OFFS_ALL	50
#define CHANCE_SHIFT_AMPL_ALL	50
#define CHANCE_SHIFT_FREQ_ALL	75

Joint *random_joint() {
	return World::Joints(rand() % World::Joints());
}

Spring *random_spring(Joint *J) {
	return J->GetSpring(rand() % J->Springs());
}

bool chance(int percent) {
	return rand()%100<percent;
}

int plus_or_minus() {
	return rand()%2 ? -1 : 1;
}

float mutate_float(float f, float max_displacement) {
	 return f + f * (rand()/(float) RAND_MAX) * (max_displacement/100.0) * plus_or_minus();
}

void mutate_joint_position(int joint) {
	Joint *J = World::Joints(joint);
	
	float oldx = J->X();
	float oldy = J->Y();
	float x = mutate_float(oldx,MAX_JOINT_DISPLACEMENT);
	float y = mutate_float(oldy,MAX_JOINT_DISPLACEMENT);
	J->MoveTo(x,y);
	for(int i=0;i<J->Springs();i++) { 
		float dy = oldy-y;
		float dx = oldx-x;
		float length = J->GetSpring(i)->Length()+sqrt(dy*dy+dx*dx);
		if (length > MAX_SPRING_LENGTH) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Bounds exceeded: "<<length<<"\n";
			#endif
			J->MoveTo(oldx,oldy);
			break;
		}
	}
}

void mutate_add_spring(int joint) {
	Joint *J = World::Joints(joint);

	if (chance(CHANCE_ADD_JOINT)) {
		World::AddJoint(J->X(),J->Y(),0);
		mutate_joint_position(World::Joints()-1);
		World::AddSpring(joint,World::Joints()-1);
	}
	else {
		int count = 0;
		int k;
		do {
			if (count++>99) return;
			k = rand() % World::Joints();
		} while (J->ConnectedTo(World::Joints(k)));
	
		Joint *K = World::Joints(k);
		float dy = K->Y() - J->Y();
		float dx = K->X() - J->X();
		float length = sqrt(dy*dy+dx*dx);
		if (length < MAX_SPRING_LENGTH) 
			World::AddSpring(joint,k);
		#if DEBUG_MUTATIONS
		else
			cerr<<"DEBUG_MUTATE: Bounds exceeded.\n";
		#endif
	}
	
}

void mutate_add_muscle() {
	Spring *S;
	Joint *K;
	Spring *T;
	int count=0;
	do {
		if (count++>99) return;
		S = random_spring(random_joint());
	} while (S->MuscleAmplitude());
	

	S->AddMuscle(rand()/(float) RAND_MAX * MAX_MUSCLE_FREQ,rand()/(float) RAND_MAX * MAX_MUSCLE_AMPL,rand()/(float) RAND_MAX * MAX_MUSCLE_OFFS);
}

void mutate_muscle_frequency() {

	if (chance(CHANCE_SHIFT_FREQ_ALL)) {
		float ofs = (rand()/(float) RAND_MAX) * (MAX_FREQ_DISPLACEMENT/100.0) * plus_or_minus();
		
		for(int i=0;i<World::Joints();i++) 
			for (int j=0;j<World::Joints(i)->Springs();j++) {
				float f = World::Joints(i)->GetSpring(j)->MuscleFrequency();
				f = f + f*ofs;
				if (f > 0 && f < MAX_MUSCLE_FREQ)
					World::Joints(i)->GetSpring(j)->MuscleFrequency(f);
				#if DEBUG_MUTATIONS
				else
					cerr<<"DEBUG_MUTATE: Bounds exceeded.\n";
				#endif
			}
	}
	else { 
		Spring *S;
		int count = 0;
		do {
			if (count++>99) return;
			S = random_spring(random_joint());
		} while (! S->MuscleAmplitude());

		float f = mutate_float(S->MuscleFrequency(),MAX_FREQ_DISPLACEMENT);
		if (f>0 && f < MAX_MUSCLE_FREQ) 
			S->MuscleFrequency(mutate_float(S->MuscleFrequency(),MAX_FREQ_DISPLACEMENT));
		#if DEBUG_MUTATIONS
		else
			cerr<<"DEBUG_MUTATE: Bounds exceeded.\n";
		#endif
	}
}

void mutate_muscle_amplitude() {
	
	Spring *S;
	int count = 0;

	if (chance(CHANCE_SHIFT_AMPL_ALL)) {
		float ofs = (rand()/(float) RAND_MAX) * (MAX_AMPL_DISPLACEMENT/100.0) * plus_or_minus();
		
		for(int i=0;i<World::Joints();i++)
			for(int j=0;j<World::Joints(i)->Springs();j++)
				if (World::Joints(i)->SpringEnd(j)) {
					Spring *S = World::Joints(i)->GetSpring(j);
					float f = S->MuscleAmplitude();
					f = f+f*ofs;
					if (f>0 && f<MAX_MUSCLE_AMPL) 
						S->MuscleAmplitude(f+f*ofs);
					#if DEBUG_MUTATIONS
					else
						cerr<<"Bounds exceeded.\n";
					#endif

				}
	}
	else {
		do {
			if (count++>99) return;
			S = random_spring(random_joint());
		} while (! S->MuscleAmplitude());

		float f = mutate_float(S->MuscleAmplitude(),MAX_AMPL_DISPLACEMENT);
		if (f>0 && f<MAX_MUSCLE_AMPL) 
			S->MuscleAmplitude(mutate_float(S->MuscleAmplitude(),MAX_AMPL_DISPLACEMENT));
		#if DEBUG_MUTATIONS
		else
			cerr<<"Bounds exceeded.\n";
		#endif
	}
}

void mutate_muscle_offset() {
	Spring *S;
	int count = 0;

	if(chance(CHANCE_SHIFT_OFFS_ALL)) {
		float ofs = (rand()/(float) RAND_MAX) * (MAX_OFFS_DISPLACEMENT/100.0) * plus_or_minus();

		for(int i=0;i<World::Joints();i++)
			for(int j=0;j<World::Joints(i)->Springs();j++)
				if (World::Joints(i)->SpringEnd(j)) {
					Spring *S = World::Joints(i)->GetSpring(j);
					float f = S->MuscleOffset();
					f = f+f*ofs;
					if (f>0 && f<MAX_MUSCLE_OFFS)
						S->MuscleOffset(f+f*ofs);
#if DEBUG_MUTATIONS
					else
						cerr<<"Bounds exceeded.\n";
#endif
				}
	}
	else {
		do {
			if (count++>99) return;
			S = random_spring(random_joint());
		} while (! S->MuscleAmplitude());
		float f = mutate_float(S->MuscleOffset(),MAX_OFFS_DISPLACEMENT);
		if (f>0 && f<MAX_MUSCLE_OFFS)
			S->MuscleOffset(mutate_float(S->MuscleOffset(),MAX_OFFS_DISPLACEMENT));
#if DEBUG_MUTATIONS
		else
			cerr<<"Bounds exceeded.\n";
#endif
	}
}

void mutate_graft(const char* fn) {
	int initialcount = World::Joints();
	int a = rand() % World::Joints();
	
	Spring *S;
	
	World::Include(fn);

	int b = rand() % (World::Joints()-initialcount)+initialcount;
	
	
	float dx = World::Joints(b)->X() - World::Joints(a)->X();
	float dy = World::Joints(b)->Y() - World::Joints(a)->Y();
	
	for(int i=initialcount;i<World::Joints();i++) {
		World::Joints(i)->MoveTo(World::Joints(i)->X()-dx,World::Joints(i)->Y()-dy);
	}	

	for(int i=0;i<World::Joints(b)->Springs();i++) {
		S = World::Joints(b)->GetSpring(i);
		int end = World::Joints(b)->SpringEnd(i);
		World::Joints(a)->ConnectSpring(*S,end);
		S->ConnectedJoint(end,a);
	}		

	World::RemoveJoint(b);
}

void mutate_graft2(const char *fn) {
	Spring *S;
	int initialcount = World::Joints();
	int a = rand() % initialcount;
	int b;
	
	for (b = 0; b<initialcount; b++)
		if (a!=b && World::Joints(a)->ConnectedTo(World::Joints(b)))
			break;

	float dy = World::Joints(b)->Y() - World::Joints(a)->Y();
	float dx = World::Joints(b)->X() - World::Joints(a)->X();
	float angle = atan2(dy,dx);

	World::Include(fn);
	int c = rand() % (World::Joints()-initialcount)+initialcount;
	int d = -1;
		
	for (int i = initialcount; i<World::Joints(); i++) {
		if (d<0 && c!=d && World::Joints(c)->ConnectedTo(World::Joints(i))) d = i;
		float oldx = World::Joints(i)->X();
		float oldy = World::Joints(i)->Y();
		float x = oldx * cos(angle) - oldy * sin(angle);
		float y = oldx * sin(angle) + oldy * cos(angle);
		World::Joints(i)->MoveTo(x,y);
	}

	dy = World::Joints(a)->Y() - World::Joints(c)->Y();
	dx = World::Joints(a)->X() - World::Joints(c)->X();
	for (int i = initialcount; i<World::Joints(); i++) {
		World::Joints(i)->MoveTo(World::Joints(i)->X()+dx,World::Joints(i)->Y()+dy);
	}
	World::Joints(d)->MoveTo(World::Joints(b)->X(),World::Joints(b)->Y());
	for(int i=0; i<World::Joints(c)->Springs(); i++) {
		S = World::Joints(c)->GetSpring(i);
		if (!World::Joints(a)->ConnectedTo(World::Joints(c))) {
			int end = World::Joints(c)->SpringEnd(i);
			World::Joints(a)->ConnectSpring(*S,end);
			S->ConnectedJoint(end,a);
		}
	}
	for(int i=0; i<World::Joints(d)->Springs(); i++) {
		S = World::Joints(d)->GetSpring(i);
		if (!World::Joints(b)->ConnectedTo(World::Joints(d))) {
			int end = World::Joints(d)->SpringEnd(i);
			World::Joints(b)->ConnectSpring(*S,end);
			S->ConnectedJoint(end,b);
		}
	}

	World::RemoveJoint(c);
	World::RemoveJoint(d);
}			
	
void mutate_remove_spring() {

	Joint *J = random_joint();
	int a = rand() % J->Springs();
	Spring *S = J->GetSpring(a);

	for(int i=0;i<World::Joints();i++) {
		Joint *K = World::Joints(i);
		if (J != K) 
			for(int b=0;b<K->Springs();b++)
				if (S==K->GetSpring(b)) {
					J->DisconnectSpring(a);
					K->DisconnectSpring(b);
				}
	}

	delete S;
}

void mutate_merge_joints() {
	
	int a = rand() % World::Joints();
	Joint *J = World::Joints(a);
	Joint *K = random_joint();

	if (J!=K) {
		float dy = K->Y() - J->Y();
		float dx = K->X() - J->X();
		float length = sqrt(dy*dy+dx*dx);
		if (length < MAX_SPRING_LENGTH) {
			K->MoveTo(J->X(),J->Y());
			for(int i=0;i<J->Springs();i++) {
				Spring *S = J->GetSpring(i);
				if (! K->ConnectedTo(J))
					K->ConnectSpring(*S,J->SpringEnd(i));
			}
		
			World::RemoveJoint(a);
		}
#if DEBUG_MUTATIONS
		else 
			cerr<<"Bounds exceeded.\n";
#endif
	}
}

void mutate_splice_spring() {
	int j = rand() % World::Joints();	
	Joint *J = World::Joints(j);
	Joint *K;
	int a = rand() % J->Springs();
	Spring *S = J->GetSpring(a);
	int b;
	for(int k=0;k<World::Joints();k++) 
		if (j!=k) {
			K = World::Joints(k);
			for(b=0;b<K->Springs();b++)
				if(S==K->GetSpring(b)) {
					//J->DisconnectSpring(a);
					//delete K->DisconnectSpring(b);
					//cerr << "Splicing "<<j<<","<<k<<"\n";
					World::AddJoint((J->X()+K->X())/2,(J->Y()+K->Y())/2,0);
					Joint *L = World::Joints(World::Joints()-1);
					//K->DisconnectSpring(b);
					L->ConnectSpring(*S,J->SpringEnd(a));
					World::AddSpring(k,World::Joints()-1);
					//cerr << "New joint "<<World::Joints()-1<<"\n";
					return;
				}
		}
}
					

void mutate_remove_muscle() {
	Spring *S = random_spring(random_joint());
	S->MuscleAmplitude(0);
	S->MuscleFrequency(0);
	S->MuscleOffset(0);
}

int main(int argc, char **argv) {

	if (argc<3) {
		cerr << "Usage: mutate alpha.def beta.def [output.def] [random_seed]\n";
		return 1;
	}

	if (argc>4) {
		srand(atoi(argv[4]));
	}
	else {

		srand(time(NULL) % getpid());
	}

	World::Load(argv[1]);
	cerr <<"\n";

	int mutations = 0;

	int chance_total = 
	 CHANCE_ADD_SPRING		+
	 CHANCE_ADD_MUSCLE		+
	 CHANCE_SHIFT_JOINT		+
	 CHANCE_SHIFT_FREQ		+
	 CHANCE_SHIFT_AMPL		+
	 CHANCE_SHIFT_OFFS		+
	 CHANCE_GRAFT			+
	 CHANCE_GRAFT2			+
	 CHANCE_REMOVE_SPRING	+
	 CHANCE_REMOVE_MUSCLE	+
	 CHANCE_MERGE_JOINTS	+
	 CHANCE_SPLICE_SPRING;

	int num_mutations = rand() % MAX_MUTATIONS;
	for(int i=0;i<num_mutations || !mutations;i++) {
		int running_total = 0;
		int roll = rand()%chance_total;	

		#if DEBUG_MUTATIONS
		cerr<<"DEBUG_MUTATE: Rolled a "<<roll<<" of "<<chance_total<<"\n";
		#endif

		if (roll >= running_total && roll < running_total+CHANCE_ADD_SPRING) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Adding a spring\n";
			#endif
			mutations++;
			mutate_add_spring(rand() % World::Joints());
		}
		running_total += CHANCE_ADD_SPRING;
		
		if (roll >= running_total && roll < running_total+CHANCE_SHIFT_JOINT) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Moving a joint\n";
			#endif
			mutations++;
			mutate_joint_position(rand() % World::Joints());
		}
		running_total += CHANCE_SHIFT_JOINT;
		
		if (roll >= running_total && roll < running_total+CHANCE_ADD_MUSCLE) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Adding a muscle\n";
			#endif
			mutations++;
			mutate_add_muscle();
		}
		running_total += CHANCE_ADD_MUSCLE;

		if (roll >= running_total && roll < running_total+CHANCE_SHIFT_FREQ) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Tweaking muscle frequency\n";
			#endif
			mutations++;
			mutate_muscle_frequency();
		}
		running_total += CHANCE_SHIFT_FREQ;

		if (roll >= running_total && roll < running_total+CHANCE_SHIFT_AMPL) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Tweaking muscle amplitude\n";
			#endif
			mutations++;
			mutate_muscle_amplitude();
		}
		running_total += CHANCE_SHIFT_AMPL;

		if (roll >= running_total && roll < running_total+CHANCE_SHIFT_OFFS) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Tweaking muscle offset\n";
			#endif
			mutations++;
			mutate_muscle_offset();
		}
		running_total += CHANCE_SHIFT_OFFS;

		if (roll >= running_total && roll < running_total+CHANCE_GRAFT) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Grafting "<<argv[2]<<"\n";
			#endif
			mutations++;
			mutate_graft(argv[2]);
		}
		running_total += CHANCE_GRAFT;

		if (roll >= running_total && roll < running_total+CHANCE_MERGE_JOINTS) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Merging joints\n";
			#endif
			mutations++;
			mutate_merge_joints();
		}
		running_total += CHANCE_MERGE_JOINTS;

		if (roll >= running_total && roll < running_total+CHANCE_REMOVE_SPRING) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Removing spring\n";
			#endif
			mutations++;
			mutate_remove_spring();
		}
		running_total += CHANCE_REMOVE_SPRING;

		if (roll >= running_total && roll < running_total+CHANCE_REMOVE_MUSCLE) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Removing muscle\n";
			#endif
			mutations++;
			mutate_remove_muscle();
		}
		running_total += CHANCE_REMOVE_MUSCLE;

		if (roll >= running_total && roll < running_total+CHANCE_GRAFT2) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Double grafting "<<argv[2]<<"\n";
			#endif
			mutations++;
			mutate_graft2(argv[2]);
		}
		running_total += CHANCE_GRAFT2;

		if (roll >= running_total && roll < running_total+CHANCE_SPLICE_SPRING) {
			#if DEBUG_MUTATIONS
			cerr<<"DEBUG_MUTATE: Splicing a spring\n";
			#endif
			mutations++;
			mutate_splice_spring();
		}
		running_total += CHANCE_SPLICE_SPRING;
	}

	cerr << mutations << " mutations.\n";
	World::Save(argc>3 ? argv[3] : "output.def");
}