using System;
using System.Windows.Forms;

namespace aiNnFingers
{
	public class SOM
	{
		public SOM()
		{

		}

		public RichTextBox richTextBox;

		//#include <stdlib.h>
		//#include <stdio.h>
		//#include <math.h>


		//typedef int           BOOL;
		//typedef int           INT;
		//typedef double        REAL;

		//#define FALSE         0
		//#define TRUE          1
		//#define NOT           !
		//#define AND           &&
		//#define OR            ||

		//#define MIN_REAL      -HUGE_VAL
		const double MIN_REAL = double.MinValue;
		//#define MAX_REAL      +HUGE_VAL
		const double MAX_REAL = double.MaxValue;
		//#define MIN(x,y)      ((x)<(y) ? (x) : (y))
		//#define MAX(x,y)      ((x)>(y) ? (x) : (y))

		//#define PI            (2*asin(1))
		//#define sqr(x)        ((x)*(x))

		public double SQR(double x)
		{
			return x * x;
		}
		public double SQR(int x)
		{
			return (double) x * x;
		}


		public struct LAYER
		{                     /* A LAYER OF A NET:                     */
			public double           Units;         /* - number of units in this layer       */
			public double[]         Output;        /* - output of ith unit                  */
			public double[][]        Weight;        /* - connection weights to ith unit      */
			public double[]         StepSize;      /* - size of search steps of ith unit    */
			public double[]         dScoreMean;    /* - mean score delta of ith unit        */
		} 

		public struct NET
		{                     /* A NET:                                */
			public LAYER        InputLayer;    /* - input layer                         */
			public LAYER        KohonenLayer;  /* - Kohonen layer                       */
			public LAYER        OutputLayer;   /* - output layer                        */
			public double           Winner;        /* - last winner in Kohonen layer        */
			public double          Alpha;         /* - learning rate for Kohonen layer     */
			public double          Alpha_;        /* - learning rate for output layer      */
			public double          Alpha__;       /* - learning rate for step sizes        */
			public double          Gamma;         /* - smoothing factor for score deltas   */
			public double          Sigma;         /* - parameter for width of neighborhood */
		} 


		/******************************************************************************
				R A N D O M S   D R A W N   F R O M   D I S T R I B U T I O N S
		 ******************************************************************************/


		Random r;
		const int RAND_MAX = Int32.MaxValue;
		void InitializeRandoms()
		{
			//srand(4711);
			r = new Random(4715);
		} 


		double RandomEqualREAL(double Low, double High)
		{
			//return ((REAL) rand() / RAND_MAX) * (High-Low) + Low;
			double retVal = ((double) r.Next() / (double) RAND_MAX) * (High-Low) + Low; //268 times
			return retVal;
		}     


		double RandomNormalREAL(double Mu, double Sigma)
		{
			double x,fx;

			do 
			{
				x = RandomEqualREAL(Mu-3*Sigma, Mu+3*Sigma);
				fx = (1 / (Math.Sqrt(2*Math.PI)*Sigma)) * Math.Exp(-SQR(x-Mu) / (2*SQR(Sigma)));
			} while (fx < RandomEqualREAL(0, 1));
			return x;
		}      


		/******************************************************************************
					   A P P L I C A T I O N - S P E C I F I C   C O D E
		 ******************************************************************************/


		//#define ROWS          25
		const double ROWS = 25;
		//#define COLS          25
		const double COLS = 25;

		//#define N             2
		const double N = 2;
		//#define C             (ROWS * COLS)
		const double C = ROWS * COLS;
		//#define M             1
		const double M = 1;

		//#define TRAIN_STEPS   10000
		const double TRAIN_STEPS = 10000;
		//#define BALANCED      100
		const double BALANCED = 100;

		//FILE*                 f;


		void InitializeApplication(ref NET Net)
		{
			double i;
   
			for (i=0; i<Net.KohonenLayer.Units; i++) 
			{
				Net.KohonenLayer.StepSize[(int)i] = 1;
				Net.KohonenLayer.dScoreMean[(int)i] = 0;
			}
			//f = fopen("SOM.txt", "w");
		}


		void WriteNet(ref NET Net)
		{
			double  r,c;
			double x,y,z;

			richTextBox.AppendText("\n\n\n");
			//fprintf(f, "\n\n\n");
			for (r=0; r<ROWS; r++) 
			{
				for (c=0; c<COLS; c++) 
				{
					int rRowsC = (int)r * (int)ROWS + (int)c;
					x = Net.KohonenLayer.Weight[rRowsC][0];
					y = Net.KohonenLayer.Weight[rRowsC][1];
					z = Net.OutputLayer.Weight[0][rRowsC];
					richTextBox.AppendText("([" + x.ToString("F") + ", " + y.ToString("F") + "]), " + z.ToString("F"));
					//fprintf(f, "([%5.1f°, %5.1f°], %5.1fN)    ", x, y, z);
				}
				richTextBox.AppendText("\n");
				//fprintf(f, "\n");
			}
		}


		void FinalizeApplication(ref NET Net)
		{
			//fclose(f);
		}


		/******************************************************************************
							 S I M U L A T I N G   T H E   P O L E
		 ******************************************************************************/

		/* SIMULATION PARAMETERS FOR THE POLE:   */
		//#define L             1              /* - length of pole [m]                  */
		const double L = 1;
		//#define Mc            1              /* - mass of cart [kg]                   */
		const double Mc = 1;
		//#define Mp            1              /* - mass of pole [kg]                   */
		const double Mp = 1;
		//#define G             9.81           /* - acceleration due to gravity [m/s²]  */
		const double G = 9.81;
		//#define T             0.1            /* - time step [s]                       */
		const double T = 0.1;
		//#define STEPS         10             /* - simulation steps in one time step   */
		const double STEPS = 10;


		public struct POLE
		{                     /* STATE VARIABLES OF A POLE:            */
			public double          x;             /* - position of cart [m]                */
			public double          xDot;          /* - velocity of cart [m/s]              */
			public double          w;             /* - angle of pole [°]                   */
			public double          wDot;          /* - angular velocity of pole [°/s]      */
			public double          F;             /* - force applied to cart               */
		}                              /*   during current time step [N]        */


		void InitializePole(ref POLE Pole)
		{
			do 
			{
				Pole.x    = 0;
				Pole.xDot = 0;
				Pole.w    = RandomEqualREAL(-30, 30);
				Pole.wDot = 0;
				Pole.F    = 0;
			} while (Pole.w == 0);
		}


		void SimulatePole(ref POLE Pole)
		{
			double  s;
			double x, xDot, xDotDot;           
			double w, wDot, wDotDot;                   
			double F;                    

			x    = Pole.x;
			xDot = Pole.xDot;
			w    = (Pole.w    / 180) * Math.PI;
			wDot = (Pole.wDot / 180) * Math.PI;
			F    = Pole.F;
			for (s=0; s<STEPS; s++) 
			{

				wDotDot = (G*Math.Sin(w) + Math.Cos(w) * ((-F - Mp*L*SQR(wDot)*Math.Sin(w)) / (Mc+Mp))) / 
					(L * ((double) 4/3 - (Mp*SQR(Math.Cos(w))) / (Mc+Mp)));

				xDotDot = (F + Mp*L * (SQR(wDot)*Math.Sin(w) - wDotDot*Math.Cos(w))) / (Mc+Mp);

				x    += (T / STEPS) * xDot;
				xDot += (T / STEPS) * xDotDot;
				w    += (T / STEPS) * wDot;
				wDot += (T / STEPS) * wDotDot;
			}
			Pole.x    = x;
			Pole.xDot = xDot;
			Pole.w    = (w    / Math.PI) * 180;
			Pole.wDot = (wDot / Math.PI) * 180;
		}


		bool PoleStillBalanced(ref POLE Pole)
		{
			return (Pole.w >= -60) && (Pole.w <= 60);
		}


		double ScoreOfPole(ref POLE Pole)
		{
			double retVal = -SQR(Pole.w);
			return retVal;
		}


		/******************************************************************************
								  I N I T I A L I Z A T I O N
		 ******************************************************************************/


		void GenerateNetwork(ref NET Net)
		{
			double i;

			Net.InputLayer   = new LAYER(); //(LAYER*) malloc(sizeof(LAYER));
			Net.KohonenLayer = new LAYER(); //(LAYER*) malloc(sizeof(LAYER));
			Net.OutputLayer  = new LAYER(); //(LAYER*) malloc(sizeof(LAYER));

			Net.InputLayer.Units        = N;
			Net.InputLayer.Output       = new double[(int)N]; //(double*)  calloc(N, sizeof(double));
      
			Net.KohonenLayer.Units      = C;
			Net.KohonenLayer.Output     = new double[(int)C]; //(double*)  calloc(C, sizeof(double));
			Net.KohonenLayer.Weight     = new double[(int)C][]; //(double**) calloc(C, sizeof(double*));
			Net.KohonenLayer.StepSize   = new double[(int)C]; //(double*)  calloc(C, sizeof(double));
			Net.KohonenLayer.dScoreMean = new double[(int)C]; //(double*)  calloc(C, sizeof(double));
      
			Net.OutputLayer.Units       = M;
			Net.OutputLayer.Output      = new double[(int)M]; //(double*)  calloc(M, sizeof(double));
			Net.OutputLayer.Weight      = new double[(int)M][]; //(double**) calloc(M, sizeof(double*));
      
			for (i=0; i<C; i++) 
			{
				Net.KohonenLayer.Weight[(int)i] = new double[(int)N]; //(double*) calloc(N, sizeof(double));
			}
			for (i=0; i<M; i++) 
			{
				Net.OutputLayer.Weight[(int)i] = new double[(int)C]; //(double*) calloc(C, sizeof(double));
			}
		}


		void RandomWeights(ref NET Net)
		{
			double i,j;
   
			for (i=0; i<Net.KohonenLayer.Units; i++) 
			{
				for (j=0; j<Net.InputLayer.Units; j++) 
				{
					Net.KohonenLayer.Weight[(int)i][(int)j] = RandomEqualREAL(-30, 30);
				}
			}
			for (i=0; i<Net.OutputLayer.Units; i++) 
			{
				for (j=0; j<Net.KohonenLayer.Units; j++) 
				{
					Net.OutputLayer.Weight[(int)i][(int)j] = 0;
				}
			}
		}


		void SetInput(ref NET Net, double[] Input)
		{
			double i;
   
			for (i=0; i<Net.InputLayer.Units; i++) 
			{
				Net.InputLayer.Output[(int)i] = Input[(int)i];
			}
		}


		void GetOutput(ref NET Net, double[] Output)
		{
			double i;
   
			for (i=0; i<Net.OutputLayer.Units; i++) 
			{
				Output[(int)i] = Net.OutputLayer.Output[(int)i];
			}
		}


		/******************************************************************************
							 P R O P A G A T I N G   S I G N A L S
		 ******************************************************************************/


		void PropagateToKohonen(ref NET Net)
		{
			double  i,j;
			double Out, Weight, Sum, MinOut;

			for (i=0; i<Net.KohonenLayer.Units; i++) 
			{
				Sum = 0;
				for (j=0; j<Net.InputLayer.Units; j++) 
				{
					Out = Net.InputLayer.Output[(int)j];
					Weight = Net.KohonenLayer.Weight[(int)i][(int)j];
					Sum += SQR(Out - Weight);
				}
				Net.KohonenLayer.Output[(int)i] = Math.Sqrt(Sum);
			}
			MinOut = MAX_REAL;
			for (i=0; i<Net.KohonenLayer.Units; i++) 
			{
				if (Net.KohonenLayer.Output[(int)i] < MinOut)
				{
					Net.Winner = i;
					MinOut = Net.KohonenLayer.Output[(int)Net.Winner];
				}
			}
		}


		void PropagateToOutput(ref NET Net)
		{
			double i;

			for (i=0; i<Net.OutputLayer.Units; i++) 
			{
				Net.OutputLayer.Output[(int)i] = Net.OutputLayer.Weight[(int)i][(int)Net.Winner];
			}
		}


		void PropagateNet(ref NET Net)
		{
			PropagateToKohonen(ref Net);
			PropagateToOutput(ref Net);
		}


		/******************************************************************************
								T R A I N I N G   T H E   N E T
		 ******************************************************************************/


		double Neighborhood(ref NET Net, int i)
		{
			double  iRow, iCol, jRow, jCol; //int
			double Distance;

			iRow = i / COLS; 
			jRow = Net.Winner / COLS;
			iCol = i % COLS; 
			jCol = Net.Winner % COLS;

			Distance = Math.Sqrt(SQR(iRow-jRow) + SQR(iCol-jCol));

			double retVal = Math.Exp(-SQR(Distance) / (2*SQR(Net.Sigma)));
			return retVal;
		}


		void TrainKohonen(ref NET Net, double[] Input)
		{
			double  i,j;
			double Out, Weight, Lambda = 0, StepSize = 0;

			for (i=0; i<Net.KohonenLayer.Units; i++) 
			{
				for (j=0; j<Net.InputLayer.Units; j++) 
				{
					Out = Input[(int)j];
					Weight = Net.KohonenLayer.Weight[(int)i][(int)j];
					Lambda = Neighborhood(ref Net, (int)i);
					Net.KohonenLayer.Weight[(int)i][(int)j] += Net.Alpha * Lambda * (Out - Weight);
				}
				StepSize = Net.KohonenLayer.StepSize[(int)i];
				Net.KohonenLayer.StepSize[(int)i] += Net.Alpha__ * Lambda * -StepSize;
			}
		}


		void TrainOutput(ref NET Net, double[] Output)
		{
			double  i,j;
			double Out, Weight, Lambda;

			for (i=0; i<Net.OutputLayer.Units; i++) 
			{
				for (j=0; j<Net.KohonenLayer.Units; j++) 
				{
					Out = Output[(int)i];
					Weight = Net.OutputLayer.Weight[(int)i][(int)j];
					Lambda = Neighborhood(ref Net, (int)j);
					Net.OutputLayer.Weight[(int)i][(int)j] += Net.Alpha_ * Lambda * (Out - Weight);
				}
			}
		}


		void TrainUnits(ref NET Net, double[] Input, double[] Output)
		{
			TrainKohonen(ref Net, Input);
			TrainOutput(ref Net, Output);
		}


		int iter = 0;
		void TrainNet(ref NET Net)
		{
			double  n,t; //int
			POLE Pole = new POLE();
			double wOld, wNew, ScoreOld, ScoreNew, dScore, dScoreMean, StepSize;
			double [] Input = new double[(int)N];
			double [] Output = new double[(int)M];
			double [] Target = new double[(int)M];

			n = 0;
			while (n<TRAIN_STEPS) 
			{
				t = 0;
				InitializePole(ref Pole);
				richTextBox.AppendText(" Time     Angle     Force\n");
				//fprintf(f, " Time     Angle     Force\n");
				double out1 = t * T;
				double out2 = Pole.w;
				double out3 = Pole.F;
				//richTextBox.AppendText(iter.ToString() + "  " + out1.ToString("F") + "\t" + out2.ToString("F") + "\t" + out3.ToString("F") + "\n");
				//fprintf(f, "%4.1fs    %5.1f°    %5.1fN\n", t * T, Pole.w, Pole.F);
				wOld = Pole.w;
				ScoreOld = ScoreOfPole(ref Pole);
				SimulatePole(ref Pole);
				wNew = Pole.w;
				ScoreNew = ScoreOfPole(ref Pole);
				while (PoleStillBalanced(ref Pole) && (t<BALANCED)) 
				{
					iter = iter + 1;

					n++;
					t++;
					Net.Alpha   = 0.5 * Math.Pow(0.01, (double) n / (double) TRAIN_STEPS);
					Net.Alpha_  = 0.5 * Math.Pow(0.01, (double) n / (double) TRAIN_STEPS);
					Net.Alpha__ = 0.005;
					Net.Gamma   = 0.05;
					Net.Sigma   = 6.0 * Math.Pow(0.2, (double) n / (double) TRAIN_STEPS);
					Input[0] = wOld;
					Input[1] = wNew;
					SetInput(ref Net, Input);
					PropagateNet(ref Net);
					GetOutput(ref Net, Output);
					Pole.F = Output[0];
					StepSize = Net.KohonenLayer.StepSize[(int)Net.Winner];
					Pole.F += StepSize * RandomNormalREAL(0, 10);
					out1 = t * T;
					out2 = Pole.w;
					out3 = Pole.F;
					//richTextBox.AppendText(iter.ToString() + "  " + out1.ToString("F") + "\t" + out2.ToString("F") + "\t" + out3.ToString("F") + "\n");
					//fprintf(f, "%4.1fs    %5.1f°    %5.1fN\n", t * T, Pole.w, Pole.F);
					wOld = Pole.w;
					ScoreOld = ScoreOfPole(ref Pole);
					SimulatePole(ref Pole);
					wNew = Pole.w;
					ScoreNew = ScoreOfPole(ref Pole);
					dScore = ScoreNew - ScoreOld;
					dScoreMean = Net.KohonenLayer.dScoreMean[(int)Net.Winner];
					if (dScore > dScoreMean) 
					{
						Target[0] = Pole.F;
						TrainUnits(ref Net, Input, Target);
					}
					double winnerVal = Net.Gamma * (dScore - dScoreMean);
					Net.KohonenLayer.dScoreMean[(int)Net.Winner] += winnerVal;
				}
				Application.DoEvents();
				if (PoleStillBalanced(ref Pole))
				{
					out1 = t * T;
					richTextBox.AppendText("Pole still balanced after " + out1.ToString() + " ...\n\n");
					//fprintf(f, "Pole still balanced after %0.1fs ...\n\n", t * T);
				}
				else
				{
					out1 = (t+1) * T;
					richTextBox.AppendText("Pole fallen after " + out1.ToString() + " ...\n\n");
					//fprintf(f, "Pole fallen after %0.1fs ...\n\n", (t+1) * T);
				}
			}
		}


		/******************************************************************************
											M A I N
		 ******************************************************************************/


		public void main()
		{
			NET Net = new NET();

			InitializeRandoms();
			GenerateNetwork(ref Net);
			RandomWeights(ref Net);
			InitializeApplication(ref Net);
			TrainNet(ref Net);
			WriteNet(ref Net);
			FinalizeApplication(ref Net);
		}
	}
}