/*
 *  Source machine generated by GadToolsBox V2.0b
 *  which is (c) Copyright 1991-1993 Jaba Development
 *
 *  GUI Designed by : Michael Kaiser (ETG 215)
 *
 *  Compile using:
 *   sc link cartpolegui.c LIB lib:fuzzy.lib lib:sctools.lib lib:scmieee.lib lib:sc.lib
 */

#include <exec/types.h>
#include <exec/memory.h>
#include <intuition/intuition.h>
#include <intuition/classes.h>
#include <intuition/classusr.h>
#include <intuition/imageclass.h>
#include <intuition/gadgetclass.h>
#include <libraries/gadtools.h>
#include <graphics/displayinfo.h>
#include <graphics/gfxbase.h>
#include <clib/exec_protos.h>
#include <clib/intuition_protos.h>
#include <clib/gadtools_protos.h>
#include <clib/graphics_protos.h>
#include <clib/utility_protos.h>

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

#include "inc:fuzzlib.h"

#include "cartpolegui.h"

#define THIRTY_DEGREES (PI/6.0)

/* ---------------------------------------------------------------------------------------------------- */

static char                controllername[512]     = "";
static char                controllerpathname[256] = "";
static char                controllerfilename[256] = "fuzzy.ctrl";
static char                resultname[512]         = "";
static char                resultpathname[256]     = "";
static char                resultfilename[256]     = "pendulum.out";
static struct FL_system   *fuzzysystem             = 0;

/* ---------------------------------------------------------------------------------------------------- */

/* pendulum */

#define GRAVITY 9.8
#define MASSCART 1.0
#define MASSPOLE 0.1
#define TOTAL_MASS (MASSPOLE + MASSCART)
#define LENGTH 0.5		  /* actually half the pole's length */
#define POLEMASS_LENGTH (MASSPOLE * LENGTH)
#define TAU 0.02		  /* seconds between state updates */

static  double               x,x_dot,theta,theta_dot,force;
static  double               ox,ox_dot,otheta,otheta_dot;

static FILE                  *outfile = NULL;

/* gui stuff */

static struct Screen         *Scr = NULL;
static UBYTE                 *PubScreenName = NULL;
static APTR                   VisualInfo = NULL;
static struct Window         *Fuzzy_CartpoleWnd = NULL;
static struct Gadget         *Fuzzy_CartpoleGList = NULL;
static struct Gadget         *Fuzzy_CartpoleGadgets[9];
static UWORD                  Fuzzy_CartpoleLeft = 121;
static UWORD                  Fuzzy_CartpoleTop = 38;
static UWORD                  Fuzzy_CartpoleWidth = 227;
static UWORD                  Fuzzy_CartpoleHeight = 230;
static UBYTE                 *Fuzzy_CartpoleWdt = (UBYTE *)"Work Window";

static struct TextAttr topaz8 = {
	( STRPTR )"topaz.font", 8, 0x00, 0x01 };

static UWORD Fuzzy_CartpoleGTypes[] = {
	TEXT_KIND,
	BUTTON_KIND,
	STRING_KIND,
	STRING_KIND,
	STRING_KIND,
	STRING_KIND,
	BUTTON_KIND,
	TEXT_KIND,
	BUTTON_KIND
};

static struct NewGadget Fuzzy_CartpoleNGad[] = {
	105, 163, 104, 14, NULL, NULL, GD_controllernamegadget, 0, NULL, NULL,
	11, 162, 85, 16, (UBYTE *)"Controller", NULL, GD_controllergadget, PLACETEXT_IN, NULL, NULL,
	20, 120, 71, 14, (UBYTE *)"x ", NULL, GD_xgadget, PLACETEXT_RIGHT, NULL, NULL,
	20, 141, 71, 14, (UBYTE *)"x´ ", NULL, GD_xdotgadget, PLACETEXT_RIGHT, NULL, NULL,
	117, 120, 71, 14, (UBYTE *)"ø ", NULL, GD_thetagadget, PLACETEXT_RIGHT, NULL, NULL,
	118, 141, 71, 14, (UBYTE *)"ø´", NULL, GD_thetadotgadget, PLACETEXT_RIGHT, NULL, NULL,
	60, 205, 98, 16, (UBYTE *)"Start/Stop", NULL, GD_startstopgadget, PLACETEXT_IN, NULL, NULL,
	105, 184, 104, 14, NULL, NULL, GD_resultfilenamegadget, 0, NULL, NULL,
	11, 183, 85, 16, (UBYTE *)"Output to", NULL, GD_resultfilegadget, PLACETEXT_IN, NULL, NULL
};

static ULONG Fuzzy_CartpoleGTags[] = {
	(GTTX_Border), TRUE, (TAG_DONE),
	(TAG_DONE),
	(GTST_String),(ULONG) "0.0", (GTST_MaxChars), 8, (TAG_DONE),
	(GTST_String),(ULONG) "0.0", (GTST_MaxChars), 8, (TAG_DONE),
	(GTST_String),(ULONG) "0.0", (GTST_MaxChars), 8, (TAG_DONE),
	(GTST_String),(ULONG) "0.0", (GTST_MaxChars), 8, (TAG_DONE),
	(TAG_DONE),
	(GTTX_Border), TRUE, (TAG_DONE),
	(TAG_DONE)
};

/* ------------------------------------------------------------------------------------------------ */

int SetupScreen( void )
{
	if ( ! ( Scr = LockPubScreen( PubScreenName )))
		return( 1L );

	if ( ! ( VisualInfo = GetVisualInfo( Scr, TAG_DONE )))
		return( 2L );

	return( 0L );
}

/* ------------------------------------------------------------------------------------------------ */

void CloseDownScreen( void )
{
	if ( VisualInfo ) {
		FreeVisualInfo( VisualInfo );
		VisualInfo = NULL;
	}

	if ( Scr        ) {
		UnlockPubScreen( NULL, Scr );
		Scr = NULL;
	}
}

/* ------------------------------------------------------------------------------------------------ */

void Fuzzy_CartpoleRender( void )
{
	UWORD		offx, offy;

	offx = Fuzzy_CartpoleWnd->BorderLeft;
	offy = Fuzzy_CartpoleWnd->BorderTop;


	DrawBevelBox( Fuzzy_CartpoleWnd->RPort, offx + 18, offy + 16, 187, 97, 
		     GT_VisualInfo, VisualInfo, GTBB_Recessed, TRUE, TAG_DONE );
}

/* ------------------------------------------------------------------------------------------------ */

int OpenFuzzy_CartpoleWindow( void )
{
	struct NewGadget	ng;
	struct Gadget	*g;
	UWORD		lc, tc;
	UWORD		offx = Scr->WBorLeft, offy = Scr->WBorTop + Scr->RastPort.TxHeight + 1;

	if ( ! ( g = CreateContext( &Fuzzy_CartpoleGList )))
		return( 1L );

	for( lc = 0, tc = 0; lc < Fuzzy_Cartpole_CNT; lc++ ) {

		CopyMem((char * )&Fuzzy_CartpoleNGad[ lc ], (char * )&ng, (long)sizeof( struct NewGadget ));

		ng.ng_VisualInfo = VisualInfo;
		ng.ng_TextAttr   = &topaz8;
		ng.ng_LeftEdge  += offx;
		ng.ng_TopEdge   += offy;

		Fuzzy_CartpoleGadgets[ lc ] = g = CreateGadgetA((ULONG)Fuzzy_CartpoleGTypes[ lc ], g, &ng, ( struct TagItem * )&Fuzzy_CartpoleGTags[ tc ] );

		while( Fuzzy_CartpoleGTags[ tc ] ) tc += 2;
		tc++;

		if ( NOT g )
			return( 2L );
	}

	if ( ! ( Fuzzy_CartpoleWnd = OpenWindowTags( NULL,
				WA_Left,	Fuzzy_CartpoleLeft,
				WA_Top,		Fuzzy_CartpoleTop,
				WA_Width,	Fuzzy_CartpoleWidth,
				WA_Height,	Fuzzy_CartpoleHeight + offy,
				WA_IDCMP,	TEXTIDCMP|BUTTONIDCMP|INTEGERIDCMP|IDCMP_CLOSEWINDOW|IDCMP_REFRESHWINDOW,
				WA_Flags,	WFLG_DRAGBAR|WFLG_DEPTHGADGET|WFLG_CLOSEGADGET|WFLG_SMART_REFRESH,
				WA_Gadgets,	Fuzzy_CartpoleGList,
				WA_Title,	Fuzzy_CartpoleWdt,
				WA_ScreenTitle,	"GadToolsBox V2.0b © 1991-1993",
				WA_PubScreen,	Scr,
				TAG_DONE )))
	return( 4L );

	GT_RefreshWindow( Fuzzy_CartpoleWnd, NULL );

	Fuzzy_CartpoleRender();

	return( 0L );
}

void CloseFuzzy_CartpoleWindow( void )
{
	if ( Fuzzy_CartpoleWnd        ) {
		CloseWindow( Fuzzy_CartpoleWnd );
		Fuzzy_CartpoleWnd = NULL;
	}

	if ( Fuzzy_CartpoleGList      ) {
		FreeGadgets( Fuzzy_CartpoleGList );
		Fuzzy_CartpoleGList = NULL;
	}
}

/* ------------------------------------------------------------------------------------------------ */

void ExecPole(double force, double *x, double *x_dot, double *theta, double *theta_dot)

{
    double xacc,thetaacc,costheta,sintheta,temp;

    costheta = cos(*theta);
    sintheta = sin(*theta);

    temp = (force + POLEMASS_LENGTH * *theta_dot * *theta_dot * sintheta)
		         / TOTAL_MASS;

    thetaacc = (GRAVITY * sintheta - costheta* temp)
	       / (LENGTH * (1.3333333 - MASSPOLE * costheta * costheta
                                              / TOTAL_MASS));

    xacc  = temp - POLEMASS_LENGTH * thetaacc* costheta / TOTAL_MASS;

    *x           += TAU * *x_dot;
    *x_dot       += TAU * xacc;
    *theta       += TAU * *theta_dot;
    *theta_dot   += TAU * thetaacc;
}

/* ------------------------------------------------------------------------------------------------ */

static void ShowInfo(struct Screen *S)

{ 
  register int 	 i;
  ULONG		 chipmem,maxmem,fastmem,fullmem,maxchipmem;
  char		 *txt[20];

  /* ----------------------------------------------------------------- */

  chipmem     = AvailMem(MEMF_CHIP);
  maxmem      = AvailMem(MEMF_FAST|MEMF_LARGEST);
  maxchipmem  = AvailMem(MEMF_CHIP|MEMF_LARGEST);
  fastmem     = AvailMem(MEMF_FAST);
  fullmem     = AvailMem(MEMF_PUBLIC);
   
  for (i = 0; i < 20; i++)
      {
	txt[i] = malloc(80);
	sprintf(txt[i]," ");
      }
 
  sprintf(txt[0]," %s",FL_Getinfo());
  sprintf(txt[1]," ");
  sprintf(txt[2]," Fuzzy Development Software for the Amiga");
  sprintf(txt[3]," © 1994 by Michael Kaiser");
  sprintf(txt[4]," ");
  sprintf(txt[5]," Cartpole Balancing Demo based on");
  sprintf(txt[6]," H.R. Berenji & P. Khedkar");
  sprintf(txt[7]," Learning and tuning fuzzy controllers through reinforcements");
  sprintf(txt[8]," IEEE Transactions on Neural Networks, Vol. 3, No. 5, 1992");
  sprintf(txt[9]," ");
  sprintf(txt[10]," Window created using Jan van den Baard's GadToolsBox");
  sprintf(txt[11]," See the documentation for registration details.");

  DispMultipleMessage(S,12,txt);

  for (i = 0; i < 20; i++)
      free(txt[i]);

}

/* ------------------------------------------------------------------------------------------------ */

static void CheckPath(char *path)

{
  int l;

  l = strlen(path);
  if (l <= 0)
    return;

  l --;

  if ((path[l] == '/') || (path[l] == ':'))
    return;

  l++;
  path[l] = '/';
  l++;
  path[l] = 0;
}

/* ------------------------------------------------------------------------------------------------ */

int ExecWindow(int *run)

{
  struct Gadget 		*gadget;
  struct IntuiMessage 	        *imsg;
  struct StringInfo             *sinfo;
  char                           buffer[20];
  int                            change;

  while (imsg = GT_GetIMsg(Fuzzy_CartpoleWnd->UserPort)) {
    switch (imsg->Class) {
    case IDCMP_GADGETUP:
      gadget = (struct Gadget*)imsg->IAddress;
      switch(gadget->GadgetID) {
      case GD_xgadget:
	sinfo = (struct StringInfo*)Fuzzy_CartpoleGadgets[GDX_xgadget]->SpecialInfo;
	x = atof(sinfo->Buffer);
	break;
      case GD_xdotgadget:
	sinfo = (struct StringInfo*)Fuzzy_CartpoleGadgets[GDX_xdotgadget]->SpecialInfo;
	x_dot = atof(sinfo->Buffer);
	break;
      case GD_thetagadget:
	sinfo = (struct StringInfo*)Fuzzy_CartpoleGadgets[GDX_thetagadget]->SpecialInfo;
	theta = atof(sinfo->Buffer);
	break;
      case GD_thetadotgadget:
	sinfo = (struct StringInfo*)Fuzzy_CartpoleGadgets[GDX_thetadotgadget]->SpecialInfo;
	theta_dot = atof(sinfo->Buffer);
	break;
      case GD_controllergadget:
	if (LoadFileRequest(Scr,"Cartpole controller",controllerpathname,controllerfilename,""))
	  {
	    CheckPath(controllerpathname);
	    sprintf(controllername,"%s%s",controllerpathname,controllerfilename);
	  }
	GT_SetGadgetAttrs(Fuzzy_CartpoleGadgets[GDX_controllernamegadget],Fuzzy_CartpoleWnd,0L,
			  GTTX_Text,controllerfilename,TAG_DONE);
	GT_RefreshWindow(Fuzzy_CartpoleWnd,0L);
	break;
      case GD_resultfilegadget:
	if (SaveFileRequest(Scr,"Cartpole result",resultpathname,resultfilename,""))
	  {
	    if (outfile)
	      fclose(outfile);
	    CheckPath(resultpathname);
	    sprintf(resultname,"%s%s",resultpathname,resultfilename);
	    if (outfile = fopen(resultname,"r"))
	      {
		fclose(outfile);
		outfile = 0;
		if (!Decision(Scr,"File exists! Overwrite it ?"))
		  return(1);
	      }
	    if (!(outfile = fopen(resultname,"w")))
	      {
		DispMessage(Scr,"Could not open output file !");
		return(1);
	      }
	    GT_SetGadgetAttrs(Fuzzy_CartpoleGadgets[GDX_resultfilenamegadget],Fuzzy_CartpoleWnd,0L,
			      GTTX_Text,resultfilename,TAG_DONE);
	    GT_RefreshWindow(Fuzzy_CartpoleWnd,0L);
	  }
	break;
      case GD_startstopgadget:
	if (*run)
	  *run = 0;
	else
	  {
	    if (fuzzysystem)
	      FL_Kill_System(fuzzysystem);
	    if (fuzzysystem = (FL_Read_System(controllername)))
	      {
		*run = 1;
		if (outfile)
		  fprintf(outfile,"# Controller: %s \n",controllername);
	      }
	    else
	      {
		*run = 0;
		DispMessage(Scr,"Error - Controllerfile is not valid!");
	      }
	  }
	break;
      }
      break;
    case IDCMP_CLOSEWINDOW:
      return(0);
    default:
      break;
    }
    GT_ReplyIMsg(imsg);
  }

  if (*run)
    {
      change = 0;

      if (ox != x)
	{
	  sprintf(buffer,"%.2lf",x);
	  GT_SetGadgetAttrs(Fuzzy_CartpoleGadgets[GDX_xgadget],Fuzzy_CartpoleWnd,0L,
			    GTST_String,buffer,TAG_DONE);
	  change = 1;
	}
      
      if (ox_dot != x_dot)
	{
	  sprintf(buffer,"%.2lf",x_dot);
	  GT_SetGadgetAttrs(Fuzzy_CartpoleGadgets[GDX_xdotgadget],Fuzzy_CartpoleWnd,0L,
			    GTST_String,buffer,TAG_DONE);
	  change = 1;
	}
      
      if (otheta != theta)
	{
	  sprintf(buffer,"%.2lf",theta);
	  GT_SetGadgetAttrs(Fuzzy_CartpoleGadgets[GDX_thetagadget],Fuzzy_CartpoleWnd,0L,
			    GTST_String,buffer,TAG_DONE);
	  change = 1;
	}
      
      if (otheta_dot != theta_dot)
	{
	  sprintf(buffer,"%.2lf",theta_dot);
	  GT_SetGadgetAttrs(Fuzzy_CartpoleGadgets[GDX_thetadotgadget],Fuzzy_CartpoleWnd,0L,
			    GTST_String,buffer,TAG_DONE);
	  change = 1;
	}

      if (change)
	{
	  GT_RefreshWindow(Fuzzy_CartpoleWnd,0L);
	}
    }

 return(1);
}

/* ------------------------------------------------------------------------------------------------ */

void DrawCart(double x, double theta, int pen)

{
  int    offx,xi2,yi2;
  double x2,y2;

  if (fabs(theta) > THIRTY_DEGREES)
    return;

  offx = Fuzzy_CartpoleWnd->Width/2 + (int)x;
  
  SetAPen(Fuzzy_CartpoleWnd->RPort,pen);
  Move(Fuzzy_CartpoleWnd->RPort,offx-10,115);
  Draw(Fuzzy_CartpoleWnd->RPort,offx-10,110);
  Draw(Fuzzy_CartpoleWnd->RPort,offx+10,110);
  Draw(Fuzzy_CartpoleWnd->RPort,offx+10,115);

  x2 = LENGTH * 100.0 * sin(theta);
  y2 = LENGTH * 100.0 * cos(theta);

  xi2 = (int)x2 + offx;
  yi2 = 110 - (int)y2;
  
  Move(Fuzzy_CartpoleWnd->RPort,offx,110);
  Draw(Fuzzy_CartpoleWnd->RPort,xi2,yi2);
}

/* -------------------------------------- main module --------------------------------------------- */

void main(int argc, char **argv)

{
  int                 num_runs;
  int                 run;
  double              drawx,oldx,oldtheta;

  num_runs = run = 0;
  x = x_dot = theta = theta_dot = force = 0;

  if (SetupScreen() == 0)
    {
      if (OpenFuzzy_CartpoleWindow() == 0)
	{
	  if (FL_Initialize())
	    {
	      ShowInfo(Scr);
	      if (fuzzysystem = FL_System("Pendel"))
		{
		  drawx = oldx = oldtheta = 0;
		  while (ExecWindow(&run)) 
		    {
		      if (run)
			{
			  FL_System_Reset(fuzzysystem);
			  ExecPole(force,&x,&x_dot,&theta,&theta_dot);
			  drawx = 20.0 * x;
			  drawx = max(-50.0,drawx);
			  drawx = min(50.0,drawx);
			  DrawCart(oldx,oldtheta,0);
			  DrawCart(drawx,theta,1);
			  oldx = drawx; oldtheta = theta;
			  FL_Set_Variables(fuzzysystem,"Pos",x,"Posdot",x_dot,"Angle",
					   theta,"Angledot",theta_dot,0,0);
			  FL_System_Run(fuzzysystem);
			  FL_Get_Variable(fuzzysystem,"Control",&force);
			  if (outfile)
			    fprintf(outfile,"%8.3lf %8.3lf %8.3lf %8.3lf %8.3lf\n",
				    force,x,x_dot,theta,theta_dot);
			  if (fabs(theta) > THIRTY_DEGREES)
			    {
			      theta = theta_dot = x = x_dot = 0;
			      DispMessage(Scr,"Controller failed !");
			      if (outfile)
				fprintf(outfile,"# Controller failed!\n");
			      run = 0;
			    }
			}
		    }
		  if (fuzzysystem)
		    FL_Kill_System(fuzzysystem);
		}
	    }
	  CloseFuzzy_CartpoleWindow();
	}
      CloseDownScreen();
    }
  if (outfile)
    fclose(outfile);
}