// Simulation code for Inferring collective behavior from a fossilized fish shoal
// 10/29/2018 updated by N.Mizumoto
// The simulation program was implemented in Microsoft Visual Studio C++ 2017

// libraries
#include "stdafx.h"
#include <vector>
#include <iostream>	// cout; cin
#include <sstream>	// to_string
#include <fstream>	// writing
#include <math.h>
using namespace std;

// Random sampling
#include <random>
//std::random_device rd;
//std::mt19937 mt(rd());
//std::uniform_real_distribution<double> rnd(0.000, 1.000);

#include <time.h>
std::uniform_real_distribution<double> distribution(0.0, 1.0);
std::random_device seed_gen;
std::mt19937 engine(seed_gen());
auto rnd = [&] { return distribution(engine); };

#define pi 3.1415926535
#define nipi 3.1415926535*2

// random angle
double dir;
double allocate() {
	dir = rnd()*nipi;
	return(dir);
}

// PBC
// modification for PBC
double aPBC(double Loc, double Length) {
	if (Loc >= Length) {
		Loc -= Length;
	}
	else if (Loc < 0) {
		Loc += Length;
	}
	return(Loc);
}

///*// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;
//*/

// Parameters
#define n 257
#define L 200
#define zor 1	// zone of replusion
#define zoo 16	// zone of orientation
#define st 10000	// * 0.1 second 
#define RotateLimit 30*pi/180
#define maxspeed 7
#define minspeed 3
#define sd 0.2 // rad (0-0.2)
#define size 5

// Variables
vector<double> Cx(n), Cy(n); // C: position vector
vector<double> C2x(n), C2y(n); // C: position vector
vector<double> Vx(n), Vy(n); // V: unit direction vector
vector<double> Dx(n), Dy(n); // D: desired direction vector
vector<double> Vangle(n);
vector<double> Dangle(n);

vector<double> n1(n), Dr1x(n), Dr1y(n);
vector<double> n2(n), Do2x(n), Do2y(n);

double AllDis[n][n], Rx[n][n], Ry[n][n], Pangle[n][n];
int i, j, step, nothing, z, iter;
double xdis, ydis, nearangle, tangle, RotateDirection, r, speed;

int main() {

	// Output first
	std::string FileName;
	int OK, plot;

	cout << "Simulations for Boids" << endl;
	cout << "Plot?" << endl;
	cin >> plot;
	cout << "Please enter the number of replications" << endl;
	cin >> iter;
	cout << "The values of parameters are as follow:" << endl;
	cout << "Number of individuals: " << n << endl;
	cout << "Field Size: " << L << endl;
	cout << "Repulsion radius: " << zor << endl;
	cout << "Orientation/Attraction radius: " << zoo << endl;
	cout << "Turining rate: " << RotateLimit << endl;
	cout << "Max Speed: " << maxspeed << endl;
	cout << "Min Speed: " << minspeed << endl;
	cout << "Error: " << sd << endl;
	cout << "Timesteps: " << st << endl;

	cout << "Are you OK?" << endl;
	cout << "Yes: 1, No: 2" << endl;
	cin >> OK;
	if (OK == 2) { return 1; }

	// parameter output
	FileName = "res_boid_sim";
	FileName += "_parameters";
	FileName += ".csv";
	std::ofstream ofs(FileName);
	ofs << "NumOfIndividuals, FieldSize, Repulsion, Orientation/Attraction, FieldOfPerception, TurningRate, MaxSpeed, MinSpeed, Error, Seconds" << std::endl;
	ofs << n << "," << L << "," << zor << "," << zoo << "," << << RotateLimit << ",";
	ofs << maxspeed << "," << minspeed << "," << sd << "," << st << std::endl;
	ofs.close();
	ofs.clear();

	// replication
	for (z = 0; z < iter; z++) {

		// initialization
		for (i = 0; i<n; i++) {
			Cx[i] = L * rnd();
			Cy[i] = L * rnd();
			Vangle[i] = nipi * rnd();
			Vx[i] = cos(Vangle[i]);
			Vy[i] = sin(Vangle[i]);
			C2x[i] = Cx[i] - size * Vx[i];
			C2y[i] = Cy[i] - size * Vy[i];
		}

		for (step = 0; step<st; step++) {

			for (i = 0; i<n; i++) {
				speed = sqrt(Dx[i] * Dx[i] + Dy[i] * Dy[i]);
				if (speed < minspeed) { speed = minspeed; }
				else if (speed > maxspeed) { speed = maxspeed; }

				Cx[i] = Cx[i] + Vx[i] * speed * 0.1;
				Cy[i] = Cy[i] + Vy[i] * speed * 0.1;
				Cx[i] = aPBC(Cx[i], L);
				Cy[i] = aPBC(Cy[i], L);
				C2x[i] = Cx[i] - size * Vx[i];
				C2y[i] = Cy[i] - size * Vy[i];
			}

			// image output
			if (plot>0) {
				Mat Img(Size(L, L), CV_8UC3, Scalar(255, 255, 255));
				namedWindow("search image", cv::WINDOW_AUTOSIZE);
				for (i = 0; i<n - 1; i++) {
					circle(Img, Point(Cx[i], Cy[i]), 1, Scalar(200, 0, 0), -1, CV_AA);
					line(Img, Point(C2x[i], C2y[i]), Point(Cx[i], Cy[i]), Scalar(200, 0, 0), 1, 4);
				}
				circle(Img, Point(Cx[n - 1], Cy[n - 1]), 1, Scalar(0, 0, 200), -1, CV_AA);
				line(Img, Point(C2x[n - 1], C2y[n - 1]), Point(Cx[n - 1], Cy[n - 1]), Scalar(0, 0, 200), 1, 4);
				imshow("search image", Img);
				waitKey(1);
			}

			// relative position of all individuals are calcurated
			for (i = 0; i<n; i++) {
				for (j = 0; j<n; j++) {
					if (i < j) {
						if (abs(Cx[i] - Cx[j]) > abs(L - abs(Cx[i] - Cx[j]))) {
							if (Cx[i] > Cx[j]) {
								Rx[i][j] = (Cx[j] + L) - Cx[i];
							}
							else { Rx[i][j] = (Cx[j] - L) - Cx[i]; }
						}
						else { Rx[i][j] = Cx[j] - Cx[i]; }

						if (abs(Cy[i] - Cy[j]) > abs(L - abs(Cy[i] - Cy[j]))) {
							if (Cy[i] > Cy[j]) {
								Ry[i][j] = (Cy[j] + L) - Cy[i];
							}
							else {
								Ry[i][j] = (Cy[j] - L) - Cy[i];
							}
						}
						else {
							Ry[i][j] = Cy[j] - Cy[i];
						}
						AllDis[i][j] = sqrt((Rx[i][j] * Rx[i][j]) + (Ry[i][j] * Ry[i][j]));
						Pangle[i][j] = atan(Ry[i][j] / Rx[i][j]);
						if (Rx[i][j] < 0) {
							Pangle[i][j] += pi;
						}
						if (Pangle[i][j] < 0) {
							Pangle[i][j] += nipi;
						}
					}
					else {
						Rx[i][j] = -Rx[j][i];
						Ry[i][j] = -Ry[j][i];
						AllDis[i][j] = AllDis[j][i];

						if (Pangle[j][i] < pi) {
							Pangle[i][j] = Pangle[j][i] + pi;
						}
						else {
							Pangle[i][j] = Pangle[j][i] - pi;
						}
					}
				}
			}


			// rule 1: replusion
			for (i = 0; i<n; i++) {
				n1[i] = 0;
				Dr1x[i] = 0;
				Dr1y[i] = 0;
				for (j = 0; j<n; j++) {
					if (AllDis[i][j] <= zor && i != j) {
						// for detection angle rule
						Dr1x[i] -= Rx[i][j] / AllDis[i][j];
						Dr1y[i] -= Ry[i][j] / AllDis[i][j];
						n1[i] ++;
					}
				}
			}

			// rule 2: orientation
			for (i = 0; i<n; i++) {
				n2[i] = 0;
				Do2x[i] = 0;
				Do2y[i] = 0;
				if (n1[i] == 0) {
					for (j = 0; j<n; j++) {
						// for detection angle rule
						if (AllDis[i][j] <= zoo && i != j) {
							Do2x[i] += Vx[j] / 2;
							Do2y[i] += Vy[j] / 2;
							Do2x[i] += Rx[i][j] / AllDis[i][j] / 2;
							Do2y[i] += Ry[i][j] / AllDis[i][j] / 2;
							n2[i] ++;
						}
					}
				}
			}


			// Determining desired direction vector
			for (i = 0; i<n; i++) {
				if (n1[i] > 0) {
					Dx[i] = Dr1x[i];
					Dy[i] = Dr1y[i];
				}
				else {
					if (n2[i] > 0) {
						Dx[i] = Do2x[i];
						Dy[i] = Do2y[i];
					}
					else {
						Dx[i] = Vx[i];
						Dy[i] = Vy[i];
					}
				}

				//// turning limitation
				// After the above process, individuals turn towards the direction vector D by the turning rate Thita.
				// If the angle between V and D is less than Thita * tau, V = D.

				// "atan" returns -pi/2 ~ pi/2 → xが負なら+piで補正。最終的に負なら+2piで補正。これで 0~2piの範囲になるはず。
				Dangle[i] = atan(Dy[i] / Dx[i]);
				if (Dx[i] < 0) {
					Dangle[i] += pi;
				}
				if (Dangle[i] < 0) {
					Dangle[i] += nipi;
				}

				// error
				Dangle[i] += WrappedNormal(sd);

				// rotating direction is positive +1 or negative -1?
				if (Dangle[i] > Vangle[i]) {
					if (Dangle[i] - Vangle[i] < pi) {
						RotateDirection = Dangle[i] - Vangle[i];
					}
					else if (Dangle[i] - Vangle[i] > pi) {
						RotateDirection = Dangle[i] - Vangle[i] - nipi;
					}
					else if (Dangle[i] - Vangle[i] == pi) {
						if (rnd() > 0.5) {
							RotateDirection = pi;
						}
						else {
							RotateDirection = -pi;
						}
					}
				}
				else {
					if (Dangle[i] - Vangle[i] > -pi) {
						RotateDirection = Dangle[i] - Vangle[i];
					}
					else if (Dangle[i] - Vangle[i] < -pi) {
						RotateDirection = Dangle[i] - Vangle[i] + nipi;
					}
					else {
						if (rnd() > 0.5) {
							RotateDirection = pi;
						}
						else {
							RotateDirection = -pi;
						}
					}
				}
				// limitation
				if (abs(RotateDirection) > RotateLimit) {
					RotateDirection = RotateDirection / abs(RotateDirection) * RotateLimit;
				}

				// timestep = 0.1 second
				RotateDirection *= 0.1;
				Vangle[i] += RotateDirection;
				if (Vangle[i] < 0) {
					Vangle[i] += nipi;
				}
				if (Vangle[i] > nipi) {
					Vangle[i] -= nipi;
				}

				//// move
				Vx[i] = cos(Vangle[i]);
				Vy[i] = sin(Vangle[i]);

			}


		}


		// results output
		FileName = "res_boid_sim";
		FileName += "_rep";
		FileName += std::to_string(1 + z);
		FileName += ".csv";

		ofs.open(FileName);
		ofs << "id, x, y, Angle" << endl;
		for (i = 0; i<n; i++) {
			ofs << 1 + i << "," << Cx[i] << "," << Cy[i] << "," << Vangle[i] << std::endl;
		}
		ofs.close();
		ofs.clear();
	}

	return 0;
}