#include "stdafx.h"
#include <stdlib.h>
#include <stdio.h>
#include <time.h>

#define FUNC_PARAMS_DIM 6
#define NUMBER_OF_PARTICLES 100
#define NUMBER_OF_ITERATIONS 100000
#define ACTIVE_THRESHOLD 3000
#define LOWER_ACTIVE_THRESHOLD 500

// #define DEBUG

typedef struct vector {
	int dimensionCount;
	double* coordinates;
} vector;

typedef struct dataPoint {
	vector position;
	double value;
} dataPoint;

typedef struct dataSeed {
	int numberOfPoints;
	dataPoint* points;
} dataSeed;

typedef struct particle {
	vector currentPos;
	int isActive;
} particle;

typedef struct swarmType {
	int numberOfParticles;
	particle** particles;
} swarmType;

typedef double(*funcPtr)(vector* parameters, vector* position);

double calculatePolynomial1d(vector* parameters, vector* position) {
	double value = 0.0;
	int i = 0;

	for (; i < parameters->dimensionCount; i++)
	{
		value *= position->coordinates[0];
		value += parameters->coordinates[i];
	}
	return value;
}

double leastSquares(dataSeed* tab, funcPtr func, vector* parameters)
{
	double squares = 0.0;
	for (int i = 0; i < tab->numberOfPoints; i++)
	{
		dataPoint* currentPoint = &tab->points[i];
		double currentDistance = currentPoint->value - func(parameters, &currentPoint->position);
		squares += currentDistance * currentDistance;
	}
	return squares;
}

void PrintfParticle(dataSeed * seed, particle * bestParticle)
{
	int i;
	printf("Squared distance: %lf\n", leastSquares(seed, &calculatePolynomial1d, &bestParticle->currentPos));

	for (i = 0; i < bestParticle->currentPos.dimensionCount; i++)
	{
		double coord = bestParticle->currentPos.coordinates[i];
		printf("%c: %e\n", 'A' + i, coord);
	}
}


double getRandom(double min, double max)
{
	return (double)rand() / (double)RAND_MAX * (max - min) + min;
}

void assignPosition(vector* dest, vector* src)
{
	if (dest->dimensionCount != src->dimensionCount)
	{
		fprintf(stderr, "assigningPosition of the different dimensions points\n");
		exit(1);
	}

	for (int i = 0; i < dest->dimensionCount; i++)
	{
		dest->coordinates[i] = src->coordinates[i];
	}
}

particle* initializeParticle(int dimensionNumber, double* minimums, double* maximums)
{
	struct particle* singleParticle = (particle*)malloc(sizeof(particle));
	singleParticle->currentPos.coordinates = (double*)malloc(dimensionNumber * sizeof(double));
	singleParticle->currentPos.dimensionCount = dimensionNumber;
	singleParticle->isActive = 0;

	for (int i = 0; i < dimensionNumber; i++)
	{
		singleParticle->currentPos.coordinates[i] = getRandom(minimums[i], maximums[i]);
	}
	return singleParticle;
}

swarmType* initializeSwarm(funcPtr func, int numberOfParticles, int dimensionNumber, double* minimums, double* maximums, dataSeed* seed)
{
	swarmType* swarm = (swarmType*)malloc(sizeof(swarmType));
	swarm->numberOfParticles = numberOfParticles;
	swarm->particles = (particle**)malloc(numberOfParticles * sizeof(particle*));

	for (int i = 0; i < numberOfParticles; i++) {
		swarm->particles[i] = initializeParticle(dimensionNumber, minimums, maximums);
	}

	return swarm;
}

void freeParticle(particle* singleParticle)
{
	free(singleParticle->currentPos.coordinates);
}

void freeSwarm(swarmType* swarm)
{
	for (int i = 0; i < swarm->numberOfParticles; i++)
	{
		freeParticle(swarm->particles[i]);
	}
	free(swarm->particles);
	free(swarm);
}

dataSeed* initializeSeed(FILE* file)
{
	int i;
	dataSeed* seed = (dataSeed*)malloc(sizeof(dataSeed));
	fscanf(file, "%d", &seed->numberOfPoints);
	seed->points = (dataPoint*)malloc(seed->numberOfPoints * sizeof(dataPoint));

	for (i = 0; i < seed->numberOfPoints; i++)
	{
		seed->points[i].position.dimensionCount = 1;
		seed->points[i].position.coordinates = (double*)malloc(sizeof(double));
		fscanf(file, "%lf;%lf", &(seed->points[i].position.coordinates[0]), &(seed->points[i].value));
	}
	return seed;
}

void freeSeed(dataSeed* seed)
{
	int i;
	for (i = 0; i < seed->numberOfPoints; i++)
	{
		free(seed->points[i].position.coordinates);
	}
	free(seed->points);
	free(seed);
}

void RerollLastParameter(particle* currentParticle, double* minimums, double* maximums)
{
	int lastDimNumber = currentParticle->currentPos.dimensionCount - 1;
	currentParticle->currentPos.coordinates[lastDimNumber] = getRandom(minimums[lastDimNumber], maximums[lastDimNumber]);
}

void RerollAllParameters(particle* currentParticle, double* minimums, double* maximums)
{
	int i;
	int numberOfIterations = currentParticle->currentPos.dimensionCount;
	for (i = 0; i < numberOfIterations; i++) {
		currentParticle->currentPos.coordinates[i] = getRandom(minimums[i], maximums[i]);
	}
}

void TryRerollAnyParameter(dataSeed* seed, funcPtr func, particle* currentParticle, double* minimums, double* maximums)
{
	int numberOfDimentsions = currentParticle->currentPos.dimensionCount;
	int i = (int)((double)rand() / (double)RAND_MAX * (double)numberOfDimentsions);

	double oldParam = currentParticle->currentPos.coordinates[i];
	double oldValue = leastSquares(seed, func, &currentParticle->currentPos);

	currentParticle->currentPos.coordinates[i] = getRandom(minimums[i], maximums[i]);
	double newValue = leastSquares(seed, func, &currentParticle->currentPos);

	if (newValue > oldValue) {
		currentParticle->currentPos.coordinates[i] = oldParam;
	}
}

int IsActive(particle* currentParticle, dataSeed* seed, funcPtr func)
{
	double currentSquaredDiferencesSum = leastSquares(seed, func, &currentParticle->currentPos);

	if (currentSquaredDiferencesSum < LOWER_ACTIVE_THRESHOLD)
	{
		PrintfParticle(seed, currentParticle);
	}

	return currentSquaredDiferencesSum < ACTIVE_THRESHOLD;
}

void CopyParameters(particle* currentParticle, particle* buddy)
{
	int i;
	for (i = 0; i < currentParticle->currentPos.dimensionCount; i++) {
		currentParticle->currentPos.coordinates[i] = buddy->currentPos.coordinates[i];
	}
}

void Iterate(funcPtr func, swarmType* swarm, dataSeed* seed, double* minimums, double* maximums)
{
	int i;
	for (i = 0; i < swarm->numberOfParticles; i++)
	{
		particle* currentParticle = swarm->particles[i];
		if (currentParticle->isActive) {
			TryRerollAnyParameter(seed, func, currentParticle, minimums, maximums);
		}
		else {
			int buddyIndex = (int)((double)rand() / (double)RAND_MAX * (double)(swarm->numberOfParticles - 1));
			particle* buddy = swarm->particles[buddyIndex];
			if (buddy->isActive) {
				CopyParameters(currentParticle, buddy);
				RerollLastParameter(currentParticle, minimums, maximums);
			}
			else {
				RerollAllParameters(currentParticle, minimums, maximums);
			}
		}
	}
	for (i = 0; i < swarm->numberOfParticles; i++)
	{
		particle* currentParticle = swarm->particles[i];
		currentParticle->isActive = IsActive(currentParticle, seed, func);
	}
}

particle* GetBestParticle(dataSeed* seed, funcPtr func, swarmType* swarm)
{
	int i;
	particle* bestParticle = swarm->particles[0];
	double bestSquaredDiferencesSum = leastSquares(seed, func, &swarm->particles[0]->currentPos);

	for (i = 1; i < swarm->numberOfParticles; i++)
	{
		particle* currentParticle = swarm->particles[i];

		double currentSquaredDiferencesSum = leastSquares(seed, func, &currentParticle->currentPos);

		if (currentSquaredDiferencesSum < bestSquaredDiferencesSum)
		{
			bestSquaredDiferencesSum = currentSquaredDiferencesSum;
			bestParticle = currentParticle;
		}
	}

	return bestParticle;
}

int main(int argc, char** argv)
{
	srand(time(NULL));

	double minimums[] = { -1e-15, -1e-10, -1e-10, -1e-4, -1e-4, -3.5 };
	double maximums[] = { 1e-15,  1e-10,  1e-10,  1e-4,  1e-4,  0 };

#ifdef DEBUG
	printf("Opening File: %s\n", argv[1]);
#endif

	FILE* inFile = fopen("dane.txt", "r");

#ifdef DEBUG
	printf("Initializing Seed\n");
#endif

	dataSeed* seed = initializeSeed(inFile);

#ifdef DEBUG
	printf("Seed Initialized with size: %d\n", seed->numberOfPoints);
#endif

	fclose(inFile);

	swarmType* swarm = initializeSwarm(&calculatePolynomial1d,
		NUMBER_OF_PARTICLES,
		FUNC_PARAMS_DIM,
		minimums,
		maximums,
		seed);


#ifdef DEBUG
	printf("Swarm initialized with size: %d\nIterating...\n", swarm->numberOfParticles);
#endif

	int i;

	for (i = 0; i < NUMBER_OF_ITERATIONS; i++)
	{
#ifdef DEBUG
		printf(".");
#endif

		if (!(i % (NUMBER_OF_ITERATIONS / 10))) {
			printf("%.1lf%%\n", (double)i * 100.0 / (double)NUMBER_OF_ITERATIONS);
		}
		Iterate(&calculatePolynomial1d, swarm, seed, minimums, maximums);
	}

	printf("\nResults:");

	particle* bestParticle = GetBestParticle(seed, &calculatePolynomial1d, swarm);

	PrintfParticle(seed, bestParticle);

	printf("\n");
	getchar();
	return 0;
}