// This file is a part of Framsticks SDK.  http://www.framsticks.com/
// Copyright (C) 2019-2020  Maciej Komosinski and Szymon Ulatowski.
// See LICENSE.txt for details.

#ifndef _FS_GENERAL_H_
#define _FS_GENERAL_H_

#include <iostream>
#include <vector>
#include <map>
#include <unordered_map>
#include <exception>
#include "frams/model/model.h"
#include "frams/util/multirange.h"

/** @name Values of constants used in encoding */
//@{
#define BRANCH_START '('
#define BRANCH_END ')'
#define BRANCH_SEPARATOR '^'
#define PARAM_START '{'
#define PARAM_END '}'
const char PARAM_SEPARATOR = ';';
const char PARAM_KEY_VALUE_SEPARATOR = '=';
#define NEURON_START '['
const char NEURON_END = ']';
const char NEURON_SEPARATOR = ';';
const SString NEURON_INTERNAL_SEPARATOR("_");
#define NEURON_I_W_SEPARATOR ':'
//@}

enum class SHIFT
{
	LEFT = -1,
	RIGHT = 1
};

/** @name Every modifier changes the underlying value by this multiplier */
const double MODIFIER_MULTIPLIER = 1.1;
/**
 * Used in finding the proper distance between the parts
 * distance between spheres / sphere radius
 * That default value can be changed in certain cases
 * */
const float SPHERE_RELATIVE_DISTANCE = 0.25;
/**
 * Used in finding the proper distance between the parts
 * The maximal allowed value for
 * maximal radius of the node / sphere radius
 */
const int MAX_DIAMETER_QUOTIENT = 30;
/**
 * The tolerance of the value of distance between parts
 */
const double SPHERE_DISTANCE_TOLERANCE = 0.99;


/** @name Names of node parameters and modifiers*/
//@{
#define INGESTION "i"
#define FRICTION "f"
#define STIFFNESS "st"
#define SIZE "s"
#define SIZE_X "x"
#define SIZE_Y "y"
#define SIZE_Z "z"
#define ROT_X "tx"
#define ROT_Y "ty"
#define ROT_Z "tz"
#define RX "rx"
#define RY "ry"
#define RZ "rz"
//@}
/** @name Macros and values used in collision detection */
//@{
#define DISJOINT 0
#define COLLISION 1
#define ADJACENT 2
//@}

#define HINGE_X 'b'
#define HINGE_XY 'c'

const double DEFAULT_NEURO_CONNECTION_WEIGHT = 1.0;

const char ELLIPSOID = 'E';
const char CUBOID = 'C';
const char CYLINDER = 'R';
const std::unordered_map<Part::Shape, char> SHAPETYPE_TO_GENE = {
		{Part::Shape::SHAPE_ELLIPSOID, ELLIPSOID},
		{Part::Shape::SHAPE_CUBOID,    CUBOID},
		{Part::Shape::SHAPE_CYLINDER,  CYLINDER},
};

// This map is inverse to SHAPE_TO_SYMBOL. Those two should be compatible
const std::unordered_map<char, Part::Shape> GENE_TO_SHAPETYPE = {
		{ELLIPSOID, Part::Shape::SHAPE_ELLIPSOID},
		{CUBOID,    Part::Shape::SHAPE_CUBOID},
		{CYLINDER,  Part::Shape::SHAPE_CYLINDER},
};
const int SHAPE_COUNT = 3;    // This should be the count of SHAPETYPE_TO_GENE and GENE_TO_SHAPETYPE

const char DEFAULT_JOINT = 'a';
const string JOINTS = "bc";
const string ALL_JOINTS = "abc";
const int JOINT_COUNT = JOINTS.length();
const string MODIFIERS = "IFST";
const char SIZE_MODIFIER = 's';
const vector<string> PARAMS {INGESTION, FRICTION, ROT_X, ROT_Y, ROT_Z, RX, RY, RZ, SIZE, SIZE_X, SIZE_Y, SIZE_Z,
							 STIFFNESS};

/** @name Default values of node parameters*/
static const Part defPart = Model::getDefPart();
static const Joint defJoint = Model::getDefJoint();
const std::map<Part::Shape, double> volumeMultipliers = {
		{Part::Shape::SHAPE_CUBOID, 8.0},
		{Part::Shape::SHAPE_CYLINDER, 2.0 * M_PI},
		{Part::Shape::SHAPE_ELLIPSOID, (4.0 / 3.0) * M_PI},
};
const std::map<string, double> defaultValues = {
		{INGESTION,      defPart.ingest},
		{FRICTION,       defPart.friction},
		{STIFFNESS,	 	 defJoint.stif},
		{ROT_X,          0.0},
		{ROT_Y,          0.0},
		{ROT_Z,          0.0},
		{RX,             0.0},
		{RY,             0.0},
		{RZ,             0.0},
		{SIZE,           1.0},
		{SIZE_X,         1.0},
		{SIZE_Y,         1.0},
		{SIZE_Z,         1.0}
};

const std::map<string, double> minValues = {
		{INGESTION,      0},
		{FRICTION,       0},
		{STIFFNESS,	 0.0},
		{ROT_X,          -M_PI},
		{ROT_Y,          -M_PI},
		{ROT_Z,          -M_PI},
		{RX,             -M_PI},
		{RY,             -M_PI},
		{RZ,             -M_PI},
		{SIZE,           0.01},
		{SIZE_X,         0.01},
		{SIZE_Y,         0.01},
		{SIZE_Z,         0.01}
};

const std::map<string, double> maxValues = {
		{INGESTION,      1.0},
		{FRICTION,       1.0},
		{STIFFNESS,	 0.0},
		{ROT_X,          M_PI},
		{ROT_Y,          M_PI},
		{ROT_Z,          M_PI},
		{RX,             M_PI},
		{RY,             M_PI},
		{RZ,             M_PI},
		{SIZE,           100.0},
		{SIZE_X,         100.0},
		{SIZE_Y,         100.0},
		{SIZE_Z,         100.0}
};

/** @name Number of tries of performing a mutation before GENOPER_FAIL is returned */
#define mutationTries  20

class fS_Exception : public std::exception
{
	string msg;
public:

	int errorPosition;
	virtual const char *what() const throw()
	{
		return msg.c_str();
	}

	fS_Exception(string _msg, int _errorPosition)
	{
		msg = _msg;
		errorPosition = _errorPosition;
	}
};

/**
 * Draws an integer value from given range
 * @param to maximal value
 * @param from minimal value
 * @return Drawn value
 */
int randomFromRange(int to, int from);

/**
 * Represents a substring of a larger string.
 * The reference to the original string is stored along with indexes of beginning end length of the substring.
 */
class Substring
{
public:
	char *str;        // Pointer to the beginning of the substring
	int start;        // The beginning index of substring
	int len;        // The length of substring

	Substring(const char *_str, int _start, int _len)
	{
		str = (char *) _str + _start;
		start = _start;
		len = _len;
	}

	Substring(const Substring &other)
	{
		str = other.str;
		start = other.start;
		len = other.len;
	}

	const char *c_str()
	{
		return str;
	}

	SString substr(int relativeStart, int len)
	{
		const char *substrStart = str + relativeStart;
		return SString(substrStart, len);
	}

	int indexOf(char ch)
	{
		for (int i = 0; i < len; i++)
			if (str[i] == ch)
				return i;
		return -1;
	}

	void startFrom(int index)
	{
		str += index;
		start += index;
		len -= index;
	}

	void shortenBy(int charCount)
	{
		len = std::max(len - charCount, 0);
	}

	char at(int index)
	{
		return str[index];
	}

	/**
	 * Create a new instance of multirange, corresponding to the substring
	 * @return a created multirange
	 */
	MultiRange toMultiRange()
	{
		MultiRange range;
		range.add(start, start + len - 1);
		return range;
	}
};

/**
 * Stores the state of the node.
 * The state consists od current location, the direction in which the branch develops
 * and the current default values of the parameters (default values can be changed by modifiers).
 */
class State
{
public:
	Pt3D location;  /// Location of the node
	Pt3D v;         /// The normalised vector in which current branch develops
	double fr = 1.0;      /// Friction multiplier
	double ing = 1.0;      /// Ingestion multiplier
	double s = 1.0;      /// Size multipliers
	double stif = 1.0;	/// Stiffness multipliers

	State(State *_state); /// Derive the state from parent

	State(Pt3D _location, Pt3D _v); /// Create the state from parameters

	/**
	 * Add the vector of specified length to location
	 * @param length the length of the vector
	 */
	void addVector(const double length);

	/**
	 * Rotate the vector by specified values
	 * @param rx rotation by x axis
	 * @param ry rotation by y axis
	 * @param rz rotation by z axis
	 */
	void rotate(const Pt3D &rotation);
};

/**
 * Represent a neuron and its inputs
 */
class fS_Neuron: public Neuro
{
public:
	std::map<int, double> inputs;

	fS_Neuron(const char *str, int start, int length);

	bool acceptsInputs()
	{
		return getClass()->prefinputs < int(inputs.size());
	}
};

/**
 * Represents a node in the graph that represents a genotype.
 * A node corresponds to a single part.
 * However, it also stores attributes that are specific to fS encoding, such as modifiers and joint types.
 */
class Node
{
	friend class fS_Genotype;

	friend class GenoOper_fS;

private:
	Substring *partDescription = nullptr;
	Node *parent;
	Part *part;     /// A part object built from node. Used in building the Model
	int partCodeLen; /// The length of substring that directly describes the corresponding part

	std::map<string, double> params; /// The map of all the node params
	vector<Node *> children;    /// Vector of all direct children
	std::map<char, int> modifiers;     /// Vector of all modifiers
	vector<fS_Neuron *> neurons;    /// Vector of all the neurons

	double getDistance();

	void cleanUp();

	Pt3D getRotation();

	Pt3D getVectorRotation();

	bool isPartSizeValid();

	bool hasPartSizeParam();

	/**
	 * Get the position of part type in genotype
	 *
	 * @return the position of part type
	 */
	int getPartPosition(Substring &restOfGenotype);

	/**
	 * Extract modifiers from the rest of genotype
	 * @return the remainder of the genotype
	 */
	void extractModifiers(Substring &restOfGenotype);

	/**
	 * Extract part type from the rest of genotype
	 * @return the remainder of the genotype
	 */
	void extractPartType(Substring &restOfGenotype);

	/**
	 * Extract neurons from the rest of genotype
	 * @return the remainder of the genotype
	 */
	void extractNeurons(Substring &restOfGenotype);

	/**
	 * Extract params from the rest of genotype
	 * @return the length og the remainder of the genotype
	 */
	void extractParams(Substring &restOfGenotype);

	/**
	 * Extract child branches from the rest of genotype
	 * @return vector of child branches
	 */
	vector<Substring> getBranches(Substring &restOfGenotype);

	/**
	 * Get phenotypic state that derives from ancestors.
	 * Used when building model
	 * @param _state state of the parent
	 */
	void getState(State *_state);

	/**
	 * Build children internal representations from fS genotype
	 * @param restOfGenotype part of genotype that describes the subtree
	 */
	void getChildren(Substring &restOfGenotype);

	/**
	 * Create part object from internal representation
	 */
	void createPart();

	/**
	 * Add joints between current node and the specified child
	 * Used in building model
	 * @param mode pointer to build model
	 * @param child pointer to the child
	 */
	void addJointsToModel(Model &model, Node *parent);

	/**
	 * Get all the nodes from the subtree that starts in this node
	 * @param reference to vector which contains nodes
	 */
	void getAllNodes(vector<Node *> &allNodes);


	/**
	 * Build model from the subtree that starts in this node
	 * @param pointer to model
	 */
	void buildModel(Model &model, Node *parent);

public:
	char joint = DEFAULT_JOINT;           /// Set of all joints
	Part::Shape partType;  /// The type of the part
	State *state = nullptr; /// The phenotypic state that inherits from ancestors

	Node(Substring &genotype, Node *parent);

	~Node();

	/**
	 * Get fS representation of the subtree that starts from this node
	 * @param result the reference to an object which is used to contain fS genotype
	 */
	void getGeno(SString &result);

	/**
	 * Calculate the effective size of the part (after applying all multipliers and params)
	 * @return The effective size
	 */
	Pt3D calculateSize();

	/**
	 * Calculate the effective volume of the part
	 * @return The effective volume
	 */
	double calculateVolume();

	/**
	 * Change the value of the size parameter by given multiplier
	 * Do not change the value if any of the size restrictions is not satisfied
	 * @param paramKey
	 * @param multiplier
	 * @param ensureCircleSection
	 * @return True if the parameter value was change, false otherwise
	 */
	bool changeSizeParam(string paramKey,  bool ensureCircleSection);

	/**
	 * Counts all the nodes in subtree
	 * @return node count
	 */
	int getNodeCount();

	/**
	 * Extract the value of parameter or return default if parameter not exists
	 * @return the param value
	 */
	double getParam(string key);
};

/**
 * Represents an fS genotype.
 */
class fS_Genotype
{
	friend class Node;

	friend class GenoOper_fS;

private:
	/**
	 * Draws a node that has an index greater that specified
	 * @param fromIndex minimal index of the node
	 * @return pointer to drawn node
	 */
	Node *chooseNode(int fromIndex=0);

	/**
	 * Draws a value from defined distribution
	 * @return Drawn value
	 */
	void randomFromDistribution();

	/**
	 * Find a node that is nearest (euclidean distance to specified node) and is not a child of specified node
	 * @return Nearest node
	 */
	Node *getNearestNode(vector<Node *> allNodes, Node *node);

public:
	Node *startNode = nullptr;    /// The start (root) node. All other nodes are its descendants


	static int precision;
	static bool TURN_WITH_ROTATION;

	/**
	 * Build internal representation from fS format
	 * @param genotype in fS format
	 */
	fS_Genotype(const string &genotype);

	~fS_Genotype();

	void getState();

	/**
	 * Get all existing nodes
	 * @return vector of all nodes
	 */
	vector<Node *> getAllNodes();

	/**
	 * Get all the neurons from the subtree that starts in given node
	 * @param node The beginning of subtree
	 * @return The vector of neurons
	 */
	static vector<fS_Neuron *> extractNeurons(Node *node);

	/**
	 * Get the index of the neuron in vector of neurons
	 * @param neurons
	 * @param changedNeuron
	 * @return
	 */
	static int getNeuronIndex(vector<fS_Neuron *> neurons, fS_Neuron *changedNeuron);

	/**
	 * Left- or right- shift the indexes of neuro connections by the given range
	 * @param neurons
	 * @param start The beginning of the range
	 * @param end The end of the range
	 * @param shift
	 */
	static void shiftNeuroConnections(vector<fS_Neuron *> &neurons, int start, int end, SHIFT shift);

	/**
	 * Get all existing neurons
	 * @return vector of all neurons
	 */
	vector<fS_Neuron *> getAllNeurons();

	/**
	 * Counts all the nodes in genotype
	 * @return node count
	 */
	int getNodeCount();

	/**
	 * Check if sizes of all parts in genotype are valid
	\retval error_position 1-based
	\retval 0 when all part sizes are valid
	 */
	int checkValidityOfPartSizes();

	void validateNeuroInputs();

	/**
	 * Builds Model object from internal representation
	 * @param a reference to a model that will contain a built model
	 */
	void buildModel(Model &model);

	/**
	 * Adds neuro connections to model
	 * @param a reference to a model where the connections will be added
	 */
	void buildNeuroConnections(Model &model);

	/**
	 * @return genotype in fS format
	 */
	SString getGeno();

	/**
	 * After creating or deleting a new neuron, rearrange other neurons so that the inputs match
	 */
	void rearrangeNeuronConnections(fS_Neuron *newNeuron, SHIFT shift);

};


#endif