#define _USE_MATH_DEFINES

#include <stdlib.h>
#include <time.h>
#include <math.h>

#include <climits>
#include <cfloat>
#include <cmath>

#include <boost/multiprecision/cpp_int.hpp>

#include "externals.h"
#include "typedValue.h"
#include "stack.h"
#include "word.h"
#include "context.h"
#include "mathMacro.h"

const BigInt kBigInt_FLT_MAX(FLT_MAX);
const BigInt kBigInt_Minus_FLT_MAX(-FLT_MAX);
const BigInt kBigInt_DBL_MAX(DBL_MAX);
const BigInt kBigInt_Minus_DBL_MAX(-DBL_MAX);

static double deg2rad(double inTheta) { return inTheta/180.0*M_PI; }
static BigFloat deg2rad(BigFloat inTheta) { return inTheta/180.0*M_PI; }
static double rad2deg(double inTheta) { return inTheta/M_PI*180.0; }
static BigFloat rad2deg(BigFloat inTheta) { return inTheta/M_PI*180.0; }

void InitDict_Math() {
	Install(new Word("true",WORD_FUNC {
		inContext.DS.emplace_back(true);
		NEXT;
	}));

	Install(new Word("false",WORD_FUNC {
		inContext.DS.emplace_back(false);
		NEXT;
	}));

	Install(new Word("+",WORD_FUNC { TwoOp(+); }));
	Install(new Word("-",WORD_FUNC { TwoOp(-); }));

	Install(new Word("*",WORD_FUNC { TwoOp(*); }));
	Install(new Word("/",WORD_FUNC { TwoOp(/); }));

	Install(new Word("%",WORD_FUNC {
		Stack& ds=inContext.DS;
		if(ds.size()<2) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue tos=Pop(ds);
		TypedValue& second=ds.back();

		// second %= tos;
		ModAssign(second,tos);
		NEXT;
   }));

	Install(new Word("&",WORD_FUNC { BitOp(&); }));
	Install(new Word("|",WORD_FUNC { BitOp(|); }));
	Install(new Word("^",WORD_FUNC { BitOp(^); }));
	Install(new Word(">>",WORD_FUNC { BitShiftOp(>>); }));
	Install(new Word("<<",WORD_FUNC { BitShiftOp(<<); }));

	// bitwise NOT.
	Install(new Word("~",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt: 		tos.intValue=~tos.intValue;		break;
			case kTypeLong:		tos.longValue=~tos.longValue;	break;
			case kTypeBigInt:	*tos.bigIntPtr=~*tos.bigIntPtr;	break;
			default:
				return inContext.Error_InvalidType(E_TOS_INT_OR_LONG_OR_BIGINT,tos);
		}
		NEXT;
	}));

	Install(new Word(">",WORD_FUNC { CmpOp(>); }));
	Install(new Word("<",WORD_FUNC { CmpOp(<); }));

	Install(new Word(">=",WORD_FUNC { CmpOp(>=); }));
	Install(new Word("<=",WORD_FUNC { CmpOp(<=); }));

	Install(new Word("==",WORD_FUNC { CmpOp(==); }));
	Install(new Word("!=",WORD_FUNC { CmpOp(!=); }));

	Install(new Word("and",WORD_FUNC { BoolOp(&&); }));
	Install(new Word("or",WORD_FUNC { BoolOp(||); }));
	Install(new Word("xor",WORD_FUNC { BoolOp(!=); }));
	Install(new Word("not",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_AT_LEAST_2); }
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType!=kTypeBool) {
			return inContext.Error_InvalidType(E_TOS_BOOL,tos);
		}
		tos.boolValue = tos.boolValue != true;
		NEXT;
	}));

	Install(new Word("&&",WORD_FUNC {
		if(inContext.DS.size()<2) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue tos=Pop(inContext.DS);
		if(tos.dataType!=kTypeBool && tos.dataType!=kTypeWord) {
			return inContext.Error_InvalidType(E_TOS_BOOL_OR_WP,tos);
		}

		TypedValue second=Pop(inContext.DS);
		if(second.dataType!=kTypeBool && second.dataType!=kTypeWord) {
			return inContext.Error_InvalidType(E_SECOND_BOOL_OR_WP,second);
		}

		if(tos.dataType==kTypeBool) {
			if(tos.boolValue==false) {
				inContext.DS.emplace_back(false);
				goto next;
			}
		} else {
			if(inContext.Exec(tos)==false) { return false; }
			tos=Pop(inContext.DS);
			if(tos.dataType!=kTypeBool) {
				return inContext.Error(E_WP_AT_TOS_SHOULD_PUSH_A_BOOL);
			}
			if(tos.boolValue==false) {
				inContext.DS.emplace_back(false);
				goto next;
			}
		}

		if(second.dataType==kTypeBool) {
			if(second.boolValue==false) {
				inContext.DS.emplace_back(false);
				goto next;
			}
		} else {
			if(inContext.Exec(second)==false) {
				return false;
			}
			second=Pop(inContext.DS);
			if(second.dataType!=kTypeBool) {
				return inContext.Error(E_WP_AT_SECOND_SHOULD_PUSH_A_BOOL);
			}
			if(second.boolValue==false) {
				inContext.DS.emplace_back(false);
				goto next;
			}
		}
		inContext.DS.emplace_back(true);
next:
		NEXT;
	}));
	Install(new Word("||",WORD_FUNC {
		if(inContext.DS.size()<2) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue tos=Pop(inContext.DS);
		if(tos.dataType!=kTypeBool && tos.dataType!=kTypeWord) {
			return inContext.Error_InvalidType(E_TOS_BOOL_OR_WP,tos);
		}

		TypedValue second=Pop(inContext.DS);
		if(second.dataType!=kTypeBool && second.dataType!=kTypeWord) {
			return inContext.Error_InvalidType(E_SECOND_BOOL_OR_WP,tos);
		}

		if(tos.dataType==kTypeBool) {
			if(tos.boolValue==true) {
				inContext.DS.emplace_back(true);
				goto next;
			}
		} else {
			if(inContext.Exec(tos)==false) { return false; }
			tos=Pop(inContext.DS);
			if(tos.dataType!=kTypeBool) {
				return inContext.Error(E_WP_AT_TOS_SHOULD_PUSH_A_BOOL);
			}
			if(tos.boolValue==true) {
				inContext.DS.emplace_back(true);
				goto next;
			}
		}

		if(second.dataType==kTypeBool) {
			if(second.boolValue==true) {
				inContext.DS.emplace_back(true);
				goto next;
			}
		} else {
			if(inContext.Exec(second)==false) {
				return false;
			}
			second=Pop(inContext.DS);
			if(second.dataType!=kTypeBool) {
				return inContext.Error(E_WP_AT_SECOND_SHOULD_PUSH_A_BOOL);
			}
			if(second.boolValue==true) {
				inContext.DS.emplace_back(true);
				goto next;
			}
		}
		inContext.DS.emplace_back(false);
next:
		NEXT;
	}));

	Install(new Word("sqrt",WORD_FUNC { OneArgFloatingFunc(sqrt); }));
	Install(new Word("exp", WORD_FUNC { OneParamFunc(exp);  }));
	Install(new Word("log", WORD_FUNC { OneParamFunc(log);  }));
	Install(new Word("log10", WORD_FUNC { OneParamFunc(log10);  }));

	Install(new Word("sin",WORD_FUNC { OneParamFunc(sin); }));
	Install(new Word("cos",WORD_FUNC { OneParamFunc(cos); }));
	Install(new Word("tan",WORD_FUNC { OneParamFunc(tan); }));

	Install(new Word("asin",WORD_FUNC { OneParamFunc(asin); }));
	Install(new Word("acos",WORD_FUNC { OneParamFunc(acos); }));
	Install(new Word("atan",WORD_FUNC { OneParamFunc(atan); }));

	Install(new Word("floor",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		FloorOrCeil(floor,tos);
		NEXT;
	}));

	Install(new Word("ceil",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		FloorOrCeil(ceil,tos);
		NEXT;
	}));

	Install(new Word("deg-to-rad",WORD_FUNC { OneParamFunc(deg2rad); }));
	Install(new Word("rad-to-deg",WORD_FUNC { OneParamFunc(rad2deg); }));

	Install(new Word("pow",WORD_FUNC {
		using namespace boost::multiprecision;

		if(inContext.DS.size()<2) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue tos=Pop(inContext.DS);
		TypedValue& second=ReadTOS(inContext.DS);
		if(second.dataType==kTypeDouble) {
			switch(tos.dataType) {
				case kTypeInt: /* double x int -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.intValue);
					break;
				case kTypeLong: /* double x long -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.longValue);
					break;
				case kTypeFloat: /* double x float -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.floatValue);
					break;
				case kTypeDouble: /* double x double -> dobule */
					second.doubleValue=pow(second.doubleValue,tos.doubleValue);
					break;
				case kTypeBigInt: { /* double x bigInt -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.doubleValue),
									  BigFloat(*tos.bigIntPtr));
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				case kTypeBigFloat: { /* double x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.doubleValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeBigInt) {
			switch(tos.dataType) {
				case kTypeInt:	/* bigInt x int -> bigInt */
					*second.bigIntPtr=pow(*second.bigIntPtr,tos.intValue);
					break;
				case kTypeLong:	/* bigInt x long -> bigInt */
					*second.bigIntPtr=pow(*second.bigIntPtr,tos.longValue);
					break;
				case kTypeFloat: {	/* bigInt x float -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),
									  BigFloat(tos.floatValue));
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				case kTypeDouble: {	/* bigInt x double -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),
									  BigFloat(tos.doubleValue));
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				case kTypeBigInt: /* bigInt x bigInt -> bigInt */
					return inContext.Error_InvalidType(E_OUT_OF_SUPPORT_TOS_SECOND,
													   tos,second);
				case kTypeBigFloat: { /* bigInt x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),*tos.bigFloatPtr);
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeBigFloat) {
			switch(tos.dataType) {
				case kTypeInt: /* bigFloat x int -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.intValue));
					break;
				case kTypeLong: /* bigFloat x long -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											 BigFloat(tos.longValue));
					break;
				case kTypeFloat: /* bigFloat x float -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.floatValue));
					break;
				case kTypeDouble: /* bigFloat x double -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.doubleValue));
					break;
				case kTypeBigInt: /* bigFloat x bigInt -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(*tos.bigIntPtr));
					break;
				case kTypeBigFloat: /* bigFloat x bigFloat -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,*tos.bigFloatPtr);
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeInt) {
			switch(tos.dataType) {
				case kTypeInt:	/* int x int -> int */
					second.intValue=(int)pow(second.intValue,tos.intValue);
					break;
				case kTypeLong:	/* int x long -> long */
					second.longValue=(long)pow((long)second.intValue,tos.longValue);
					second.dataType=kTypeLong;
					break;
				case kTypeFloat: /* int x float -> float */
					second.floatValue=(float)pow((float)second.intValue,
												 tos.floatValue);
					second.dataType=kTypeFloat;
					break;
				case kTypeDouble: /* int x double -> double */
					second.doubleValue=(double)pow((double)second.intValue,
												   tos.doubleValue);
					second.dataType=kTypeDouble;
					break;
				case kTypeBigInt: /* int x bigInt -> OutOfSupport */
					return inContext.Error_InvalidType(E_OUT_OF_SUPPORT_TOS_SECOND,
													   tos,second);
				case kTypeBigFloat: { /* int x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.intValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeLong) {
			switch(tos.dataType) {
				case kTypeInt:	/* long x int -> long */
					second.longValue=(long)pow(second.longValue,(long)tos.intValue);
					break;
				case kTypeLong:	/* long x long -> long */
					second.longValue=(long)pow(second.longValue,tos.longValue);
					break;
				case kTypeFloat:	/* long x float -> float */
					second.floatValue=(float)pow((float)second.longValue,
												 tos.floatValue);
					second.dataType=kTypeFloat;
					break;
				case kTypeDouble:	/* long x double -> double */
					second.doubleValue=(double)pow((double)second.longValue,
												   tos.doubleValue);
					second.dataType=kTypeDouble;
					break;
				case kTypeBigInt: /* long x bigInt -> OutOfSupport */
					return inContext.Error_InvalidType(E_OUT_OF_SUPPORT_TOS_SECOND,
													   tos,second);
				case kTypeBigFloat: { /* long x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.longValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeFloat) {
			switch(tos.dataType) {
				case kTypeInt: /* float x int -> float */
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.intValue);
					break;
				case kTypeLong: /* float x long -> float*/
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.longValue);
					break;
				case kTypeFloat: /* float x float -> float */
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.floatValue);
					break;
				case kTypeDouble: /* float x double -> dobule */
					second.doubleValue=pow((double)second.floatValue,tos.doubleValue);
					second.dataType=kTypeDouble;
					break;
				case kTypeBigInt: { /* float x bigInt -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.floatValue),
									  BigFloat(*tos.bigIntPtr));
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				case kTypeBigFloat: { /* float x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.floatValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else {
onError:
			return inContext.Error_InvalidType(E_INVALID_DATA_TYPE_TOS_SECOND,
											   tos,second);
		}
		NEXT;
	}));

	Install(new Word("0?",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:
				tos.dataType=kTypeBool;
				tos.boolValue=tos.intValue==0;
				break;
			case kTypeLong:
				tos.dataType=kTypeBool;
				tos.boolValue=tos.longValue==0;
				break;
			case kTypeBigInt: {
					BigInt *biPtr=tos.bigIntPtr;
					tos.dataType=kTypeBool;
					tos.boolValue=*biPtr==0;
					delete(biPtr);
				}
				break;
			case kTypeFloat:
				tos.dataType=kTypeBool;
				tos.boolValue=tos.floatValue==0;
				break;
			case kTypeDouble:
				tos.dataType=kTypeBool;
				tos.boolValue=tos.doubleValue==0;
				break;
			case kTypeBigFloat: {
					BigFloat *bfPtr=tos.bigFloatPtr;
					tos.dataType=kTypeBool;
					tos.boolValue=*bfPtr==0;
					delete(bfPtr);
				}
				break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	// equivalent to '1 +'.
	Install(new Word("1+",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:		tos.intValue+=1;		break;
			case kTypeFloat:	tos.floatValue+=1;		break;
			case kTypeDouble:	tos.doubleValue+=1;		break;
			case kTypeBigInt:	*tos.bigIntPtr+=1;		break;
			case kTypeBigFloat:	*tos.bigFloatPtr+=1;	break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	// equivalent to '1 -'.
	Install(new Word("1-",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:		tos.intValue-=1;	break;
			case kTypeFloat:	tos.floatValue-=1;	break;
			case kTypeDouble:	tos.doubleValue-=1;	break;
			case kTypeBigInt:	*tos.bigIntPtr-=1;	break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word("2/",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:		tos.intValue/=2;		break;
			case kTypeFloat:	tos.floatValue/=2.0f;	break;
			case kTypeDouble:	tos.doubleValue/=2.0;	break;
			case kTypeBigInt:	*tos.bigIntPtr/=2;		break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word("rand-max",WORD_FUNC {
		inContext.DS.emplace_back(RAND_MAX);
		NEXT;
	}));

	// [0,RAND_MAX]
	Install(new Word("rand",WORD_FUNC {
		inContext.DS.emplace_back(rand());
		NEXT;
	}));

	// [0,1]
	Install(new Word("random",WORD_FUNC {
		inContext.DS.emplace_back((float)rand()/(float)RAND_MAX);
		NEXT;
	}));

	Install(new Word("randomize",WORD_FUNC {
		srand((unsigned int)time(NULL));
		NEXT;
	}));

	// 整数 --- 数値
	// n --- t
	// s.t. t is an integer and t in [0,n).
	Install(new Word("rand-to",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue tos=Pop(inContext.DS);
		if(tos.dataType!=kTypeInt) {
			return inContext.Error_InvalidType(E_TOS_INT,tos);
		}

		const int n=tos.intValue;
		if(n<0 || RAND_MAX<n) { return inContext.Error(E_TOS_POSITIVE_INT,n); }
		int t=(int)(rand()/((float)RAND_MAX+1)*n);
		inContext.DS.emplace_back(t);
		NEXT;
	}));

	Install(new Word("pi",WORD_FUNC {
		inContext.DS.emplace_back(M_PI);
		NEXT;
	}));

	Install(new Word(">int",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType==kTypeFloat) {
			if(tos.floatValue<INT_MIN || INT_MAX<tos.floatValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.floatValue;
		} else if(tos.dataType==kTypeDouble) {
			if(tos.doubleValue<INT_MIN || INT_MAX<tos.doubleValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.doubleValue;
		} else if(tos.dataType==kTypeLong) {
			if(tos.longValue<(long)INT_MIN || (long)INT_MAX<tos.longValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.longValue;
		} else if(tos.dataType==kTypeBigInt) {
			if(*tos.bigIntPtr<INT_MIN || INT_MAX<*tos.bigIntPtr) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=static_cast<int>(*tos.bigIntPtr);
		} else if(tos.dataType==kTypeAddress) {
			tos.dataType=kTypeInt;
		} else if(tos.dataType!=kTypeInt) {
			return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		tos.dataType=kTypeInt;
		NEXT;
	}));

	Install(new Word(">long",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType==kTypeFloat) {
			if(tos.floatValue<LONG_MIN || LONG_MAX<tos.floatValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=(long)tos.floatValue;
		} else if(tos.dataType==kTypeDouble) {
			if(tos.doubleValue<LONG_MIN || LONG_MAX<tos.doubleValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=(long)tos.doubleValue;
		} else if(tos.dataType==kTypeInt) {
			tos.longValue=(long)tos.intValue;
		} else if(tos.dataType==kTypeBigInt) {
			if(*tos.bigIntPtr<LONG_MIN || LONG_MAX<*tos.bigIntPtr) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=static_cast<long>(*tos.bigIntPtr);
		} else if(tos.dataType!=kTypeLong) {
			return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		tos.dataType=kTypeLong;
		NEXT;
	}));

	Install(new Word(">INT",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue tos=Pop(inContext.DS);
		BigInt bigInt;
		switch(tos.dataType) {
			case kTypeInt:	  bigInt=tos.intValue;		break;
			case kTypeLong:	  bigInt=tos.longValue;		break;
			case kTypeFloat:  bigInt=static_cast<BigInt>(tos.floatValue);	break;
			case kTypeDouble: bigInt=static_cast<BigInt>(tos.doubleValue);	break;
			case kTypeString: bigInt=BigInt(*tos.stringPtr); break;
			case kTypeBigInt:
				// do nothing
				break;
			case kTypeBigFloat: bigInt=static_cast<BigInt>(*tos.bigFloatPtr); break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER_OR_STRING,tos);
		}
		if(tos.dataType!=kTypeBigInt) {
			inContext.DS.emplace_back(bigInt);
		} else {
			inContext.DS.emplace_back(*tos.bigIntPtr);
		}
		NEXT;
	}));

	Install(new Word(">float",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:
				tos.floatValue=(float)tos.intValue;
				tos.dataType=kTypeFloat;
				break;
			case kTypeLong:
				tos.floatValue=(float)tos.longValue;
				tos.dataType=kTypeFloat;
				break;
			case kTypeBigInt: {
					if(*tos.bigIntPtr>kBigInt_FLT_MAX
					  || *tos.bigIntPtr<kBigInt_Minus_FLT_MAX) {
						return inContext.Error(
								E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
					}
					float f=tos.ToFloat(inContext);
					delete tos.bigIntPtr;
					tos.floatValue=f;
					tos.dataType=kTypeFloat;
				}
				break;
			case kTypeFloat:
				// do nothing
				break;
			case kTypeDouble:
				if(tos.doubleValue>FLT_MAX || tos.doubleValue<-FLT_MAX) {
					return inContext.Error(E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
				}
				tos.floatValue=(float)tos.doubleValue;
				tos.dataType=kTypeFloat;
				break;
			case kTypeBigFloat: {
					if(*tos.bigFloatPtr>FLT_MAX || *tos.bigFloatPtr<-FLT_MAX) {
						return inContext.Error(
								E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
					}
					float f=tos.ToFloat(inContext);
					delete tos.bigFloatPtr;
					tos.floatValue=f;
					tos.dataType=kTypeFloat;
				}
				break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word(">double",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:
				tos.doubleValue=(double)tos.intValue;
				tos.dataType=kTypeDouble;
				break;
			case kTypeLong:
				tos.doubleValue=(double)tos.longValue;
				tos.dataType=kTypeDouble;
				break;
			case kTypeBigInt: {
					if(*tos.bigIntPtr>kBigInt_DBL_MAX
					  || *tos.bigIntPtr<kBigInt_Minus_DBL_MAX) {
						return inContext.Error(
								E_CAN_NOT_CONVERT_TO_DOUBLE_DUE_TO_OVERFLOW);
					}
					double t=tos.ToDouble(inContext);
					delete tos.bigIntPtr;
					tos.doubleValue=t;
					tos.dataType=kTypeDouble;
				}
				break;
			case kTypeFloat:
				tos.doubleValue=(double)tos.floatValue;
				tos.dataType=kTypeDouble;
				break;
			case kTypeDouble:
				// do nothing
				break;
			case kTypeBigFloat: {
					if(*tos.bigFloatPtr>DBL_MAX || *tos.bigFloatPtr<-DBL_MAX) {
						return inContext.Error(
								E_CAN_NOT_CONVERT_TO_DOUBLE_DUE_TO_OVERFLOW);
					}
					double t=tos.ToDouble(inContext);
					delete tos.bigFloatPtr;
					tos.doubleValue=t;
					tos.dataType=kTypeDouble;
				}
				break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word(">FLOAT",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue tos=Pop(inContext.DS);
		BigFloat bigFloat;
		switch(tos.dataType) {
			case kTypeInt:	  bigFloat=tos.intValue;	break;
			case kTypeLong:	  bigFloat=tos.longValue;	break;
			case kTypeFloat:  bigFloat=tos.floatValue;	break;
			case kTypeDouble: bigFloat=tos.doubleValue;	break;
			case kTypeString: bigFloat=BigFloat(*tos.stringPtr); break;
			case kTypeBigInt: bigFloat=static_cast<BigFloat>(*tos.bigIntPtr); break;
			case kTypeBigFloat: /* do nothing */ 		break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER_OR_STRING,tos);
		}
		if(tos.dataType!=kTypeBigFloat) {
			inContext.DS.emplace_back(bigFloat);
		} else {
			inContext.DS.emplace_back(*tos.bigFloatPtr);
		}
		NEXT;
	}));

	Install(new Word(">address",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType!=kTypeInt && tos.dataType!=kTypeAddress) {
			return inContext.Error_InvalidType(E_TOS_INT,tos);
		}
		tos.dataType=kTypeAddress;
		NEXT;
	}));
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2015-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#include <iostream>

#include "dune/common/fvector.hh"

#include "linalg/numaMatrix.hh"

using namespace Kaskade;

#ifdef UNITTEST
#include <boost/timer/timer.hpp>

int main(void)
{
  using namespace std;

  std::cout << "8x8 matrix a_ij = i+j\n";
  NumaDenseMatrix<Dune::FieldVector<double,1>> mat(8,8);
  for (auto ri=mat.begin(); ri!=mat.end(); ++ri)
    for (auto ci=ri->begin(); ci!=ri->end(); ++ci)
      *ci = ri.index()+ci.index();

  std::cout << mat << "\nFrobenius norm: " << mat.frobenius_norm() << "\n";

  std::cout << "Timing test of 100 additions of two 60000x100 matrices\n";
  NumaDenseMatrix<Dune::FieldMatrix<double,1,1>> m2(60000,100);
  for (auto ri=m2.begin(); ri!=m2.end(); ++ri)
    for (auto ci=ri->begin(); ci!=ri->end(); ++ci)
      *ci = ri.index()+ci.index();
  boost::timer::cpu_timer timer;
  for (int i=0; i<100; ++i)
  {
    m2 += m2; // these two are a no-op
    m2 /= 2;
  }
  std::cout << timer.format() << "\n";
  // subtract original data - should be zero.
  for (auto ri=m2.begin(); ri!=m2.end(); ++ri)
    for (auto ci=ri->begin(); ci!=ri->end(); ++ci)
      *ci -= ri.index()+ci.index();
  std::cout << "Frobenius norm (should be 0): " << m2.frobenius_norm() << "\n";

  NumaVector<double> vec(12);
  for (auto i=begin(vec); i!=end(vec); ++i)
    *i = i.index();
  std::cout << "vec 12: " << vec << "\ndot product (should be 506): " << vec.dot(vec) << "\n2-norm (should be 22.4944): " << vec.two_norm() << "\n";

  vec.axpy(1.0,vec);
  std::cout << "twice vec: " << vec << "\n";

  return 0;
}

#endif


#include <gtest/gtest.h>
#include <ros/package.h>

#include "mwoibn/eigen_utils/eigen_utils.h"
#include <Eigen/Core>

#include <iostream>
#include <fstream>

bool skipLine(std::ifstream* myfile, int skip)
{
  std::string line;
  try
  {
    for (int i = 0; i < skip; i++)
      getline(*myfile, line);
  }
  catch (const std::exception& exc)
  {
    ADD_FAILURE() << exc.what(); // add proper throws
  }
  return true;
}

Eigen::MatrixXd readMatrix(std::ifstream* myfile, int rows, int cols)
{

  std::string line;

  std::vector<double> data;
  std::vector<double> new_data;
  for (int i = 0; i < rows; i++)
  {
    getline(*myfile, line);
    std::istringstream is(line);
    new_data = std::vector<double>(std::istream_iterator<double>(is),
                                   std::istream_iterator<double>());
    data.insert(data.end(), new_data.begin(), new_data.end());
  }

  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      J(&data[0], rows,
        cols); // requires c++11, "cannot-appear-in-a-constant-expression"

  return J;
}

::testing::AssertionResult compareMatrices(Eigen::MatrixXd m1,
                                           Eigen::MatrixXd m2, double eps)
{

  if (!m1.rows() == m2.rows())
    return ::testing::AssertionFailure()
           << " matrices has diffrenet row numbers"
           << "m1: " << m1.rows() << ", m2: " << m2.rows();
  if (!m1.cols() == m2.cols())
    return ::testing::AssertionFailure()
           << " matrices has diffrenet columns numbers"
           << "m1: " << m1.cols() << ", m2: " << m2.cols();

  for (int i = 0; i < m1.rows(); i++)
  {
    for (int j = 0; j < m1.cols(); j++)
    {
      if (fabs(m1(i, j) - m2(i, j)) > eps)
        return ::testing::AssertionFailure() << "row " << i << ", columns " << j
                                             << ", value a " << m1(i, j)
                                             << ", value b " << m2(i, j);
    }
  }

  return ::testing::AssertionSuccess();
}

// Check if class initialization works correctly

// Check all class methods, orientation is not fully supported therefore it is
// not checked during tests
TEST(EigenUtilsTest, methods)
{

  // READ FILE
  std::string path = ros::package::getPath("eigen_utils");
  std::string file_name = path + "/test/resources/eigen_test.txt";
  std::ifstream myfile(file_name);
  if (!myfile.is_open())
    FAIL() << "couldn't open the file, abort";

  // Prepare variables
  float eps = 0.0001;
  Eigen::MatrixXd J_test;
  Eigen::MatrixXd J;

  // READ TEST MATRICES
  Eigen::MatrixXd m_square;
  Eigen::MatrixXd m_nonsquare;

  try
  {
    // read square matrix
    if (!skipLine(&myfile, 1))
      FAIL() << "couldn't continue, file finished prematurely";
    m_square = readMatrix(&myfile, 3, 3);

    // read non-sqaure
    if (!skipLine(&myfile, 1))
      FAIL() << "couldn't continue, file finished prematurely";
    m_nonsquare = readMatrix(&myfile, 5, 9);
  }
  catch (const std::exception& exc)
  {
    ADD_FAILURE() << exc.what();
  }
  catch (...)
  {
    ADD_FAILURE()
        << "Unknown exception, while getting PointHandling information";
  }

  // CHECK_METHODS
  try
  {
    // FIRST MATRIX TEST: pseudoInverse for square matrix with too low damping
    // Skip to first line
    if (!skipLine(&myfile, 1))
      FAIL() << "couldn't continue, file finished prematurely";
    // Read first matrix
    J = readMatrix(&myfile, 3, 3);
    // Compute test matrix
    J_test = mwoibn::eigen_utils::pseudoInverse(m_square, 1e-18);
    // Test first matrix
    EXPECT_TRUE(compareMatrices(J, J_test, 1));

    // SECOND MATRIX TEST: pseudoInverse for square matrix with correct damping
    if (!skipLine(&myfile, 1))
      FAIL() << "couldn't continue, file finished prematurely";
    J = readMatrix(&myfile, 3, 3);
    J_test = mwoibn::eigen_utils::pseudoInverse(m_square, 1e-8);
    EXPECT_TRUE(compareMatrices(J, J_test, eps));

    // THIRD MATRIX TEST: pseudoInverse for non-square matrix with defult
    // damping
    if (!skipLine(&myfile, 1))
      FAIL() << "couldn't continue, file finished prematurely";
    J = readMatrix(&myfile, 9, 5);
    J_test = Eigen::MatrixXd::Zero(9, 5);
    J_test = mwoibn::eigen_utils::pseudoInverse(m_nonsquare);
    EXPECT_TRUE(compareMatrices(J, J_test, eps));

    // FOURTH MATRIX TEST: agumented Null Space Projection Matrix
    Eigen::MatrixXd P;
    if (!skipLine(&myfile, 1))
      FAIL() << "couldn't continue, file finished prematurely";
    Eigen::MatrixXd P0 = readMatrix(&myfile, 9, 9);

    if (!skipLine(&myfile, 1))
      FAIL() << "couldn't continue, file finished prematurely";
    J = readMatrix(&myfile, 9, 5);
    J_test = Eigen::MatrixXd::Zero(9, 5);
    J_test = mwoibn::eigen_utils::agumentedNullSpaceProjection(m_nonsquare, P0, P);
    EXPECT_TRUE(compareMatrices(J, J_test, eps));
  }
  catch (const std::exception& exc)
  {
    ADD_FAILURE() << exc.what();
  }
  catch (...)
  {
    ADD_FAILURE()
        << "Unknown exception, while getting PointHandling information";
  }
}

int main(int argc, char** argv)
{
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/**
 * @file radauthreetimestepping.cc
 * @brief NPDE homework RadauThreeTimestepping
 * @author Erick Schulz, edited by Oliver Rietmann
 * @date 08/04/2019
 * @copyright Developed at ETH Zurich
 */

#include "radauthreetimestepping.h"

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseLU>
#include <cmath>
#include <iostream>
#include <unsupported/Eigen/KroneckerProduct>

namespace RadauThreeTimestepping {

/**
 * @brief Implementation of the right hand side (time dependent) source vector
 * for the parabolic heat equation
 * @param dofh A reference to the DOFHandler
 * @param time The time at which to evaluate the source vector
 * @returns The source vector at time `time`
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::VectorXd rhsVectorheatSource(const lf::assemble::DofHandler &dofh,
                                    double time) {
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());
  // Right-hand side vector has to be set to zero initially
  Eigen::VectorXd phi(N_dofs);
  //====================
  // Your code goes here
  //====================
  return phi;
}
/* SAM_LISTING_END_1 */

/**
 * @brief Heat evolution solver: the solver obtains the
 * discrete evolution operator from the Radau3MOLTimestepper class and
 * repeatedly iterates its applicaiton starting from the initial condition
 * @param dofh The DOFHandler object
 * @param m is total number of steps until final time final_time (double)
 * @param final_time The duration for which to solve the PDE
 * @returns The solution at the final timestep
 */
/* SAM_LISTING_BEGIN_6 */
Eigen::VectorXd solveHeatEvolution(const lf::assemble::DofHandler &dofh,
                                   unsigned int m, double final_time) {
  Eigen::VectorXd discrete_heat_sol(dofh.NumDofs());
  //====================
  // Your code goes here
  //====================
  return discrete_heat_sol;
}
/* SAM_LISTING_END_6 */

/* Implementing member function Eval of class LinFEMassMatrixProvider*/
Eigen::Matrix<double, 3, 3> LinFEMassMatrixProvider::Eval(
    const lf::mesh::Entity &tria) {
  Eigen::Matrix<double, 3, 3> elMat;
  //====================
  // Your code goes here
  //====================
  return elMat;  // return the local mass element matrix
}

/* Implementing constructor of class Radau3MOLTimestepper */
/* SAM_LISTING_BEGIN_4 */
Radau3MOLTimestepper::Radau3MOLTimestepper(const lf::assemble::DofHandler &dofh)
    : dofh_(dofh) {
  //====================
  // Your code goes here
  // Add any additional members you need in the header file
  //====================
}
/* SAM_LISTING_END_4 */

/* Implementation of Radau3MOLTimestepper member functions */
// The function discreteEvolutionOperator() returns the discretized evolution
// operator as obtained from the Runge-Kutta Radau IIA 2-stages method using the
// Butcher table as stored in the Radau3MOLTimestepper class
/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd Radau3MOLTimestepper::discreteEvolutionOperator(
    double time, double tau, const Eigen::VectorXd &mu) const {
  Eigen::VectorXd discrete_evolution_operator(dofh_.NumDofs());
  //====================
  // Your code goes here
  //====================
  return discrete_evolution_operator;
}
/* SAM_LISTING_END_5 */

}  // namespace RadauThreeTimestepping


#include <iostream>
#include <iomanip>
#include <fstream>
#include <cmath>
#include <vector>
#include <chrono>
#include <string>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include "../src/markovchains/MarkovChain.h"

using namespace Eigen;
using namespace std;
using namespace MarkovChain;

// Eigen Flags
//#define EIGEN_NDEBUG
//#define EIGEN_NO_STATIC_ASSERT

//// ENUMERATED CLASS FOR POLICY TYPE
enum class Policies
{
    IDLE,
    HIRE_LOW_WAGE,
    HIRE_HIGH_WAGE,
    LAYOFF_LOW_WAGE,
    LAYOFF_HIGH_WAGE,
    EXIT,
};

//// FUNCTION PHI THAT RETURNS SCALING OF HIRING ENTRY BY FIRM SIZE (PART OF THE PARAMETERS OF THE MODEL!!!)
double phi(int total_employment){
    return total_employment + 1;
}

//// MAIN PROGRAM
int main()
{
    const bool verbose = true;
    const bool graphs = true;
    const bool tables = false;
    const bool load_guess_from_file = false;
    const bool save_guess_to_file = false;

    // Greetings
    if (verbose) {
        cout <<"----------------------------------------------\n";
        cout <<"------| SOLVING THE PROBLEM OF THE FIRM |-----\n";
        cout <<"----------------------------------------------\n\n";
    }

    // Start Clock
    auto t0 = std::chrono::high_resolution_clock::now();

    // Endogenous Variables (should be passed as arguments of the function)
    double hiring_rate_low = 0.1;
    double hiring_rate_high = 3.0;
    double quit_rate_low = 0.07;

    // Parameters of the state space
    const int nw = 2;       // ASSUME N_w = 2 for the time being
    const int nz = 4;       // For now fix it at nz = 4
    const int nbar = 60;
    const double wl = 1.0;
    const double wh = 1.25;

    // Parameters of the model (read from file)
    const double decreasing_returns = 0.666666666;
    const double real_rate = 0.01;
    const double discount_rate = log(1.0 + real_rate);
    const double fixed_cost = 1.5;
    const double exit_rate = 1e5;
    const double layoff_rate = 13.0;
    const double layoff_cost = 1.0;
    const double exogenous_separations_rate = 1.0/16.0;
    const double aggregate_productivity = 1.0;

    // Parameters for HJB
    const double hjb_criterion = 1e-10;
    const int hjb_maxit = 100;
    const double hjb_step_size = 1e8;
    const double hjb_criterion_BiCGSTAB = 1e-6; // Must be relative to criterion_hjb

    // Parameters for KFE
    const double kfe_criterion = 1e-10;
//    const int kfe_maxit = 100;
    const double kfe_step_size = 1e8;
    const double criterion_kfe_BiCGSTAB = 1e-6; // Must be relative to criterion_hjb

    // Infinitesimal Generator of Poisson Process for Productivity and set \mathcal{Z}.
    Matrix4d Q;
    Q(0,0) = -0.00; Q(0,1) =  0.00; Q(0,2) =  0.00; Q(0,3) =  0.00;
    Q(1,0) =  0.10; Q(1,1) = -0.55; Q(1,2) =  0.30; Q(1,3) =  0.15;
    Q(2,0) =  0.10; Q(2,1) =  0.25; Q(2,2) = -0.60; Q(2,3) =  0.25;
    Q(3,0) =  0.10; Q(3,1) =  0.15; Q(3,2) =  0.30; Q(3,3) = -0.55;

    Vector4d productivity;
    productivity(0) = 0.00;
    productivity(1) = 2.00;
    productivity(2) = 2.75;
    productivity(3) = 3.50;

    // Entrants
    Vector4d distribution_of_entrants;
    distribution_of_entrants(0) = 0.00;
    distribution_of_entrants(1) = 0.25;
    distribution_of_entrants(2) = 0.50;
    distribution_of_entrants(3) = 0.25;

    // ADD PRINTING OF PARAMETERS
    if (verbose){
        // PRINT PARAMETERS HERE
    }

    // Type Definitions for Clarity
    typedef Triplet<double> T;
    typedef SparseMatrix<double, ColMajor> SM;
    typedef SparseMatrix<double, RowMajor> SMr;

    //// STEP 1: CREATE SPARSE MATRICES & PROFIT VECTORS
    // NOTE: formulation removes for the first time all states such that n_l(omega) + n_h(omega) > n_bar.

    if (verbose){
        cout <<"--------| CREATING SPARSE MATRICES... |-------\n";
    }

    const int num_endog_states = (nbar + 1) * (nbar + 2) / 2;
    const int num_tot_states = nz * num_endog_states;

    // Create matrix for lookup between idx and labor force (not trivial mapping, will try to fix this in the future)
    MatrixX3i table_of_states(num_endog_states, 3);
    MatrixXi table_of_states_inv = MatrixXi::Zero(nbar+1, nbar+1);
    int k = 0;
    for (int j = 0; j <= nbar; j += 1) {
        for (int i = 0; i <= nbar; i += 1) {
            if (i + j <= nbar) {
                table_of_states(k,0) = k;
                table_of_states(k,1) = i;
                table_of_states(k,2) = j;
                table_of_states_inv(i,j) = k;
                k += 1;
            }
        }
    }

    // Define triplets + sparse matrices, and vectors for profits and severance pay
    vector<T> triplet_stay; triplet_stay.reserve(num_tot_states * (2 * nz + 6));
//    vector<T> triplet_layoff_low; triplet_layoff_low.reserve(2 * num_tot_states);
//    vector<T> triplet_layoff_high; triplet_layoff_high.reserve(2 * num_tot_states);
//    vector<T> triplet_hire_low; triplet_hire_low.reserve(2 * num_tot_states);
//    vector<T> triplet_hire_high; triplet_hire_high.reserve(2 * num_tot_states);
//    vector<T> triplet_exit; triplet_exit.reserve(num_tot_states);
//    SM stay(num_tot_states,num_tot_states);
//    SM endog_sep(num_tot_states,num_tot_states);
//    SM layoff_low(num_tot_states,num_tot_states);
//    SM layoff_high(num_tot_states,num_tot_states);
//    SM hire_low(num_tot_states,num_tot_states);
//    SM hire_high(num_tot_states,num_tot_states);
//    SM exit(num_tot_states,num_tot_states);
    VectorXd profits(num_tot_states);
    VectorXd severance_pay_low(num_tot_states);
    VectorXd severance_pay_high(num_tot_states);

    // Populate objects by looping over all states, endogenous and exogenous.
    for (int i = 0; i < num_tot_states; i += 1) {
        int idx_endog = i % num_endog_states;
        int nl = table_of_states(idx_endog, 1);
        int nh = table_of_states(idx_endog, 2);
        int n = nl + nh;
        int z = i / num_endog_states;
        if ( nl != 0) {
            triplet_stay.emplace_back(T(i, i, - (exogenous_separations_rate + quit_rate_low) * nl));
            triplet_stay.emplace_back(T(i, i - 1, (exogenous_separations_rate + quit_rate_low) * nl));
//            triplet_layoff_low[2 * i] = T(i, i, -layoff_rate * nl);
//            triplet_layoff_low[2 * i + 1] = T(i, i - 1, layoff_rate * nl);
        }
        if (nh != 0) {
            triplet_stay.emplace_back(T(i, i, - exogenous_separations_rate * nh));
            triplet_stay.emplace_back(T(i, i - (nbar - nh + 2), exogenous_separations_rate * nh));
//            triplet_layoff_high[2 * i] = T(i, i  , -layoff_rate * nh);
//            triplet_layoff_high[2 * i + 1] = T(i, i - (nbar - nh + 2), layoff_rate * nh);
        }
        if (n != nbar) {
//            triplet_hire_low[2 * i] = T(i, i, - hiring_rate_low * phi(n));
//            triplet_hire_low[2 * i + 1] = T(i, i + 1, hiring_rate_low * phi(n));
//            triplet_hire_high[2 * i] = T(i, i, - hiring_rate_high * phi(n));
//            triplet_hire_high[2 * i + 1] = T(i, i + (nbar - nh + 1), hiring_rate_high * phi(n));
        }
//        triplet_exit[i] = T(i, i, - exit_rate);
        for (int k = 0; k < nz; k += 1) {
            triplet_stay.emplace_back(T(i, i + (k - z) * num_endog_states, Q(z,k)));
        }
        profits(i) = productivity(z) * pow(n, decreasing_returns) - wl * nl - wh * nh - fixed_cost;
        severance_pay_low(i) = -layoff_cost * layoff_rate * wl * nl;
        severance_pay_high(i) = -layoff_cost * layoff_rate * wh * nh;
    }
//    triplet_layoff_low.insert(triplet_layoff_low.end(), triplet_stay.begin(), triplet_stay.end());
//    triplet_layoff_high.insert(triplet_layoff_high.end(), triplet_stay.begin(), triplet_stay.end());
//    triplet_hire_low.insert(triplet_hire_low.end(), triplet_stay.begin(), triplet_stay.end());
//    triplet_hire_high.insert(triplet_hire_high.end(), triplet_stay.begin(), triplet_stay.end());
//    triplet_exit.insert(triplet_exit.end(), triplet_stay.begin(), triplet_stay.end());
//    stay.setFromTriplets(triplet_stay.begin(), triplet_stay.end());
//    endog_sep.setFromTriplets(triplet_endog_sep.begin(),triplet_endog_sep.end());
//    layoff_low.setFromTriplets(triplet_layoff_low.begin(), triplet_layoff_low.end());
//    layoff_high.setFromTriplets(triplet_layoff_high.begin(), triplet_layoff_high.end());
//    hire_low.setFromTriplets(triplet_hire_low.begin(), triplet_hire_low.end());
//    hire_high.setFromTriplets(triplet_hire_high.begin(), triplet_hire_high.end());
//    exit.setFromTriplets(triplet_exit.begin(), triplet_exit.end());

    // Matrices for Hiring are h * matrix!
//    hire_low = hiring_rate_low * hire_low;
//    hire_high = hiring_rate_high * hire_high;
//    endog_sep = quit_rate_low * endog_sep;
//    for (int i = 0; i < triplet_endog_sep.size(); i += 1){
//        triplet_endog_sep[i].value() = quit_rate_low * triplet_endog_sep[i].value();
//    }

    // Time
    auto t1 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_sparse = t1 - t0;
    if (verbose) {
        cout <<"Sparse Matrices Computed in: " << elapsed_sparse.count() << "\n\n";
        cout <<"--------| SOLVING THE HJB EQUATION... |-------\n";
    }

    //// STEP 2: SOLVE HJB EQUATION

    // Set guess of value function or try loading it from file.
    VectorXd value = VectorXd::LinSpaced(num_tot_states, 0, 1);
    if (load_guess_from_file) {
        ifstream inf("value_guess.dat", ios::binary);
        if (!inf) {
            ;
        } else {
            int nbar_file;
            int nz_file;
            inf >> nbar_file;
            inf >> nz_file;
            if (nbar_file == nbar && nz_file == nz){
                int i = 0;
                while (inf && i < num_tot_states) {
                    inf >> value(i);
                    i += 1;
                }
            }
        }
    }

    // Preallocate objects for HJB equation loop
    VectorXd value_next(num_tot_states);
    vector<Policies> policy;
    policy.reserve(num_tot_states);
//    VectorXd profits_vector_for_hjb(num_tot_states);
//    MatrixXi::Index maxRow, maxCol;
//    Matrix<double, Dynamic, Dynamic> alternatives(num_tot_states, 6);

    // Identity Matrix
    SMr speye(num_tot_states,num_tot_states);
    speye.setIdentity();

    // Auxiliary Matrix for Linear System

    vector<T> triplet_sparse_matrix_for_hjb;
//    triplet_sparse_matrix_for_hjb.reserve(num_tot_states * (nz * 2 + 6));

    // Solver Definition
    // BiCGSTAB
    BiCGSTAB<SMr> hjb_solver;
    hjb_solver.setTolerance(hjb_criterion_BiCGSTAB * hjb_criterion);

    // SparseLU
//    SparseLU<SM, COLAMDOrdering<int>> hjb_solver;

    // While Loop
    double max_diff = -1;
    double diff = -1;
    int iteration_hjb = 0;
    double supnorm_hjb = 1e100;
    while (iteration_hjb < hjb_maxit && supnorm_hjb > hjb_criterion) {

//        profits_vector_for_hjb = profits;
        VectorXd profits_vector_for_hjb = profits;

        vector<T> triplet_update_infinitesimal_generator;
        triplet_update_infinitesimal_generator.insert(triplet_update_infinitesimal_generator.end(),
                                                      triplet_stay.begin(),
                                                      triplet_stay.end());

        // Infinitesimal Generator (no entry)
        SMr infinitesimal_generator(num_tot_states,num_tot_states);
//        infinitesimal_generator.reserve(VectorXi::Constant(num_tot_states, 3 + nz));

        SMr A(num_tot_states,num_tot_states);

        // Populate objects by looping over all states, endogenous and exogenous.
        for (int i = 0; i < num_tot_states; i += 1) {
            int idx_endog = i % num_endog_states;
            int nl = table_of_states(idx_endog, 1);
            int nh = table_of_states(idx_endog, 2);
            int n = nl + nh;
            int z = i / num_endog_states;

            ArrayXd alternatives = ArrayXd::Constant(6, -1);

            alternatives(0) = 0;
            if (nl != 0) {
                alternatives(3) = layoff_rate * nl * (value(i - 1) - layoff_cost * wl - value(i));
            }
            if (nh != 0) {
                alternatives(4) = layoff_rate * nh * (value(i - (nbar - nh + 2)) - layoff_cost * wh - value(i));
            }
            if (n != nbar) {
                alternatives(1) = hiring_rate_low * phi(n) * (value(i + 1) - value(i));
                alternatives(2) = hiring_rate_high * phi(n) * (value(i + (nbar - nh + 1)) - value(i));
            }
            alternatives(5) = -exit_rate * value(i);

            ArrayXd::Index maxIdx;
            alternatives.maxCoeff(&maxIdx);
            policy[i] = static_cast<Policies>(maxIdx);

            switch (policy[i])
            {
                case Policies::IDLE:
                    break;

                case Policies::HIRE_LOW_WAGE:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -hiring_rate_low * phi(n)));
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i + 1, hiring_rate_low * phi(n)));
                    break;

                case Policies::HIRE_HIGH_WAGE:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -hiring_rate_high * phi(n)));
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i + (nbar - nh + 1), hiring_rate_high * phi(n)));
                    break;

                case Policies::LAYOFF_LOW_WAGE:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -layoff_rate * nl));
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i - 1, layoff_rate * nl));
                    profits_vector_for_hjb(i) += severance_pay_low(i);
                    break;

                case Policies::LAYOFF_HIGH_WAGE:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -layoff_rate * nh));
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i - (nbar - nh + 2), layoff_rate * nh));
                    profits_vector_for_hjb(i) += severance_pay_high(i);
                    break;

                case Policies::EXIT:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -exit_rate));
                    break;
            }
        }

        VectorXd b(num_tot_states);

        b = profits_vector_for_hjb + (1 / hjb_step_size) * value;
        infinitesimal_generator.setFromTriplets(triplet_update_infinitesimal_generator.begin(),
                                                triplet_update_infinitesimal_generator.end());
        A = (1 / hjb_step_size + discount_rate) * speye - infinitesimal_generator;

//        cout << A;
//        cout <<"\n"<< b;
//        for (int i = 0; i < num_tot_states; i += 1){
//            cout << "\n" << static_cast<int>(policy[i]);
//        }

        // BiCGSTAB
        hjb_solver.compute(A);
        value_next = hjb_solver.solveWithGuess(b,value);

        // SparseLU
//        hjb_solver.analyzePattern(A);
//        hjb_solver.factorize(A);
//        value_next = hjb_solver.solve(b);

        // Compute Supnorm
        max_diff = -1;
        diff = -1;
        for (int i = 0; i < num_tot_states; i += 1){
            diff = abs(value(i) - value_next(i));
            if (diff > max_diff) {
                max_diff = diff;
            }
        }
        supnorm_hjb = max_diff;

        // Update value function
        value = value_next;

        // Update iteration count and display advancement
        iteration_hjb += 1;
        if (verbose) {
            if (iteration_hjb % 5 == 0 || iteration_hjb ==1){
                cout <<"Iteration = "<<iteration_hjb<<", SupNorm = "<<supnorm_hjb<<"\n";
            }
        }

        // Save vector of triplets for fast creation of final sparse matrix
        if (supnorm_hjb < hjb_criterion) {
            triplet_sparse_matrix_for_hjb.insert(triplet_sparse_matrix_for_hjb.end(),
                                                 triplet_update_infinitesimal_generator.begin(),
                                                 triplet_update_infinitesimal_generator.end());
        }
    }

    // Add entrants to the Sparse Infinitesimal Generator
    for (int i = 0; i < num_tot_states; i += 1) {
        switch (policy[i])
        {
            case Policies::IDLE:break;
            case Policies::HIRE_LOW_WAGE:break;
            case Policies::HIRE_HIGH_WAGE:break;
            case Policies::LAYOFF_LOW_WAGE:break;
            case Policies::LAYOFF_HIGH_WAGE:break;
            case Policies::EXIT:
                for (int k = 0; k < nz; k += 1) {
                    triplet_sparse_matrix_for_hjb.emplace_back(T(i, num_endog_states * k, exit_rate * distribution_of_entrants(k)));
                }
                break;
        }
    }
//    sparse_matrix_for_hjb.setZero()
    SMr sparse_matrix_for_hjb(num_tot_states,num_tot_states);
    sparse_matrix_for_hjb.setFromTriplets(triplet_sparse_matrix_for_hjb.begin(), triplet_sparse_matrix_for_hjb.end());

    // Save Value Function to File
    if (save_guess_to_file) {
        ofstream outf("value_guess.dat", ios::binary);
        if (!outf) {
            ;
        } else {
            std::streamsize ss = std::cout.precision();
            outf.precision(16);
            outf << nbar << endl;
            outf << nz << endl;
            for(int i =0; i < num_tot_states; i += 1) {
                outf << value(i) << endl;
            }
            outf.precision(ss);
        }
    }

    // Time
    auto t2 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_hjb = t2 - t1;
    if (verbose) {
        if (iteration_hjb % 5 != 0) {
            cout << "Iteration = " << iteration_hjb << ", SupNorm = " << supnorm_hjb << "\n";
        }
        cout <<"----------------------------------------------\n";
        cout <<"HJB Equations solved in: " << elapsed_hjb.count() << "\n\n";
        cout <<"--------| SOLVING THE KF EQUATIONS... |-------\n";
    }

    //// STEP 3: SOLVE KF EQUATIONS

    // Solve KF Equations using Sparse CTMC Class (REMEMBER!! A is the infinitesimal generator, SparseCTMC takes care
    // of getting the adjoint).
    SparseCTMC endogenous_ctmc = SparseCTMC(sparse_matrix_for_hjb);
    VectorXd distribution;
    distribution = endogenous_ctmc.getStationaryDistribution(kfe_step_size);
    cout << "Check Sum to 1:" << distribution.sum() << "\n";

    // Polish distribution trimming low values
    double cumsum = 0.0;
    for (int i = 0; i < num_tot_states; i += 1){
        if (abs(distribution(i)) < kfe_criterion) {
            distribution(i) = 0.0;
        }
        cumsum += distribution(i);
    }
    distribution = distribution / cumsum;

    // Time
    auto t3 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_kfe = t3 - t2;
    if (verbose) {
        cout <<"KF Equations solved in: " << elapsed_kfe.count() << "\n\n";
        cout <<"--------|   COMPUTING AGGREGATES...   |-------\n";
    }

    //// STEP 4: COMPUTE AGGREGATES
    //TODO: Add aggregates
    // Aggregates: Demand Intensities and Employment Shares
    double agg_hiring_flow_low_wage = 0;
    double agg_hiring_flow_high_wage = 0;
    double agg_demand_intensity_low_wage = 0;
    double agg_demand_intensity_high_wage = 0;
    double agg_employment_low_wage = 0;
    double agg_employment_high_wage = 0;

    // Statistics by Firm Size
    VectorXd firm_size_distribution = VectorXd::Zero(nbar + 1);
    VectorXd mean_wage_by_firm_size_temp = VectorXd::Zero(nbar + 1);
    VectorXd layoffs_low_wage_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd layoffs_high_wage_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd layoffs_total_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd mass_layoffs_low_wage_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd mass_layoffs_high_wage_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd mass_layoffs_total_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd endogenous_separations_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd poaching_by_firm_size = VectorXd::Zero(nbar + 1);

    // Poaching
    VectorXd poaching_by_state(num_tot_states);
    double agg_fraction_poached;
    double agg_flow_of_quits;
    // TODO ADD POACHING

    // NL - NH graph unconditional on productivity level
    VectorXd unconditional_distribution = VectorXd::Zero(num_endog_states);

    for (int i = 0; i < num_tot_states; i += 1){
        int idx_endog = i % num_endog_states;
        int nl = table_of_states(idx_endog, 1);
        int nh = table_of_states(idx_endog, 2);
        int n = nl + nh;
        int z = i / num_endog_states;

        agg_employment_low_wage += nl * distribution(i);
        agg_employment_high_wage += nh * distribution(i);

        unconditional_distribution(idx_endog) += distribution(i);

        firm_size_distribution(n) += distribution(i);

        mean_wage_by_firm_size_temp(n) += (wl * nl + wh * nh) * distribution(i) / n;

        endogenous_separations_by_firm_size(n) += quit_rate_low * nl * distribution(i);

        switch (policy[i]){
            case Policies::HIRE_HIGH_WAGE:
                agg_demand_intensity_high_wage += distribution(i) * phi(n) * hiring_rate_high;
                break;
            case Policies::HIRE_LOW_WAGE:
                agg_demand_intensity_low_wage += distribution(i) * phi(n) * hiring_rate_low;
                break;
            case Policies::IDLE:break;
            case Policies::LAYOFF_LOW_WAGE:
                layoffs_low_wage_by_firm_size(n) += distribution(i) * nl * layoff_rate;
                layoffs_total_by_firm_size(n) += distribution(i) * nl * layoff_rate;
                break;
            case Policies::LAYOFF_HIGH_WAGE:
                layoffs_high_wage_by_firm_size(n) += distribution(i) * nh * layoff_rate;
                layoffs_total_by_firm_size(n) += distribution(i) * nh * layoff_rate;
                break;
            case Policies::EXIT:
                mass_layoffs_low_wage_by_firm_size(n) += distribution(i) * nl * exit_rate;
                mass_layoffs_high_wage_by_firm_size(n) += distribution(i) * nh * exit_rate;
                break;
        }

    }
    mass_layoffs_total_by_firm_size = mass_layoffs_low_wage_by_firm_size + mass_layoffs_high_wage_by_firm_size;
    agg_demand_intensity_low_wage = agg_hiring_flow_high_wage / hiring_rate_high;
    agg_demand_intensity_low_wage = agg_hiring_flow_low_wage / hiring_rate_low;

    int max_firm_size = 0;
    for (int i = 0; i <= nbar; i += 1){
        if (firm_size_distribution(i) > kfe_criterion) {
            max_firm_size = i;
        }
    }
    VectorXd mean_wage_by_firm_size(max_firm_size + 1);
    mean_wage_by_firm_size = mean_wage_by_firm_size_temp.head(max_firm_size + 1);
    for (int i = 0; i <= max_firm_size; i += 1){
        mean_wage_by_firm_size(i) = mean_wage_by_firm_size(i) / firm_size_distribution(i);
    }

    // Time
    auto t4 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_agg = t4 - t3;
    if (verbose) {
        cout <<"Aggregates computed in: " << elapsed_agg.count() << "\n\n";
    }

    //// TOTAL TIME ELAPSED
    auto t_end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_tot = t_end - t0;
    if (verbose) {
        cout <<"--------|            DONE!            |-------\n";
        cout << "Total Execution Time = " << elapsed_tot.count() << endl;
        cout <<" \n\n";
    }

    //// DISPLAY RESULTS FOR CLARITY

    //// TABLES
    if (tables) {
        cout << "----------------------------------------------\n";
        cout << "--------|    STATISTICS OF INTEREST   |-------\n";
        cout << "----------------------------------------------\n\n";

        cout << "Aggregate Employment:\n";
        cout << "Low Wage\t" << agg_employment_low_wage << "\n";
        cout << "High Wage\t" << agg_employment_high_wage << "\n";

        cout << "\n\n";
    }

    //// GRAPHS
    if (graphs) {
        cout << "----------------------------------------------\n";
        cout << "--------|            PLOTS            |-------\n";
        cout << "----------------------------------------------\n\n";

        //// POLICY FUNCTIONS
        cout << "--------|       POLICY FUNCTIONS      |-------\n\n";
        cout << "\033[1mLegend\033[0m\n";
        cout << "Color\t\tCode\tPolicy\n";
        cout << "\033[1;34mBlue\t\t2\t\tHire at High Wage\033[0m\n";
        cout << "\033[1;36mCyan\t\t1\t\tHire at Low Wage\033[0m\n";
        cout << "\033[1;32mGreen\t\t0\t\tStay\033[0m\n";
        cout << "\033[1;35mMagenta\t\t3\t\tLay Off at Low Wage\033[0m\n";
        cout << "\033[1;31mRed\t\t\t4\t\tLay Off at High Wage\033[0m\n";
        cout << "\033[1;30mWhite\t\t5\t\tExit\033[0m\n";

        for (int z = 0; z < nz; z += 1){
            cout << "\n\n";
            cout << "Productivity is: " << productivity(z) << "\n";
            for (int row = nbar; row >= 0; row -= 1){
                if (row == nbar) {
                    cout << "NH\t" << row << "\t";
                } else {
                    cout << "\n\t" << row <<"\t";
                }
                for (int col = 0; col <= nbar; col += 1){
                    if (row + col <= nbar) {
                        int index_endog = table_of_states_inv(col,row);
                        int i = z * num_endog_states + index_endog;
                        switch (policy[i]) {
                            case Policies::IDLE:
                                cout << "\033[1;32m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::HIRE_LOW_WAGE:
                                cout << "\033[1;36m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::HIRE_HIGH_WAGE:
                                cout << "\033[1;34m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::LAYOFF_LOW_WAGE:
                                cout << "\033[1;35m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::LAYOFF_HIGH_WAGE:
                                cout << "\033[1;31m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::EXIT:
                                cout << "\033[1;30m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                        }
                    } else if (col < nbar){
                        cout << "\t";
                    }
                }
            }
            cout << "\n\t\t";
            for (int col = 0; col <= nbar; col += 1){
                cout << col << "\t";
            }
            cout << "\n\t\tNL\n";
        }

        //// UNCONDITIONAL DISTRIBUTION
        cout << "\n\n";

        cout << "--------| UNCONDITIONAL DISTRIBUTION  |-------\n\n";
        double graph_max = unconditional_distribution.maxCoeff();
        VectorXd graph_cutoffs = VectorXd::LinSpaced(7,kfe_criterion,graph_max+1e-6);
        string colors[7] = {"35", "31", "33", "32", "36", "34", "30"};

        cout << "\033[1mLegend\033[0m\n";
        cout << "Color\t\tInterval\n";
        cout << "\033[1;34mBlue\t\t[ " << graph_cutoffs(5) << ", " << graph_cutoffs(6) << " ]\033[0m\n";
        cout << "\033[1;36mCyan\t\t[ " << graph_cutoffs(4) << ", " << graph_cutoffs(5) << " )\033[0m\n";
        cout << "\033[1;32mGreen\t\t[ " << graph_cutoffs(3) << ", " << graph_cutoffs(4) << " )\033[0m\n";
        cout << "\033[1;33mYellow\t\t[ " << graph_cutoffs(2) << ", " << graph_cutoffs(3) << " )\033[0m\n";
        cout << "\033[1;31mRed\t\t\t[ " << graph_cutoffs(1) << ", " << graph_cutoffs(2) << " )\033[0m\n";
        cout << "\033[1;35mMagenta\t\t( " << "0" << ", " << graph_cutoffs(1) << " )\033[0m\n";
        cout << "\033[1;30mWhite\t\tLess than tolerance (essentially zero).\033[0m\n\n";

        for (int row = nbar; row >= 0; row -= 1){
            if (row == nbar) {
                cout << "NH\t" << row << "\t";
            } else {
                cout << "\n\t" << row << "\t";
            }
            for (int col = 0; col <= nbar; col += 1){
                if (row + col <= nbar) {
                    int index_endog = table_of_states_inv(col,row);
                    for (int j = 1; j < 7; j += 1){
                        if (unconditional_distribution(index_endog) >= kfe_criterion && unconditional_distribution(index_endog) < graph_cutoffs(j)){
                            cout << "\033[1;"<< colors[j-1] <<"mx\033[0m\t";
                            break;
                        } else if (unconditional_distribution(index_endog) < kfe_criterion){
                            cout << "\033[1;"<< colors[6] <<"mx\033[0m\t";
                            break;
                        }
                    }
                } else if (col < nbar){
                    cout << "\t";
                }
            }
        }
        cout << "\n\t\t";
        for (int col = 0; col <= nbar; col += 1){
            cout << col << "\t";
        }
        cout << "\n\t\tNL\n";

        //// FIRM SIZE DISTRIBUTION
        cout << "\n\n";
        cout << "--------|    FIRM SIZE DISTRIBUTION   |-------\n\n";

        graph_max = firm_size_distribution.maxCoeff();
        graph_cutoffs = VectorXd::LinSpaced(21,kfe_criterion,graph_max);
        for (int row = 21; row > 0; row -= 1){
            std::streamsize ss = std::cout.precision();
            if (row == 21) {
                cout.precision(2);
                cout << "f(x)" << std::fixed << graph_cutoffs(20) << "\t";
                cout.precision(ss);
            } else {
                cout.precision(2);
                cout << "\n\t" << std::fixed << graph_cutoffs(row-1) <<"\t";
                cout.precision(ss);
            }
            for (int col = 0; col <= nbar; col += 1){
                if (firm_size_distribution(col) < graph_cutoffs(row-1)){
                    cout << "\t";
                } else {
                    cout << "\033[1;34mx\033[0m\t";
                }
            }
        }
        cout << "\n\t\t\t";
        for (int col = 0; col <= nbar; col += 1){
            cout << col << "\t";
        }
        cout << "\n\t\t\tTotal Employees\n";

        //// MEAN WAGE BY FIRM SIZE
        cout << "\n\n";
        cout << "--------|    MEAN WAGE BY FIRM SIZE   |-------\n\n";

        graph_cutoffs = VectorXd::LinSpaced(21,wl,wh);
        for (int row = 21; row > 1; row -= 1){
            std::streamsize ss = std::cout.precision();
            if (row == 21) {
                cout.precision(2);
                cout << "f(x)" << std::fixed << graph_cutoffs(20) << "\t";
                cout.precision(ss);
            } else {
                cout.precision(2);
                cout << "\n\t" << std::fixed << graph_cutoffs(row-1) <<"\t";
                cout.precision(ss);
            }
            for (int col = 1; col <= max_firm_size; col += 1){
                if (mean_wage_by_firm_size(col) > graph_cutoffs(row-1) || mean_wage_by_firm_size(col) <= graph_cutoffs(row-2)){
                    cout << "\t";
                } else {
                    cout << "\033[1;34mx\033[0m\t";
                }
            }
        }
        cout << "\n\t\t\t";
        for (int col = 1; col <= max_firm_size; col += 1){
            cout << col << "\t";
        }
        cout << "\n\t\t\tTotal Employees\n";

        //// TODO ADD OTHER GRAPHS

    }
}


#include "case.hpp"
#include "catch.hpp"
#include "quadeigs.hpp"
#include "timer.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cmath>
#include <fmt/format.h>
#include <iostream>
#include <vector>

using namespace Eigen;
using Td = Triplet<double>;

TEST_CASE("random (4, 4)", "[quadeigs]") {
  int ndim = 4;
  MatrixXd matM = MatrixXd::Random(ndim, ndim);
  MatrixXd matD = MatrixXd::Random(ndim, ndim);
  MatrixXd matK = MatrixXd::Random(ndim, ndim);
  Case1 c1(matM, matD, matK);
  fmt::print("{:>30s}, {:>15s}\n", "alphas", "betas");
  for (int i = 0; i < ndim; ++i) {
    fmt::print("{:14.5f}+{:14.5f}i, {:15.5f}\n", c1.alphas(i).real(),
               c1.alphas(i).imag(), c1.betas(i));
  }

  QuadEigs qe(matM, matD, matK);
  int m = 4;
  VectorXcd ev = qe.eigenvalues(m);
}

TEST_CASE("random (10, 10)", "[quadeigs]") {
  int ndim = 10;
  MatrixXd matM = MatrixXd::Random(ndim, ndim);
  MatrixXd matD = MatrixXd::Random(ndim, ndim);
  MatrixXd matK = MatrixXd::Random(ndim, ndim);
  Case1 c1(matM, matD, matK);
  for (int i = 0; i < ndim; ++i) {
    if (std::abs(c1.betas(i)) > 1.0e-8) {
      fmt::print("{:14.5f}+{:14.5f}i\n", c1.alphas(i).real() / c1.betas(i),
                 c1.alphas(i).imag() / c1.betas(i));
    }
  }
  fmt::print("---------------------------------------------------\n");

  QuadEigs qe(matM, matD, matK);
  int m = 4;
  VectorXcd ev = qe.eigenvalues(m);
}

TEST_CASE("Identity (20, 20)", "[quadeigs]") {
  int ndim = 20;
  MatrixXd matM = MatrixXd::Identity(ndim, ndim);
  MatrixXd matD = MatrixXd::Identity(ndim, ndim);
  MatrixXd matK = MatrixXd::Identity(ndim, ndim) * 0.2;
  for (int i = 1; i < ndim; ++i) {
    matK(i, i - 1) = -0.1;
    matK(i - 1, i) = -0.1;
  }
  std::cout << matK.inverse() << std::endl;
  Case1 c1(matM, matD, matK);
  for (int i = 0; i < ndim; ++i) {
    if (std::abs(c1.betas(i)) > 1.0e-8) {
      fmt::print("{:14.5f}+{:14.5f}i\n", c1.alphas(i).real() / c1.betas(i),
                 c1.alphas(i).imag() / c1.betas(i));
    }
  }
  fmt::print("---------------------------------------------------\n");

  QuadEigs qe(matM, matD, matK);
  int m = 4;
  VectorXcd ev = qe.eigenvalues(m);
}

TEST_CASE("inverse benchmark", "[quadeigs]") {
  int ndim = 20;
  MatrixXd matD = MatrixXd::Identity(ndim, ndim) * 2;
  for (int i = 1; i < ndim; ++i) {
    matD(i, i - 1) = -1.0;
    matD(i - 1, i) = -1.0;
  }
  MatrixXd matId = MatrixXd::Identity(ndim, ndim);
  Timer::begin("inv-dense");
  MatrixXd matDi = matD.ldlt().solve(matId);
  Timer::end("inv-dense");

  SparseMatrix<double> matS(ndim, ndim);
  std::vector<Td> tl;
  for (int i = 0; i < ndim; ++i) {
    tl.push_back(Td(i, i, 2.0));
  }
  for (int i = 1; i < ndim; ++i) {
    tl.push_back(Td(i, i - 1, -1.0));
    tl.push_back(Td(i - 1, i, -1.0));
  }
  matS.setFromTriplets(tl.begin(), tl.end());
  SparseMatrix<double> matI(ndim, ndim);
  matI.setIdentity();

  Timer::begin("inv-sparse");
  SimplicialLDLT<SparseMatrix<double>> solver;
  solver.compute(matS);
  SparseMatrix<double> matSi = solver.solve(matI);
  Timer::end("inv-sparse");

  std::cout << Timer::summery() << std::endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <cstddef>
#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/convex_hull.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/io/wkt/write.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/util/as_range.hpp>

template <typename Geometry>
void test_geometry(std::string const& wkt,
                      std::size_t size_original, std::size_t size_hull,
                      double expected_area)
{

    Geometry geometry;
    bg::read_wkt(wkt, geometry);

    typedef typename bg::point_type<Geometry>::type P;
    typename bg::strategy::side::services::default_strategy
        <
            typename bg::cs_tag<P>::type
        >::type side;

    boost::ignore_unused(side);

    typedef typename bg::range_type<Geometry>::type range_type;
    typedef typename boost::range_const_iterator<range_type>::type iterator;

    range_type const& range = bg::as_range<range_type>(geometry);

    iterator it1 = boost::begin(range);
    iterator it3 = it1++;
    iterator it2 = it1++;

    for (;
        it2 != boost::end(range);
        ++it1, ++it2, ++it3)
    {
       // Last/closing point
       if (it1 == boost::end(range))
       {
           it1 = boost::begin(range) + 1;
       }
       int s = side.apply(*it1, *it2, *it3);

       if (s != 1)
       {
          std::cout << "NOT CONVEX!";
       }
       if (s == 0)
       {
          std::cout << " COLLINEAR!";
       }

       std::cout
           << " " << bg::wkt(*it3)
           << " " << bg::wkt(*it2)
           << " " << bg::wkt(*it1)
           << " " << s
           << std::endl;
    }

    std::cout << bg::area(geometry) << " " << bg::wkt(geometry) << std::endl;
}

template <typename P>
void test_all()
{
    // rectangular, with concavity
    test_geometry<bg::model::polygon<P> >(
        "polygon((1 1, 1 4, 3 4, 3 3, 4 3, 4 4, 5 4, 5 1, 1 1))",
                9, 5, 12.0);

   // concavity at start/closing point
   test_geometry<bg::model::polygon<P> >(
        "polygon((1 1,0 2,3 3,1 0,1 1))", 9, 5, 12.0);

    // from sample polygon, with concavity
    test_geometry<bg::model::polygon<P> >(
        "polygon((2.0 1.3, 2.4 1.7, 2.8 1.8, 3.4 1.2, 3.7 1.6,3.4 2.0, 4.1 3.0"
        ", 5.3 2.6, 5.4 1.2, 4.9 0.8, 2.9 0.7,2.0 1.3))",
                12, 8, 5.245);
}

int test_main(int, char* [])
{
    //test_all<bg::model::d2::point_xy<int> >();
    //test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

#if defined(HAVE_TTMATH)
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/all.h++>
#include <mla/vector/all.h++>
#include <mla/matrix/convert.h++>

#include <mla/operations/level3/syrk.h++>

typedef boost::mpl::list<
	mla::matrix::DenseRowMajor<float>,
	mla::matrix::DenseRowMajor<double>
> matrix_type_list;

BOOST_AUTO_TEST_SUITE(test_operations)

BOOST_AUTO_TEST_CASE_TEMPLATE( matrix_level3_syrk_ones, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	mla::matrix::DenseRowMajor<Scalar> from(matrix_size, 1);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		from.setValue( i, 0, (Scalar)1.0f );
	}

	MatrixType A(matrix_size, 1);
	mla::matrix::convert(from, A);

	MatrixType C(matrix_size, matrix_size);

	Scalar const alfa = 1.0;
	Scalar const beta = 1.0;
	mla::syrk(alfa, A, beta, C);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		for(unsigned int j = 0; j < matrix_size; j++)
		{
			BOOST_CHECK_CLOSE( C.getValue(i,j), (Scalar)1.0f, 0.001f);
		}
	}
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2014-2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include <iostream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_pointlike_linear
#endif

#include <boost/test/included/unit_test.hpp>

#include "test_distance_common.hpp"

typedef bg::model::point<double,2,bg::cs::cartesian>  point_type;
typedef bg::model::multi_point<point_type>            multi_point_type;
typedef bg::model::segment<point_type>                segment_type;
typedef bg::model::linestring<point_type>             linestring_type;
typedef bg::model::multi_linestring<linestring_type>  multi_linestring_type;

namespace services = bg::strategy::distance::services;
typedef bg::default_distance_result<point_type>::type return_type;

typedef bg::strategy::distance::pythagoras<> point_point_strategy;
typedef bg::strategy::distance::projected_point<> point_segment_strategy;

//===========================================================================

template <typename Strategy>
void test_distance_point_segment(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/segment distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<point_type, segment_type> tester;

    tester::apply("point(0 0)", "segment(2 0,3 0)", 2, 4, strategy);
    tester::apply("point(2.5 3)", "segment(2 0,3 0)", 3, 9, strategy);
    tester::apply("point(2 0)", "segment(2 0,3 0)", 0, 0, strategy);
    tester::apply("point(3 0)", "segment(2 0,3 0)", 0, 0, strategy);
    tester::apply("point(2.5 0)", "segment(2 0,3 0)", 0, 0, strategy);

    // distance is a NaN
    tester::apply("POINT(4.297374e+307 8.433875e+307)",
                  "SEGMENT(26 87,13 95)",
                  0, 0, strategy, false);
}

//===========================================================================

template <typename Strategy>
void test_distance_point_linestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/linestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<point_type, linestring_type> tester;

    tester::apply("point(0 0)", "linestring(2 0,3 0)", 2, 4, strategy);
    tester::apply("point(2.5 3)", "linestring(2 0,3 0)", 3, 9, strategy);
    tester::apply("point(2 0)", "linestring(2 0,3 0)", 0, 0, strategy);
    tester::apply("point(3 0)", "linestring(2 0,3 0)", 0, 0, strategy);
    tester::apply("point(2.5 0)", "linestring(2 0,3 0)", 0, 0, strategy);

    // linestring with a single point
    tester::apply("point(0 0)", "linestring(2 0)", 2, 4, strategy);

    // distance is a NaN
    tester::apply("POINT(4.297374e+307 8.433875e+307)",
                  "LINESTRING(26 87,13 95)",
                  0, 0, strategy, false);
}

//===========================================================================

template <typename Strategy>
void test_distance_point_multilinestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/multilinestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            point_type, multi_linestring_type
        > tester;

    tester::apply("point(0 0)",
                  "multilinestring((-5 0,-3 0),(2 0,3 0))",
                  2, 4, strategy);
    tester::apply("point(2.5 3)",
                  "multilinestring((-5 0,-3 0),(2 0,3 0))",
                  3, 9, strategy);
    tester::apply("point(2 0)",
                  "multilinestring((-5 0,-3 0),(2 0,3 0))",
                  0, 0, strategy);
    tester::apply("point(3 0)",
                  "multilinestring((-5 0,-3 0),(2 0,3 0))",
                  0, 0, strategy);
    tester::apply("point(2.5 0)",
                  "multilinestring((-5 0,-3 0),(2 0,3 0))",
                  0, 0, strategy);
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((10 10,10 0),(0.0 -0.0,0.0 -0.0))",
                  0, 0, strategy);
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((10 10,10 0),(1 1,1 1))",
                  sqrt(2.0), 2, strategy);
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((10 10,10 0),(1 1,2 2))",
                  sqrt(2.0), 2, strategy);
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((10 10,10 0),(20 20,20 20))",
                  10, 100, strategy);

    // multilinestrings containing an empty linestring
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((),(10 0),(20 20,20 20))",
                  10, 100, strategy);
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((),(10 0),(),(20 20,20 20))",
                  10, 100, strategy);

    // multilinestrings containing a linestring with a single point
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((10 0),(20 20,20 20))",
                  10, 100, strategy);
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((20 20,20 20),(10 0))",
                  10, 100, strategy);

    // multilinestring with a single-point linestring and empty linestrings
    tester::apply("POINT(0 0)",
                  "MULTILINESTRING((),(20 20,20 20),(),(10 0))",
                  10, 100, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_linestring_multipoint(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "linestring/multipoint distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            linestring_type, multi_point_type
        > tester;

    tester::apply("linestring(2 0,0 2,100 100)",
                  "multipoint(0 0,1 0,0 1,1 1)",
                  0, 0, strategy);
    tester::apply("linestring(4 0,0 4,100 100)",
                  "multipoint(0 0,1 0,0 1,1 1)",
                  sqrt(2.0), 2, strategy);
    tester::apply("linestring(1 1,2 2,100 100)",
                  "multipoint(0 0,1 0,0 1,1 1)",
                  0, 0, strategy);
    tester::apply("linestring(3 3,4 4,100 100)",
                  "multipoint(0 0,1 0,0 1,1 1)",
                  sqrt(8.0), 8, strategy);

    // linestring with a single point
    tester::apply("linestring(1 8)",
                  "multipoint(0 0,3 0,4 -7,10 100)",
                  sqrt(65.0), 65, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multipoint_multilinestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/multilinestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            multi_point_type, multi_linestring_type
        > tester;

    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((2 0,0 2),(2 2,3 3))",
                  0, 0, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((3 0,0 3),(4 4,5 5))",
                  0.5 * sqrt(2.0), 0.5, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((4 4,5 5),(1 1,2 2))",
                  0, 0, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((3 3,4 4),(4 4,5 5))",
                  sqrt(8.0), 8, strategy);

    // multilinestring with empty linestring
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((),(3 3,4 4),(4 4,5 5))",
                  sqrt(8.0), 8, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((3 3,4 4),(),(4 4,5 5))",
                  sqrt(8.0), 8, strategy);

    // multilinestrings with a single-point linestrings
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((3 3),(4 4,5 5))",
                  sqrt(8.0), 8, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((4 4,5 5),(3 3))",
                  sqrt(8.0), 8, strategy);

    // multilinestring with a single-point linestring and empty linestring
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multilinestring((4 4,5 5),(),(3 3))",
                  sqrt(8.0), 8, strategy);

    // 21890717 - assertion failure in distance(Pt, Box)
    {
        multi_point_type mpt;
        bg::read_wkt("multipoint(1 1,1 1,1 1,1 1,1 1,1 1,1 1,1 1,1 1)", mpt);
        multi_linestring_type mls;
        linestring_type ls;
        point_type pt(std::numeric_limits<double>::quiet_NaN(), 1.0);
        ls.push_back(pt);
        ls.push_back(pt);
        mls.push_back(ls);
        bg::distance(mpt, mls);
    }
}

//===========================================================================

template <typename Strategy>
void test_distance_multipoint_segment(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/segment distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<multi_point_type, segment_type> tester;

    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "segment(2 0,0 2)",
                  0, 0, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "segment(4 0,0 4)",
                  sqrt(2.0), 2, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "segment(1 1,2 2)",
                  0, 0, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "segment(3 3,4 4)",
                  sqrt(8.0), 8, strategy);
    tester::apply("multipoint(4 4,5 5,2 2,3 3)",
                  "segment(0 0,1 1)",
                  sqrt(2.0), 2, strategy);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_more_empty_input_pointlike_linear(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "testing on empty inputs... " << std::flush;
#endif
    bg::model::linestring<Point> line_empty;
    bg::model::multi_point<Point> multipoint_empty;
    bg::model::multi_linestring<bg::model::linestring<Point> > multiline_empty;

    Point point = from_wkt<Point>("point(0 0)");
    bg::model::linestring<Point> line =
        from_wkt<bg::model::linestring<Point> >("linestring(0 0,1 1)");

    // 1st geometry is empty
    test_empty_input(multipoint_empty, line, strategy);

    // 2nd geometry is empty
    test_empty_input(point, line_empty, strategy);
    test_empty_input(point, multiline_empty, strategy);

    // both geometries are empty
    test_empty_input(multipoint_empty, line_empty, strategy);
    test_empty_input(multipoint_empty, multiline_empty, strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << "done!" << std::endl;
#endif
}

//===========================================================================
//===========================================================================
//===========================================================================

BOOST_AUTO_TEST_CASE( test_all_point_segment )
{
    test_distance_point_segment(point_point_strategy()); // back-compatibility
    test_distance_point_segment(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_point_linestring )
{
    test_distance_point_linestring(point_point_strategy()); // back-compatibility
    test_distance_point_linestring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_point_multilinestring )
{
    test_distance_point_multilinestring(point_point_strategy()); // back-compatibility
    test_distance_point_multilinestring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_linestring_multipoint )
{
    test_distance_linestring_multipoint(point_point_strategy()); // back-compatibility
    test_distance_linestring_multipoint(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_multipoint_multilinestring )
{
    test_distance_multipoint_multilinestring(point_point_strategy()); // back-compatibility
    test_distance_multipoint_multilinestring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_multipoint_segment )
{
    test_distance_multipoint_segment(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_empty_input_pointlike_linear )
{
    test_more_empty_input_pointlike_linear
        <
            point_type
        >(point_segment_strategy());
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ASINH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ASINH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic sine argument: \f$\log(x+\sqrt{x^2+1})\f$.

    @par Header <boost/simd/function/asinh.hpp>

    @see log, sqrt, sinh, inc

    @par Example:

      @snippet asinh.cpp asinh

    @par Possible output:

      @snippet asinh.txt asinh

  **/
  IEEEValue asinh(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/asinh.hpp>
#include <boost/simd/function/simd/asinh.hpp>

#endif


/**
 * \file PedalToneStackFilter.cpp
 */

#include <boost/math/tools/polynomial.hpp>

#include "PedalToneStackFilter.h"
#include "IIRFilter.h"

namespace ATK
{
  template<typename DataType>
  SD1ToneCoefficients<DataType>::SD1ToneCoefficients(int nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels), R1(10e3), R2(22e3), R3(470), R4(10e3),
    C1(static_cast<DataType>(0.018e-6)), C2(static_cast<DataType>(0.027e-6)), C3(static_cast<DataType>(0.01e-6)), alpha(1)
  {
  }

  template<typename DataType>
  void SD1ToneCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    DataType tempm[2] = {static_cast<DataType>(-2 * input_sampling_rate), static_cast<DataType>(2 * input_sampling_rate)};
    DataType tempp[2] = {static_cast<DataType>(1), static_cast<DataType>(1)};
    boost::math::tools::polynomial<DataType> poly1(tempm, 1);
    boost::math::tools::polynomial<DataType> poly2(tempp, 1);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    b += poly2 * poly2;
    b += poly2 * poly1 * (C2*R3+R4*C3+alpha*(1-alpha)*R2*C2+alpha*C2*R4);
    b += poly1 * poly1 * (C3*R4*(R3*C2+alpha*(1-alpha)*R2*C2));

    a += poly2 * poly2;
    a += poly2 * poly1 * (C2*R3+R1*C1+alpha*(1-alpha)*R2*C2+(1-alpha)*C2*R1);
    a += poly1 * poly1 * (C1*R1*(R3*C2+alpha*(1-alpha)*R2*C2));

    for(int i = 0; i < in_order + 1; ++i)
    {
      coefficients_in[i] = b[i] / a[out_order];
    }
    for(int i = 0; i < out_order; ++i)
    {
      coefficients_out[i] = -a[i] / a[out_order];
    }
  }

  template<typename DataType_>
  void SD1ToneCoefficients<DataType_>::set_tone(DataType_ alpha)
  {
    if(alpha < 0 || alpha > 1)
    {
      throw std::out_of_range("Tone is outside the interval [0,1]");
    }
    this->alpha = alpha;

    setup();
  }

  template<typename DataType_>
  DataType_ SD1ToneCoefficients<DataType_>::get_tone() const
  {
    return alpha;
  }

  template<typename DataType>
  TS9ToneCoefficients<DataType>::TS9ToneCoefficients(int nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels), R1(1e3), R2(10e3), R3(1e3), R4(220), P(22e3),
  C1(static_cast<DataType>(0.022e-6)), C2(static_cast<DataType>(0.022e-6)), alpha(1)
  {
  }

  template<typename DataType>
  void TS9ToneCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    DataType tempm[2] = {static_cast<DataType>(-2 * input_sampling_rate), static_cast<DataType>(2 * input_sampling_rate)};
    DataType tempp[2] = {static_cast<DataType>(1), static_cast<DataType>(1)};
    boost::math::tools::polynomial<DataType> poly1(tempm, 1);
    boost::math::tools::polynomial<DataType> poly2(tempp, 1);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    b += R2 * poly2 * poly2;
    b += poly2 * poly1 * (alpha * C2 * R2 * R3 + alpha * (1-alpha) * C2 * P * R2 + R2 * R4 * C2);

    a += (R2 + R1) * poly2 * poly2;
    a += poly2 * poly1 * ((1-alpha) * C2 * (alpha * P * R2 + R1 * alpha * P + R1 * R2) + R4 * C2 * (R2 + R1) + R1 * C1 * R2);
    a += poly1 * poly1 * (C2 * R4 * C1 * R2 * R1 + (1-alpha) * C2 * R1 * P * C1 * R2);

    for(int i = 0; i < in_order + 1; ++i)
    {
      coefficients_in[i] = b[i] / a[out_order];
    }
    for(int i = 0; i < out_order; ++i)
    {
      coefficients_out[i] = -a[i] / a[out_order];
    }
  }

  template<typename DataType_>
  void TS9ToneCoefficients<DataType_>::set_tone(DataType_ alpha)
  {
    if(alpha < 0 || alpha > 1)
    {
      throw std::out_of_range("Tone is outside the interval [0,1]");
    }
    this->alpha = alpha;

    setup();
  }

  template<typename DataType_>
  DataType_ TS9ToneCoefficients<DataType_>::get_tone() const
  {
    return alpha;
  }

  template class SD1ToneCoefficients<float>;
  template class SD1ToneCoefficients<double>;

  template class IIRFilter<SD1ToneCoefficients<float> >;
  template class IIRFilter<SD1ToneCoefficients<double> >;

  template class TS9ToneCoefficients<float>;
  template class TS9ToneCoefficients<double>;

  template class IIRFilter<TS9ToneCoefficients<float> >;
  template class IIRFilter<TS9ToneCoefficients<double> >;
}


/*
 * MIT License
 *
 * Copyright (c) 2019 Camille Schreck
 *
 * 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.
 *
 * definition.hpp
 */

#ifndef DEFINITIONS_HPP
#define DEFINITIONS_HPP

#include <Eigen/Core>

using namespace Eigen;

//#define DOUBLE_PRECISION

#ifdef DOUBLE_PRECISION

#define FLOAT double
#define VEC2 Vector2d
#define VEC3 Vector3d
#define VEC4 Vector4d
#define VECX VectorXd
#define MAT2 Matrix2d
#define MAT3 Matrix3d
#define MAT4 Matrix4d
#define MATX MatrixXd
#define ANGLE_AXIS AngleAxisd
#define QUATERNION Quaterniond
#define COMPLEX std::complex<double>
#define VEC2C Vector2cd
#define VECXC VectorXcd
#define MATXC MatrixXcd

#else

#define FLOAT float
#define VEC2 Vector2f
#define VEC3 Vector3f
#define VEC4 Vector4f
#define VECX VectorXf
#define MAT2 Matrix2f
#define MAT3 Matrix3f
#define MAT4 Matrix4f
#define MATX MatrixXf
#define ANGLE_AXIS AngleAxisf
#define QUATERNION Quaternionf
#define COMPLEX std::complex<float>
#define VEC2C Vector2cf
#define VECXC VectorXcf
#define MATXC MatrixXcf

#endif

inline FLOAT damping(FLOAT d_coef, FLOAT x, FLOAT k) {
  return exp(-d_coef*k*k*x);
}

inline COMPLEX fund_solution(FLOAT x) {
  return COMPLEX(0, -1)/(FLOAT)4.0*sqrtf(2.0f/(M_PI*x))*exp(COMPLEX(0, 1)*(x - (FLOAT)M_PI/4.0f));
}

inline FLOAT omega(FLOAT k) {
  return sqrtf(9.81*k + 0.074/1000*pow(k, 3));
}

inline FLOAT velocity(FLOAT k) {
  return 0.5*omega(k)/k;
}
inline FLOAT velocity(FLOAT k, FLOAT omega) {
  return 0.5*omega/k;
}

// linear interpolation 1 if x = p1, 0 if x = p2
inline FLOAT interpolation(float x, float p1, float p2) {
  float d = p2 - p1;
  float dx = x - p1;
  return 1-dx/d;
}

#endif


#pragma once

#include <stdexcept>
#include <vector>

#include <Eigen/Core>

#include "LineSegment/linesegment.hh"

namespace bold
{
  template <typename T,int dim>
  class Bounds
  {
  public:
    typedef Eigen::Matrix<T,dim,1> Point;

    Bounds(Point min, Point max)
    : d_min(min),
      d_max(max)
    {
      if ((max - min).minCoeff() < 0)
        throw std::runtime_error("Max must be greater than min.");
    }

    bool operator==(Bounds<T,dim> const& other) const
    {
      return d_min == other.d_min && d_max == other.d_max;
    }

    friend std::ostream& operator<<(std::ostream& stream, Bounds<T,dim> const& line)
    {
      return stream << "Bounds (min=" << line.d_min.transpose() << " max=" << line.d_max.transpose() << ")";
    }

    Point min() const { return d_min; }
    Point max() const { return d_max; }

    Point mid() const
    {
      Eigen::Matrix<T,dim,1> mid;
      for (int i = 0; i < dim; i++)
        mid[i] = (d_min[i] + d_max[i]) / 2;
      return mid;
    }

    bool contains(Point const& v) const
    {
      return (v - d_min).minCoeff() >= 0 && (d_max - v).minCoeff() >= 0;
    }

    bool overlaps(Bounds<T,dim> const& other) const
    {
      for (int i = 0; i < dim; i++)
      {
        if (d_max[i] <= other.d_min[i] ||
            d_min[i] >= other.d_max[i])
          return false;
      }

      return true;
    }

    /** True if the any dimension of this bounding box are zero. */
    bool isEmpty() const
    {
      return (d_max - d_min).cwiseAbs().minCoeff() == 0;
    }

  protected:
    Point d_min;
    Point d_max;
  };

  template<typename T>
  class Bounds2 : public Bounds<T,2>
  {
  public:
    typedef Eigen::Matrix<T,2,1> Point;
    typedef LineSegment<T,2> LineSegmentType;

    static Bounds2<T> merge(Bounds2<T> const& a, Bounds2<T> const& b)
    {
      return Bounds2<T>(
        std::min(a.min().x(), b.min().x()),
        std::min(a.min().y(), b.min().y()),
        std::max(a.max().x(), b.max().x()),
        std::max(a.max().y(), b.max().y()));
    }

    Bounds2(T minX, T minY, T maxX, T maxY)
      : Bounds<T,2>::Bounds(Point(minX, minY), Point(maxX, maxY))
    {}

    Bounds2(Point minPoint, Point maxPoint)
      : Bounds<T,2>::Bounds(minPoint, maxPoint)
    {}

    T minDimension() const
    {
      return std::min(width(), height());
    }

    T maxDimension() const
    {
      return std::max(width(), height());
    }

    T width() const
    {
      return this->d_max.x() - this->d_min.x();
    }

    T height() const
    {
      return this->d_max.y() - this->d_min.y();
    }

    /** Returns corners in clockwise order, starting at 'min'. */
    std::vector<Point> getCorners() const
    {
      std::vector<Point> corners = {
        this->d_min, Point(this->d_min.x(), this->d_max.y()),
        this->d_max, Point(this->d_max.x(), this->d_min.y())
      };

      return corners;
    }

    std::vector<LineSegmentType, Eigen::aligned_allocator<LineSegmentType>> getEdges() const
    {
      auto corners = getCorners();
      std::vector<LineSegmentType, Eigen::aligned_allocator<LineSegmentType>> edges;
      for (unsigned i = 0, lastIndex = 3; i < 4; lastIndex = i++)
      {
        if (corners[lastIndex] != corners[i])
          edges.emplace_back(corners[lastIndex], corners[i]);
      }

      return edges;
    }
  };

  typedef Bounds2<int> Bounds2i;
  typedef Bounds2<double> Bounds2d;
  typedef Bounds<double,3> Bounds3d;
}