error C2143: syntax error : missing ';' before '*

// I90ControllerDlg.h : header file
//
//{{AFX_INCLUDES()
#include "stdafx.h"
 
 
 
#include <iostream>
#include <fstream>
#include "wirobotsdk.h"
#include "vitaminctrl.h"
#include "afxwin.h"
 
 
 
#include <math.h>
#include <vector>
//for reading .mat data
 
#include <mat.h>
#pragma comment(lib, "libmat.lib")
#pragma comment(lib,"libmx.lib")
 
//}}AFX_INCLUDES
 
#if !defined(AFX_I90ControllerDLG_H__804EE31C_32BE_4316_8F38_84907F57A737__INCLUDED_)
#define AFX_I90ControllerDLG_H__804EE31C_32BE_4316_8F38_84907F57A737__INCLUDED_
 
#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000
using namespace std;
/////////////////////////////////////////////////////////////////////////////
// CI90ControllerDlg dialog
struct detect
{
	int col[5];
	int row[5];
	//char character[5];
	char character;
};
extern struct Position
{
 double **x;
 double **xt;
};
struct PIC
{
    unsigned int nChannel;
    bool InterLeaved;
    unsigned int Width, Height, Maxval;
    BYTE *img;
};
 
class CI90ControllerDlg : public CDialog
{
	// Construction
public:
	 Position xxt;
	short direction;
	CPoint lastpoint1,lastpoint2,lastpoint11,lastpoint12;
	bool musicInPlay;
	double desiredWheelSpeed1;
	CI90ControllerDlg(CWnd* pParent = NULL);	// standard constructor
	bool draw;
	bool ismoving;
	detect colorposition;
	int variabilax1, variabilay1;
	short m_Encoder11;
	short m_Encoder22;
	//short lastEncoderPulse1[10000];
	short lastEncoderPulse1;
	//short lastEncoderPulse2[10000];
	short lastEncoderPulse2;
	//short currentEncoderPulse1[10000];
	short currentEncoderPulse1;
	//short currentEncoderPulse2[10000];
	short currentEncoderPulse2;
	//short diffEncoder1[10000];
	short diffEncoder1;
	//short diffEncoder2[10000];
	short diffEncoder2;
 
	bool firstTime;
	short lastLeftEncoder;
	short currentLeftEncoder;
	short currentLeftEncoderValue;
	
	short currentRightEncoder;
	short currentRightEncoderValue;
	short lastRightEncoder;
 
	short currentxr;
	short currentyr;
	short different;
	short m_Width,m_Height,m_Channels;
	int color;
	unsigned char *rightpixelvalue;
	unsigned char *leftpixelvalue;
	unsigned char Bvalue;
	unsigned char Gvalue;
	unsigned char Rvalue;
	//define the 6 threshold
	double thresholdblue;
	double thresholdgreen;
	double thresholdred;
	double thresholdpink;
	double thresholdpurple;
	double thresholdorange;
	//define the distance pixel for each landmark
	double bluepixeldistance;
	double greenpixeldistance;
	double redpixeldistance;
	double pinkpixeldistance;
	double purplepixeldistance;
	double orangepixeldistance;
	double beta;
 
	int numberofbluepixel;
	
	bool rightpixel,leftpixel;
	//vector with the  position of the landmark
	 double  *xL;
	double	*yL;
// Dialog Data
	//{{AFX_DATA(CI90ControllerDlg)
	enum { IDD = IDD_I90Controller_DIALOG };
	CSliderCtrl	m_PanSlider;
	CSliderCtrl	m_TiltSlider;
	CButton	m_playMusic;
	short	m_Encoder1;
	short	m_Encoder2;
	short m_IR11;
	int Ypix;
	bool avoiding,onetime,turn,drive,landmark,m_bCameraConnected;
	short	m_IR1;
	short	m_IR2;
	short	m_IR3;
	short	m_IR4;
	short	m_IR5;
	short	m_IR6;
	short	m_IR7;
	short	m_Sonar1;
	short	m_Sonar2;
	short	m_Sonar3;
	CVitaminCtrl	m_vitCtrl;
	short	m_IR8;
	CWiRobotSDK	m_MOTSDK;
	CWiRobotSDK	m_TISDK;
 
	short	m_Encoderspeed1;
	short	m_Encoderspeed2;
	short	m_Encodercurrent1;
	short	m_Encodercurrent2;
	//define the distances between landmark and robot
	short distanceL1;
	short distanceL2;
	short thetaL1;
	short thetaL2;
	short xL1;
	short xL2;
 
	///
	int numberofcolors;
 
	unsigned int colblue,colgreen,colpink,colorange,collila;
	unsigned int rowblue,rowgreen,rowpink,roworange,rowlila;
	
	//short wheelDistanceL[10000];
	short wheelDistanceL;
	short lastwheelDistanceL;
	//short wheelDistanceR[10000];
	short wheelDistanceR;
	short lastwheelDistanceR;
	//short distravelled[10000];
	short distravelled;
	short lastdistravelled;
	short currentdisttravelled;
	//double angletravelled[10000];
	double angletravelled;
	double lastangletravelled;
	double currentangletravelled;
	double distance;
	double angle;
	double 	tempAngle;
	double 	tempAngle1;
	double 	tempAngle2;
	double xr,yr,dx,dy;
	double lastxr,lastyr,lastdx,lastdy;
	bool connectcamera,pinkland;
	char *gridworldFileName ;
	CGridWorldModel *gridworldModel;
 
	//double x[10000],y[10000],dx[10000],dy[10000];
	//}}AFX_DATA
 
	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CI90ControllerDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);	// DDX/DDV support
	//}}AFX_VIRTUAL
 
// Implementation
protected:
	HICON m_hIcon;
 
	// Generated message map functions
	//{{AFX_MSG(CI90ControllerDlg)
	virtual BOOL OnInitDialog();
	
	afx_msg void OnSysCommand(UINT nID, LPARAM lParam);
	afx_msg void CI90ControllerDlg::OnPaint(); 
	afx_msg HCURSOR OnQueryDragIcon();
	afx_msg void OnStandardSensorEventDrrobotsdkcontrolctrl1();
	afx_msg void OnCustomSensorEventDrrobotsdkcontrolctrl1();
	afx_msg void OnMotorSensorEventDrrobotsdkcontrolctrl1();
	afx_msg void OnForward();
	afx_msg void OnBack();
	afx_msg void OnStop();
	afx_msg void OnTurnLeft();
	afx_msg void OnTurnRight();
	virtual void OnCancel();
	afx_msg void OnTimer(UINT nIDEvent);
	afx_msg void OnRecord();
	afx_msg void OnVoiceSegmentEventDrrobotsdkcontrolctrl1();
	afx_msg void OnVoiceSegmentEventDrrobotsdkcontrolctrl2();
	afx_msg void OnConnect();
	afx_msg void OnOnConnectionBrokenVitaminctrl1(long eConnType);
	afx_msg void OnCameraUp();
	afx_msg void OnCameraDown();
	afx_msg void OnCameraLeft();
	afx_msg void OnCameraRight();
	afx_msg void OnCameraReset();
	afx_msg void OnPan();
	afx_msg void OnCameraStop();
	afx_msg void OnCustomdrawPanSpeed(NMHDR* pNMHDR, LRESULT* pResult);
	afx_msg void OnReleasedcapturePanSpeed(NMHDR* pNMHDR, LRESULT* pResult);
	afx_msg void OnReleasedcaptureTiltSpeed(NMHDR* pNMHDR, LRESULT* pResult);
	afx_msg void OnOnConnectionOKVitaminctrl1(long eConnType);
	afx_msg void OnSnapShot();
	afx_msg void OnAVIRec();
	afx_msg void OnAudioRec();
	afx_msg void OnOnNewAudioPieceVitaminctrl1(VARIANT FAR* vPiece);
	afx_msg void OnStandardSensorEventDrrobotsdkcontrolctrl4();
	afx_msg void StandardToMonitor(double x, double y, int& xM, int& yM);
	afx_msg double Angle2PI(double tempAngle);
	afx_msg double AnglePI(double tempAngle);
	afx_msg void colordetect();
	afx_msg void ReadMat();
	detect ImageSegmentation(const unsigned char* pBGR,double* Back_Dist,double* backdoor_Dist,double* fore_dist,double* blue_Dist,double* green_Dist,double* pink_Dist,double* lila_Dist,double* orange_Dist);
	afx_msg int* MaxofVector(int *vect,int size);
	afx_msg void DriveForward(double Dis, short time);
	afx_msg int MaxofVector1(int *vect,int size);
	afx_msg void TurnDegree(double degree, short time);
	afx_msg bool LoadP5Header(ifstream &infile, PIC &pic);
	afx_msg void LoadImage(ifstream &infile, PIC &pic);
	afx_msg void WritePGM(ofstream & outfile, PIC pic);
	afx_msg void Canny_Edge(PIC Pin_t, int Mask[5][5], char *outFileName_Pout);
	//CGridWorld CGridWorld(char* filename);
	//afx_msg void Sobel(CImage* image,CImage *returnImage)
	DECLARE_EVENTSINK_MAP()
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
public:
	
	afx_msg void OnBnClickedMybutton();
	afx_msg void OnStnClickedLala();
	afx_msg void OnEnChangeDistance();
public:
 
	
 
	double alfa1;
	double alfa2;
	int variabilax,variabilay;
	int maximum,indexmax;
	double* blue_Dist ;
	double* green_Dist;
	double* lila_Dist ;
	double* orange_Dist; 
	double* pink_Dist ;
	double* Back_Dist ;
	double* backdoor_Dist;
	double* fore_dist;
 
	double* p_blue_pixel;
	double* p_green_pixel;
	double* p_pink_pixel;
	double* p_orange_pixel;
	double* p_lila_pixel;
	double* p_back_pixel;
	double* p_backdoor_pixel;
	double* p_door_pixel;
	double p_temp,p_temp1,p_temp2,p_temp3,p_temp4,p_temp5;
 
	double variabila;
	char colordetect1;
//	int numberofbluepixel;
	int numberofgreenpixel;
	int numberofpinkpixel;
	int numberoforangepixel;
	int numberoflilapixel;
	int numberofdoorpixel;
 
	int* colorlist;
	
	
	
};
 
//{{AFX_INSERT_LOCATION}}
// Microsoft Visual C++ will insert additional declarations immediately before the previous line.
 
#endif // !defined(AFX_I90ControllerDLG_H__804EE31C_32BE_4316_8F38_84907F57A737__INCLUDED_)
//defined by gabi
                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:
125:
126:
127:
128:
129:
130:
131:
132:
133:
134:
135:
136:
137:
138:
139:
140:
141:
142:
143:
144:
145:
146:
147:
148:
149:
150:
151:
152:
153:
154:
155:
156:
157:
158:
159:
160:
161:
162:
163:
164:
165:
166:
167:
168:
169:
170:
171:
172:
173:
174:
175:
176:
177:
178:
179:
180:
181:
182:
183:
184:
185:
186:
187:
188:
189:
190:
191:
192:
193:
194:
195:
196:
197:
198:
199:
200:
201:
202:
203:
204:
205:
206:
207:
208:
209:
210:
211:
212:
213:
214:
215:
216:
217:
218:
219:
220:
221:
222:
223:
224:
225:
226:
227:
228:
229:
230:
231:
232:
233:
234:
235:
236:
237:
238:
239:
240:
241:
242:
243:
244:
245:
246:
247:
248:
249:
250:
251:
252:
253:
254:
255:
256:
257:
258:
259:
260:
261:
262:
263:
264:
265:
266:
267:
268:
269:
270:
271:
272:
273:
274:
275:
276:
277:
278:
279:
280:
281:
282:
283:
284:
285:
286:
287:
288:
289:
290:
291:
292:
293:
294:
295:
296:
297:
298:
299:
300:
301:
302:
303:
304:
305:
306:
307:
308:
309:
310:
311:
312:
313:
314:
315:

// I90ControllerDlg.cpp : implementation file
//
 
 
 
 
 
 
#include "stdafx.h"
 
#include "cgridworldmodel.cpp"
#include "cgridworldmodel.h"
#include "atlimage.h"
#include "time.h"
#include "windows.h"
#include "gdiplus.h"
#include <iostream>
#include <fstream>
 #include <vector>
//#include "pixmap.h"
//#include "imglib.h"
#include "ekfreal.cpp"
//#include "ukfreal.cpp"
//#include "ekfrealass.cpp"
 
 
 
#include "direct.h"
#include "I90Controller.h"
#include "I90ControllerDlg.h"
 
 
 
using namespace std;
//using namespace Gdiplus;
typedef std::vector< std::vector<int> > Array2DInt;
typedef std::vector< std::vector<float> > Array2DFloat;
template<typename T>
std::vector< std::vector<T> > 
CreateArray2D(int nRows, int nCols)
{
    std::vector< std::vector<T> > ret;
    ret.resize(nRows);
    for( int i = 0 ; i < nRows ; i++ )
    {
        ret[i].resize(nCols);
    }
    return ret;
}
 
// create data structure to hold image
/*struct PIC
{
    unsigned int nChannel;
    bool InterLeaved;
    unsigned int Width, Height, Maxval;
    BYTE *img;
};*/
template <typename T>
 
/*T **AllocateDynamicArray( int nRows, int nCols)
{
 
    T **dynamicArray;
    dynamicArray = new T*[nRows];
    for( int i = 0 ; i < nRows ; i++ )
    dynamicArray[i] = new T [nCols];
    return dynamicArray;
}
 
template <typename T>
 
void FreeDynamicArray(T** dArray)
{
    delete [] *dArray;
    delete [] dArray;
}
 
void FreeDynamicArray(T** dArray, int nRows)
{
	for( int i = 0 ; i < nRows ; i++ )
	{
		delete [] (dArray[i]);
	}
	delete [] dArray;
}
*/
 
 
//Position calculatematrices(bool firstTime,short thetaL1,short thetaL2,short distanceL1,short distanceL2,bool rightpixel,bool leftpixel,double currentxr,double currentyr,double angletravelled,double lastangletravelled,double lastxr,double lastyr,double *xL,double *yL);
//Position calculatematrices(bool firstTime,short thetaL1,short thetaL2,short distanceL1,short distanceL2,bool rightpixel,bool leftpixel,double currentxr,double currentyr,double angletravelled,double *xL,double *yL);
Position calculatematrices(bool firstTime,short thetaL1,short thetaL2,short distanceL1,short distanceL2,bool rightpixel,bool leftpixel,double currentxr,double currentyr,double angletravelled,double lastangletravelled,double lastxr,double lastyr,double *xL,double *yL,int numberofcolors);
 
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
#define wheelR 0.085 //cm
#define NO_CONTROL -32768
#define M_PWM 0
#define M_POSITION 1
#define M_VELOCITY 2
#define cFULL_COUNT 32767
#define cWHOLE_RANGE 1200
#define CIRCLE_CNT 800 //encoder resolution
#define WheelDis 0.32
#define IMAGE_WIDTH 352
#define IMAGE_HEIGHT 240
#define robotwidth 30 //centimeters
int passedtime=0;
#define Nmax 10000;
#define PI 3.14
 
/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About
 
BOOL SaveAudioFile(LPCTSTR FileName) ;
struct WAVEFileHeader{
	short wFormatTag;
	short nChannels;
	long nSamplesPerSec;
	long nAvgBytesPerSec;
	short nBlockAlign;
	short wBitsPerSample;
	short cbSize;
}waveFormatHeader;
 
struct  FileHeader
{
	long lRiff;
	long lFileSize;
	long lWave ;
	long lFormat;
	long lFormatLength;
} audioFileHeader;
 
struct ChunkHeader
{
  long lType;
  long lLen;
}ch;
 
 
 
 
class CAboutDlg : public CDialog
{
public:
	CAboutDlg();
 
// Dialog Data
	//{{AFX_DATA(CAboutDlg)
	enum { IDD = IDD_ABOUTBOX };
	//}}AFX_DATA
 
	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CAboutDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
	//}}AFX_VIRTUAL
 
// Implementation
protected:
	//{{AFX_MSG(CAboutDlg)
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};
 
CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
	//{{AFX_DATA_INIT(CAboutDlg)
	//}}AFX_DATA_INIT
}
 
void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CAboutDlg)
	//}}AFX_DATA_MAP
}
 
BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
	//{{AFX_MSG_MAP(CAboutDlg)
		// No message handlers
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()
 
/////////////////////////////////////////////////////////////////////////////
// CI90ControllerDlg dialog
 
CI90ControllerDlg::CI90ControllerDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CI90ControllerDlg::IDD, pParent)
{
	//colorlist=new int[5];
	lastLeftEncoder =  0;
	lastRightEncoder = 0;
	currentLeftEncoder=0;
	currentRightEncoder=0;
	currentLeftEncoderValue=0;
	currentRightEncoderValue=0;
	m_Encoder1 = 0;
	m_Encoder2 = 0;
	m_Encoder11 = 0;
	m_Encoder22 = 0;
	distanceL1=255;
	distanceL2=255;
	m_IR1 = 0;
	m_IR2 = 0;
	m_IR3 = 0;
	m_IR4 = 0;
	m_IR5 = 0;
	m_IR6 = 0;
	m_IR7 = 0;
	m_Sonar1 = 0;
	m_Sonar2 = 0;
	m_Sonar3 = 0;
	m_IR8 = 0;
	m_Encoderspeed1 = 0;
	m_Encoderspeed2 = 0;
	m_Encodercurrent1 = 0;
	m_Encodercurrent2 = 0;
	
	Ypix=0;
	lastangletravelled=0;
	currentangletravelled=0;
 
	lastpoint1.x=510;
	lastpoint1.y=520;
 
	lastpoint11.x=510;
	lastpoint11.y=520;
	
	lastpoint12.x=510;
	lastpoint12.y=520;
 
	lastpoint2.x=510;
	lastpoint2.y=520;
	
	numberofgreenpixel=0;
	numberoforangepixel=0;
	numberofpinkpixel=0;
	numberofbluepixel=0;
	numberoflilapixel=0;
	numberofdoorpixel=0;
 
	m_Width=IMAGE_WIDTH;  //720  768
	m_Height=IMAGE_HEIGHT; //576  576
	m_Channels=3;
	ismoving=false;
	avoiding = true;
	pinkland = false;
//initialize the true position xt and the position calculated x
	xxt.x=new double *[2];
	xxt.x[0]=new double [1];
	xxt.x[1]=new double [1];
	xxt.x[0][0]=0;
	xxt.x[1][0]=0;
 
	thetaL1=0;
	thetaL2=0;
	variabilax=0;
	variabilay=0;
	variabilax1=0;
	variabilay1=0;
	xxt.xt=new double *[2];
	xxt.xt[0]=new double [1];
	xxt.xt[1]=new double [1];
	xxt.xt[0][0]=0;
	xxt.xt[1][0]=0;
	lastxr=0;
	lastyr=0;
	
	onetime = false;
	turn =false;
	drive = false;
	variabila=0;
	//rightpixelvalue=new unsigned char[3];
	//leftpixelvalue=new unsigned char[3];
	angletravelled=0;
	distravelled=0;
	currentdisttravelled=0;
	lastEncoderPulse1=0;
	lastEncoderPulse2=0;
	//initial position and angle
	angle=0;
	xr=0;
	yr=0;
	dx=0;
	dy=0;
	currentEncoderPulse1=0;
	currentEncoderPulse2=0;
	diffEncoder1=0;
	diffEncoder2=0;
	wheelDistanceL=0;
	wheelDistanceR=0;
	landmark =false;
	maximum=0;
	indexmax=0;
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
	firstTime= true;
	connectcamera=false;
	rightpixel=false;
	leftpixel=false;
	//we need the vector with the position of the 2 landmarks
	xL= new double [6];
	yL= new double [6];
	gridworldFileName = "Gridworld_10x10.txt";
	gridworldModel = new CGridWorldModel(gridworldFileName,10);
//blue landmark position in cm
	xL[0]=70;
	yL[0]=122;
//green landmark
	xL[1]=120;
	yL[1]=182;
//red landmark
	xL[2]=200;
	yL[2]=78;
//pink landmark 290 initial
	xL[3]=200;
	yL[3]=50;
//purple landmark
	xL[4]=254;
	yL[4]=172;
//orange landmark
	xL[5]=200;
	yL[5]=372;
	 beta=0;
	
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}
 
void CI90ControllerDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CI90ControllerDlg)
	DDX_Control(pDX, IDC_PanSpeed, m_PanSlider);
	DDX_Control(pDX, IDC_TiltSpeed, m_TiltSlider);
	DDX_Control(pDX, IDC_DRROBOTSDKCONTROLCTRL1, m_MOTSDK);
	DDX_Text(pDX, IDC_Encoder1, m_Encoder1);
	DDX_Text(pDX, IDC_Encoder2, m_Encoder2);
	DDX_Text(pDX, IDC_EDIT1,variabilax);
	DDX_Text(pDX, IDC_EDIT2,variabilax1);
	DDX_Text(pDX, IDC_EDIT3,dy);
	DDX_Text(pDX, IDC_EDIT4,angletravelled);
	DDX_Text(pDX, IDC_EDIT5,distravelled);
 
	DDX_Text(pDX, IDC_IR1, m_IR1);
	DDX_Text(pDX, IDC_IR2, m_IR2);
	DDX_Text(pDX, IDC_IR3, m_IR3);
	DDX_Text(pDX, IDC_IR4, m_IR4);
	DDX_Text(pDX, IDC_IR5, m_IR5);
	DDX_Text(pDX, IDC_IR6, m_IR6);
	DDX_Text(pDX, IDC_IR7, m_IR7);
	DDX_Text(pDX, IDC_Sonar1, m_Sonar1);
	DDX_Text(pDX, IDC_Sonar2, m_Sonar2);
	DDX_Text(pDX, IDC_Sonar3, m_Sonar3);
	DDX_Control(pDX, IDC_DRROBOTSDKCONTROLCTRL2, m_TISDK);
	DDX_Control(pDX, IDC_VITAMINCTRL1, m_vitCtrl);
 
	//}}AFX_DATA_MAP
}
 
BEGIN_MESSAGE_MAP(CI90ControllerDlg, CDialog)
	//{{AFX_MSG_MAP(CI90ControllerDlg)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDC_Forward, OnForward)
	ON_BN_CLICKED(IDC_Back, OnBack)
	ON_BN_CLICKED(IDC_Stop, OnStop)
	ON_BN_CLICKED(IDC_TurnLeft, OnTurnLeft)
	ON_BN_CLICKED(IDC_TurnRight, OnTurnRight)
	ON_WM_TIMER()
	ON_BN_CLICKED(IDC_Record, OnRecord)
	ON_BN_CLICKED(IDC_Connect, OnConnect)
	ON_BN_CLICKED(IDC_CameraUp, OnCameraUp)
	ON_BN_CLICKED(IDC_CameraDown, OnCameraDown)
	ON_BN_CLICKED(IDC_CameraLeft, OnCameraLeft)
	ON_BN_CLICKED(IDC_CameraRight, OnCameraRight)
	ON_BN_CLICKED(IDC_CameraReset, OnCameraReset)
	ON_BN_CLICKED(IDC_Pan, OnPan)
	ON_BN_CLICKED(IDC_CameraStop, OnCameraStop)
	ON_NOTIFY(NM_CUSTOMDRAW, IDC_PanSpeed, OnCustomdrawPanSpeed)
	ON_NOTIFY(NM_RELEASEDCAPTURE, IDC_PanSpeed, OnReleasedcapturePanSpeed)
	ON_NOTIFY(NM_RELEASEDCAPTURE, IDC_TiltSpeed, OnReleasedcaptureTiltSpeed)
	ON_BN_CLICKED(IDC_SnapShot, OnSnapShot)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()
 
/////////////////////////////////////////////////////////////////////////////
// CI90ControllerDlg message handlers
 
BOOL CI90ControllerDlg::OnInitDialog()
{
	
	CDialog::OnInitDialog();
 
	// Add "About..." menu item to system menu.
 
	// IDM_ABOUTBOX must be in the system command range.
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);
 
	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != NULL)
	{
		CString strAboutMenu;
		strAboutMenu.LoadString(IDS_ABOUTBOX);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}
 
	
	GetDlgItem(IDC_EDIT_SERV_IP)->SetWindowText("192.168.0.99");
	//GetDlgItem(IDC_EDIT_SERV_IP)->SetWindowText("192.168.4.12");
	GetDlgItem(IDC_EDIT_PORT)->SetWindowText("8081");
	GetDlgItem(IDC_EDIT_ROOT_ID)->SetWindowText("root");
	GetDlgItem(IDC_EDIT_ROOT_PWD)->SetWindowText("drrobot");
 
	m_PanSlider.SetRangeMin(-5, false);
	m_PanSlider.SetRangeMax(5, false);
 
	m_TiltSlider.SetRangeMin(-5, false);
	m_TiltSlider.SetRangeMax(5, false);
 
	
	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon
	
	// TODO: Add extra initialization here
	green_Dist = new double[32*32*32];
	lila_Dist = new double[32*32*32];
	orange_Dist = new double[32*32*32];
	pink_Dist = new double[32*32*32];
	Back_Dist = new double[32*32*32];
	blue_Dist = new double[32*32*32];
	//door
	backdoor_Dist = new double [32*32*32];
	fore_dist = new double [32*32*32];
 
 
	//m_iTimerCnt = 0;
	m_MOTSDK.connectRobot ("motcom");
	m_TISDK.connectRobot ("ticom");
	
	GetDlgItem(IDC_CameraUp)->EnableWindow(false);
 
	ReadMat();
	m_bCameraConnected = false;
	musicInPlay = false;
	//SetTimer(1, 20000, NULL);
	SetTimer(1, 10000, NULL);
	firstTime = true;
	
 
	return TRUE;  // return TRUE  unless you set the focus to a control
}
 
void CI90ControllerDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialog::OnSysCommand(nID, lParam);
	}
}
 
// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.
 
void CI90ControllerDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting
		
		SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);
 
		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;
		
	
		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
	CPaintDC dc(this);
	//ofstream outfile_Pout;
	
 
 
	
	
	
	//CPoint p5(577+xL[0]*0.1,467+yL[0]*0.1);
	//dc.MoveTo(lastpoint11);
	//dc.LineTo(p5);
	//lastpoint11=p5;
	
	
/*	CPoint p6(510+xL[0]*0.5,510-yL[0]*0.2);
	//blue landmark
	dc.SetPixel(510+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
	dc.SetPixel(511+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
	dc.SetPixel(509+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
	dc.SetPixel(512+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
	dc.SetPixel(508+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
	dc.SetPixel(510+xL[0]*0.5,521-yL[0]*0.2,RGB(0,0,255));
	dc.SetPixel(510+xL[0]*0.5,522-yL[0]*0.2,RGB(0,0,255));
	dc.SetPixel(510+xL[0]*0.5,518-yL[0]*0.2,RGB(0,0,255));
	dc.SetPixel(510+xL[0]*0.5,519-yL[0]*0.2,RGB(0,0,255));
 
	dc.MoveTo(lastpoint11);
	dc.LineTo(p6);
	lastpoint11=p6;
 
	CPoint p7(510+xL[1]*0.5,520-yL[1]*0.2);
	//green landmark
	dc.SetPixel(510+xL[1]*0.5,520-yL[1]*0.2,RGB(0,238,0));
	dc.SetPixel(511+xL[1]*0.5,520-yL[1]*0.2,RGB(0,238,0));
	dc.SetPixel(509+xL[1]*0.5,520-yL[1]*0.2,RGB(0,238,0));
	dc.SetPixel(512+xL[1]*0.5,520-yL[1]*0.2,RGB(0,238,0));
	dc.SetPixel(508+xL[1]*0.5,520-yL[1]*0.2,RGB(0,238,0));
	dc.SetPixel(510+xL[1]*0.5,521-yL[1]*0.2,RGB(0,238,0));
	dc.SetPixel(510+xL[1]*0.5,522-yL[1]*0.2,RGB(0,238,0));
	dc.SetPixel(510+xL[1]*0.5,518-yL[1]*0.2,RGB(0,238,0));
	dc.SetPixel(510+xL[1]*0.5,519-yL[1]*0.2,RGB(0,238,0));
	
	dc.MoveTo(lastpoint11);
	dc.LineTo(p7);
	lastpoint11=p7;
 
	CPoint p8(510+xL[5]*0.5,520-yL[5]*0.2);
	//orange landmark
	dc.SetPixel(510+xL[5]*0.5,520-yL[5]*0.2,RGB(255,69,0));
	dc.SetPixel(511+xL[5]*0.5,520-yL[5]*0.2,RGB(255,69,0));
	dc.SetPixel(509+xL[5]*0.5,520-yL[5]*0.2,RGB(255,69,0));
	dc.SetPixel(512+xL[5]*0.5,520-yL[5]*0.2,RGB(255,69,0));
	dc.SetPixel(508+xL[5]*0.5,520-yL[5]*0.2,RGB(255,69,0));
	dc.SetPixel(510+xL[5]*0.5,521-yL[5]*0.2,RGB(255,69,0));
	dc.SetPixel(510+xL[5]*0.5,522-yL[5]*0.2,RGB(255,69,0));
	dc.SetPixel(510+xL[5]*0.5,518-yL[5]*0.2,RGB(255,69,0));
	dc.SetPixel(510+xL[5]*0.5,519-yL[5]*0.2,RGB(255,69,0));
	
	dc.MoveTo(lastpoint11);
	dc.LineTo(p8);
	lastpoint11=p8;
 
 
	CPoint p9(510+xL[3]*0.5,520-yL[3]*0.2);
	//pink landmark
	dc.SetPixel(510+xL[3]*0.5,520-yL[3]*0.2,RGB(255,20,147));
	dc.SetPixel(511+xL[3]*0.5,520-yL[3]*0.2,RGB(255,20,147));
	dc.SetPixel(509+xL[3]*0.5,520-yL[3]*0.2,RGB(255,20,147));
	dc.SetPixel(512+xL[3]*0.5,520-yL[3]*0.2,RGB(255,20,147));
	dc.SetPixel(508+xL[3]*0.5,520-yL[3]*0.2,RGB(255,20,147));
	dc.SetPixel(510+xL[3]*0.5,521-yL[3]*0.2,RGB(255,20,147));
	dc.SetPixel(510+xL[3]*0.5,522-yL[3]*0.2,RGB(255,20,147));
	dc.SetPixel(510+xL[3]*0.5,518-yL[3]*0.2,RGB(255,20,147));
	dc.SetPixel(510+xL[3]*0.5,519-yL[3]*0.2,RGB(255,20,147));
	
	dc.MoveTo(lastpoint12);
	dc.LineTo(p9);
	lastpoint12=p9;
 
	CPoint p10(510+xL[4]*0.5,520-yL[4]*0.2);
	//purple landmark
	dc.SetPixel(510+xL[4]*0.5,520-yL[4]*0.2,RGB(128,0,128));
	dc.SetPixel(511+xL[4]*0.5,520-yL[4]*0.2,RGB(128,0,128));
	dc.SetPixel(509+xL[4]*0.5,520-yL[4]*0.2,RGB(128,0,128));
	dc.SetPixel(512+xL[4]*0.5,520-yL[4]*0.2,RGB(128,0,128));
	dc.SetPixel(508+xL[4]*0.5,520-yL[4]*0.2,RGB(128,0,128));
	dc.SetPixel(510+xL[4]*0.5,521-yL[4]*0.2,RGB(128,0,128));
	dc.SetPixel(510+xL[4]*0.5,522-yL[4]*0.2,RGB(128,0,128));
	dc.SetPixel(510+xL[4]*0.5,518-yL[4]*0.2,RGB(128,0,128));
	dc.SetPixel(510+xL[4]*0.5,519-yL[4]*0.2,RGB(128,0,128));
	
	dc.MoveTo(lastpoint12);
	dc.LineTo(p10);
	lastpoint12=p10;
*/
	CDialog::OnPaint();
	}
}
 
// The system calls this to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CI90ControllerDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}
 
BEGIN_EVENTSINK_MAP(CI90ControllerDlg, CDialog)
    //{{AFX_EVENTSINK_MAP(CI90ControllerDlg)
	ON_EVENT(CI90ControllerDlg, IDC_DRROBOTSDKCONTROLCTRL1, 1 /* StandardSensorEvent */, OnStandardSensorEventDrrobotsdkcontrolctrl1, VTS_NONE)
	ON_EVENT(CI90ControllerDlg, IDC_DRROBOTSDKCONTROLCTRL1, 3 /* CustomSensorEvent */, OnCustomSensorEventDrrobotsdkcontrolctrl1, VTS_NONE)
	ON_EVENT(CI90ControllerDlg, IDC_DRROBOTSDKCONTROLCTRL1, 2 /* MotorSensorEvent */, OnMotorSensorEventDrrobotsdkcontrolctrl1, VTS_NONE)
	ON_EVENT(CI90ControllerDlg, IDC_DRROBOTSDKCONTROLCTRL1, 5 /* VoiceSegmentEvent */, OnVoiceSegmentEventDrrobotsdkcontrolctrl1, VTS_NONE)
	ON_EVENT(CI90ControllerDlg, IDC_DRROBOTSDKCONTROLCTRL2, 5 /* VoiceSegmentEvent */, OnVoiceSegmentEventDrrobotsdkcontrolctrl2, VTS_NONE)
	ON_EVENT(CI90ControllerDlg, IDC_VITAMINCTRL1, 2 /* OnConnectionBroken */, OnOnConnectionBrokenVitaminctrl1, VTS_I4)
	ON_EVENT(CI90ControllerDlg, IDC_VITAMINCTRL1, 5 /* OnConnectionOK */, OnOnConnectionOKVitaminctrl1, VTS_I4)
	//}}AFX_EVENTSINK_MAP
END_EVENTSINK_MAP()
 
void CI90ControllerDlg::OnStandardSensorEventDrrobotsdkcontrolctrl1() 
{
	// TODO: Add your control notification handler code here
	m_Sonar1= m_MOTSDK.GetSensorSonar1 ();
	m_Sonar2= m_MOTSDK.GetSensorSonar2 ();
	m_Sonar3= m_MOTSDK.GetSensorSonar3 ();
	
 
	m_IR1 = m_MOTSDK.GetSensorIRRange ();
	
	
 
	UpdateData(false);
	
}
 
void CI90ControllerDlg::OnCustomSensorEventDrrobotsdkcontrolctrl1() 
{
	// TODO: Add your control notification handler code here
	m_IR2 = m_MOTSDK.GetCustomAD3 ();
	m_IR3 = m_MOTSDK.GetCustomAD4 ();
	m_IR4 = m_MOTSDK.GetCustomAD5 ();	
	m_IR5 = m_MOTSDK.GetCustomAD6 ();
	m_IR6 = m_MOTSDK.GetCustomAD7 ();
	m_IR7 = m_MOTSDK.GetCustomAD8 ();
	UpdateData(false);
}
 
void CI90ControllerDlg::OnMotorSensorEventDrrobotsdkcontrolctrl1() 
{
	// TODO: Add your control notification handler code here
	m_Encoder1 = m_MOTSDK.GetEncoderPulse1 ();
	m_Encoder2 = m_MOTSDK.GetEncoderPulse2 ();
	
	
	m_Encoderspeed1 =m_MOTSDK.GetEncoderSpeed1();    //left speed
	m_Encoderspeed2 =m_MOTSDK.GetEncoderSpeed2();    //right speed
 
	m_Encodercurrent1 = (double)m_MOTSDK.GetMotorCurrent1() / 728.0;  //left current
	m_Encodercurrent2 =(double)m_MOTSDK.GetMotorCurrent2() /728.0;    //right current
	
	currentLeftEncoder = m_Encoder1;
	currentRightEncoder = m_Encoder2;
 
	if((m_Encoderspeed1!=0)&&(m_Encoderspeed2!=0))
	{
	
		direction = m_MOTSDK.GetEncoderDir1();
 
		if (firstTime==false)
		{
			currentLeftEncoder=m_Encoder1;
			//currentLeftEncoder=cFULL_COUNT-m_Encoder1;
			//currentLeftEncoderValue=cFULL_COUNT-m_MOTSDK.GetEncoderPulse1 ();
			//currentRightEncoder=cFULL_COUNT-m_Encoder2;
			currentRightEncoder=m_Encoder2;
			//currentRightEncoderValue=cFULL_COUNT-m_MOTSDK.GetEncoderPulse2 ();
			//different = currentLeftEncoderValue - lastLeftEncoder ;
			//currentRightEncoderValue=m_MOTSDK.GetEncoderPulse2 ();
		    
			
		  
			//m_MOTSDK.DcMotorVelocityTimeCtrAll ((short)(leftcmd/10), (short)(rightcmd/10),NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,10000);
		    
			
			//for(int i=0;i<1000;i++)
			//{
 
			diffEncoder1=currentLeftEncoder-lastLeftEncoder;
				//eliminate the encoder rollover
				if(abs(diffEncoder1) >  16384)
				{
					if(currentLeftEncoder<lastLeftEncoder)
					diffEncoder1=cFULL_COUNT-lastLeftEncoder+currentLeftEncoder;
					else
					diffEncoder1=currentLeftEncoder-lastLeftEncoder-cFULL_COUNT;
				}
				
				diffEncoder1 = -(diffEncoder1*2*3.14*wheelR)/800;
				
 
			diffEncoder2=currentRightEncoder-lastRightEncoder;
				if(abs(diffEncoder2)>16384)
				{
					if(currentRightEncoder<lastRightEncoder)
					diffEncoder2=cFULL_COUNT-lastRightEncoder+currentRightEncoder;
					else
					diffEncoder2=currentRightEncoder-lastRightEncoder-cFULL_COUNT;
				}
				
				diffEncoder2 = (diffEncoder2*2*3.14*wheelR)/800;
				//diffEncoder2 = diffEncoder2*angletravelled;
				//refresh the lastencoder
				
				
				//calculate the wheeldistance--they say is the circumference 2pir/(counts/cycle)---and it is 2pir/800
				wheelDistanceL = diffEncoder1 ;
				wheelDistanceR = diffEncoder2 ;
 
				//wheelDistanceL=-(2*3.14*wheelR*diffEncoder1)/CIRCLE_CNT;
				//wheelDistanceR=(2*3.14*wheelR*diffEncoder2)/CIRCLE_CNT;
				//calculate the wheeldistance at t-1 because I need the position at t-1
				
				//wheelDistanceL=(2*3.14*wheelR*currentLeftEncoderValue)/CIRCLE_CNT;
				//wheelDistanceL=((2*3.14*wheelR)/CIRCLE_CNT)*currentLeftEncoderValue;
				//wheelDistanceR=((2*3.14*wheelR)/CIRCLE_CNT)*currentRightEncoderValue;
				//calculate the wheeldistance at t-1 because I need the position at t-1
				//lastwheelDistanceL=(2*3.14*wheelR*lastLeftEncoder)/CIRCLE_CNT;
				//lastwheelDistanceR=(2*3.14*wheelR*lastRightEncoder)/CIRCLE_CNT;
				//calculate angle and distance travelled at the current time
 
				distravelled=(wheelDistanceL+wheelDistanceR)/2;
				
				angletravelled=((double)wheelDistanceR-(double)wheelDistanceL)/31;
				
 
				
				
				//angletravelled=Angle2PI(angletravelled);
				//angletravelled=AnglePI(angletravelled);
 
				
				//if (abs(wheelDistanceR)==abs(wheelDistanceL))
				//{
				//	angletravelled=0;
				//}
				currentdisttravelled = distravelled;
				
				//angletravelled=(angletravelled*180)/3.14;
 
				//lastdx=lastdistravelled*cos(angle+(double)lastangletravelled/2);
				//lastdy=lastdistravelled*sin(angle+(double)lastangletravelled/2);
				//calculate the position at current time 
				
				angletravelled=angletravelled+lastangletravelled;
				
				dx=0.5*(diffEncoder1+diffEncoder2)*cos((double)angle+(double)angletravelled);
				dy=0.5*(diffEncoder1+diffEncoder2)*sin((double)angle+(double)angletravelled);
				currentxr=dx+lastxr;
				currentyr=dy+lastyr;
				//lastxr=currentxr-lastdx;
				//lastyr=currentyr-lastdy;
 
 
		
		}//end if first time
			
		else
 
		{
		
			lastLeftEncoder = currentLeftEncoder;
			lastRightEncoder = currentRightEncoder;
				
			angletravelled=0;
			lastxr=0;
			lastyr=0;
			currentxr=0;
			currentyr=0;
			variabilax=0;
			variabilay=0;
			variabilax1=0;
			variabilay1=0;
			m_Encoder2=0;
			m_Encoder1=0;
 
			firstTime = false;
		}
 
		
		
	}//end if m_encoderspeed1!=0
		
		//angletravelled=angletravelled+lastangletravelled;
		//distravelled=distravelled+lastdistravelled;
	
		
	/*if ((landmark == true) && (m_Encoderspeed1!=0) && (onetime == false))
				{
					if ((m_Sonar2 <255)&&(distravelled+50 < m_Sonar2))
					{
						//imageprocessing
						OnStop();
						//turning 90 degrees
						m_MOTSDK.DcMotorPositionTimeCtrAll (225+m_Encoder1,200+m_Encoder2,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,2000);
 
 
					//	desiredWheelSpeed1 =0;
					//	desiredWheelSpeed2 =200;
						
 					}
				} 
*/
	
	//OnStop();
 
UpdateData(false);
}
 
void CI90ControllerDlg::OnForward() 
{
	// TODO: Add your control notification handler code here
	short desiredWheelSpeed1=-0.1*CIRCLE_CNT/(2*3.14*wheelR);//left has to be clockwise
	short desiredWheelSpeed2=0.1*CIRCLE_CNT/(2*3.14*wheelR);//right has to be counterclockwise so it should go forward
	
	
 
		//OnConnect();
 
		short m_sonar;
		OnStop();
		if( (landmark == true) && (m_Sonar2 < 255) && (m_Sonar2 > 50) && (m_Sonar1 > 50) &&(m_Sonar3 >50))
 
		{	
			//m_sonar = m_Sonar2;
			int vect[3];
			for(int i=0; i<3; i++)
			{
				vect[i] = 0;
			}
			int result;
			m_Sonar1= m_MOTSDK.GetSensorSonar1 ();
			m_Sonar2= m_MOTSDK.GetSensorSonar2 ();
			m_Sonar3= m_MOTSDK.GetSensorSonar3 ();
			vect [0] = m_Sonar1;
			vect [1] = m_Sonar2;
			vect [2] = m_Sonar3;
			DriveForward((m_Sonar2*0.01)/2,2000);
			
			result = MaxofVector1(vect,3);
			Sleep (3000);
			if( (result > 70) && (m_Sonar2 > 50) && (m_Sonar1 > 50) &&(m_Sonar3 >50) )
 
			{
 
			if(result == m_Sonar1)
			{
				TurnDegree(-60,2000);
				Sleep (1000);
			}
 
			if(result == m_Sonar2)
			{
				TurnDegree(0,2000);
				Sleep (1000);
			}
 
			if(result == m_Sonar3)
			{
				TurnDegree(60,2000);
				Sleep (1000);
			}
			
			}
			else
			{
				TurnDegree(90,2000);
				Sleep (1000);
					if( (m_Sonar2 > 50) && (m_Sonar1 > 50) &&(m_Sonar3 >50) )
					{
						cout<<'goo';
					}
									
					else
					{
						TurnDegree(-60,2000);
						Sleep (1000);
					}
				
			}
			
			
			Sleep(1000);
			//DriveForward((m_Sonar2*0.01)/2,2000);
			//Sleep (1000);
		
			//OnForward();
			
		
		}
		else 
			{
				if((m_Sonar2 = 255) && (m_Sonar2 > 50) && (m_Sonar1 > 50) &&(m_Sonar3 >50))
				{
				DriveForward(0.3,2000);
				Sleep(2000);
				}
				else
				{
				TurnDegree(-90,2000);
				Sleep(2000);
				}
				/*if( (m_Sonar2 > 70) && (m_Sonar1 > 70) &&(m_Sonar3 >70) )
				{
				DriveForward(0.3,2000);
				Sleep(2000);
				}
				else
				{
					TurnDegree(-60,2000);
					Sleep(2000);
				}*/
 
			}
			
		//OnConnect();
		// m_vitCtrl.Disconnect();
		
	
 
	
	//m_MOTSDK.SetDcMotorControlMode (0,M_VELOCITY);	
	//m_MOTSDK.SetDcMotorControlMode (1,M_VELOCITY);
	//m_MOTSDK.SetDcMotorVelocityControlPID (0, 30, 10, 0);
	//m_MOTSDK.SetDcMotorVelocityControlPID (1, 30, 10, 0);
	//m_MOTSDK.DcMotorVelocityNonTimeCtrAll(-100,150,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL);
	//m_MOTSDK.DcMotorVelocityNonTimeCtrAll (desiredWheelSpeed1,desiredWheelSpeed2,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL);
 
	//m_MOTSDK.DcMotorVelocityTimeCtrAll (desiredWheelSpeed1,desiredWheelSpeed2,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,1000);
	//if(turn ==false)
	//{
	
	
	//turn = true;
	//}
	//if(drive ==false )
//	{
	//DriveForward(5,2000);
//	drive = true;
//	}
	//m_MOTSDK.DcMotorPositionTimeCtrAll (225+m_Encoder1,200+m_Encoder2,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,2000);
 
	ismoving=true;
}
 
void CI90ControllerDlg::OnBack() 
{
	// TODO: Add your control notification handler code here
	m_MOTSDK.SetDcMotorControlMode (0,M_VELOCITY);
	m_MOTSDK.SetDcMotorControlMode (1,M_VELOCITY);
	m_MOTSDK.SetDcMotorVelocityControlPID (0, 30, 10, 0);
	m_MOTSDK.SetDcMotorVelocityControlPID (1, 30, 10, 0);
	//m_MOTSDK.DcMotorVelocityNonTimeCtrAll (200, -200,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL); 
	//DriveForward(0.3,1000);
	TurnDegree(45,2000);
 
 
}
 
void CI90ControllerDlg::OnStop() 
{
	// TODO: Add your control notification handler code here
	
	m_MOTSDK.DisableDcMotor (0);
	m_MOTSDK.DisableDcMotor (1);
	
	angletravelled=0;
	lastxr=0;
	lastyr=0;
	currentxr=0;
	currentyr=0;
	variabilax=0;
	variabilay=0;
	variabilax1=0;
	variabilay1=0;
	//m_Encoder2=0;
	//m_Encoder1=0;
	firstTime=true;
	
	
		/*	//Sleep(1000);
	VARIANT pvData;
			 VariantInit(&pvData);
				VARIANT pvInfo; 
			   VariantInit(&pvInfo);
			  unsigned char *pData;	
			  Sleep(2000);
			//long captureresult=m_vitCtrl.GetSnapshot(4,&pvData,&pvInfo);
			long captureresult=m_vitCtrl.GetSnapshot(2,&pvData,&pvInfo);
 
				CString strName,strName2;
				//CImage * image1,*image2,image3,image4;
				
				//strName = ".\\tempSnapShot1.bmp";
				strName = ".\\tempSnapShot1.bmp";
				//strName2=".\\tempSnapShot.bmp";
				//image1 = new CImage();
				
				m_vitCtrl.SaveSnapshot(2,strName);
				
			
			//	image1->Load(".\\tempSnapShot1.bmp");
			//	image1=&image3;
				//CImage* returnImage = new	CImage();
				
				//Sobel(image1,returnImage);
				*/
	//OnSnapShot();
	if(onetime == false)
	{
	ifstream infile_Pin1,infile_Pin2;    
	/* input file */
	
    char inFileName_Pin1[128]="Target.pgm";/* input file name */
	
	//char inFileName_Pin1[128]="Doi.pgm";
   PIC Pin1, Pin2;            //source image
	 /* open input/output file */
 
    infile_Pin1.open(inFileName_Pin1, ios::binary);
 
	// check input file 
   // if(!infile_Pin1)
  //  {
   //     cout<<"Cannot open input file "<< inFileName_Pin1 <<endl;
        //return 1;
  //  }
	int Gaussian_Mask[5][5] = {{2, 4, 5, 4, 2},
                                 {4, 9, 12, 9, 4},
                                 {5, 12, 15, 12, 5},
								 {4, 9, 12, 9, 4},
                                 {2, 4, 5, 4, 2}};
								
	int numpix =0;
	 /*double Gaussian_Mask[5][5] = {{0.0378,0.0394, 0.04, 0.0394, 0.0378},
                                 {0.0394, 0.0411, 0.0417,  0.0411,0.0394},
                                 {0.04, 0.0417, 0.0423,0.0417,0.04},
								 {0.0394,0.0411, 0.0417, 0.0411, 0.0394},
                                 {0.0378, 0.0394, 0.04, 0.0394, 0.0378}};*/
	
	if (LoadP5Header(infile_Pin1, Pin1) && (onetime == false)) // load pgm (Pin) image header information
    {
        // allocate the memory for the input image
        //the dimensions of the original
 
        Pin1.img = new BYTE[Pin1.Width*Pin1.Height];
 
        LoadImage(infile_Pin1, Pin1);
   
       Canny_Edge(Pin1,Gaussian_Mask, "Targetres.pgm"); 
 
		
	   	delete []Pin1.img;
		Pin1.img =NULL;
		onetime = true;
		//m_MOTSDK.DcMotorPositionTimeCtrAll (225+m_Encoder1,200+m_Encoder2,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,2000);
		//DriveForward(5,2000);
		
		
	}
	
	/*if( (landmark == true) && (m_Sonar2 < 255))
	{
		TurnDegree(30,2000);
		Sleep(3000);
		DriveForward((m_Sonar2*0.01)/2,2000);
		Sleep (1000);
	
	}
	else 
	{
		DriveForward(0.5,1000);
		Sleep(1000);
	}
	*/
	//OnForward();
	}
	onetime = false;
	//m_MOTSDK.EnableDcMotor (0);
	//m_MOTSDK.EnableDcMotor (1);
	
	//OnStop();
	
	
 
}
 
void CI90ControllerDlg::OnTurnLeft() 
{
	// TODO: Add your control notification handler code here
	long cmd1,cmd2;
 
    cmd1 = m_Encoder1 + cWHOLE_RANGE / 3;
    cmd2 = m_Encoder2 + cWHOLE_RANGE / 3;
    
   //change cmd1, cmd2 to valid data range
    if (cmd1 < 0) cmd1 = cmd1 + cFULL_COUNT;
    if (cmd2 < 0) cmd2 = cmd2 + cFULL_COUNT;
    if (cmd1 > cFULL_COUNT) cmd1 = cmd1 - cFULL_COUNT;
    if (cmd2 > cFULL_COUNT) cmd2 = cmd2 - cFULL_COUNT;
 
 
	m_MOTSDK.SetDcMotorControlMode (0,M_POSITION);
	m_MOTSDK.SetDcMotorControlMode (1,M_POSITION);
	m_MOTSDK.SetDcMotorVelocityControlPID (0, 30, 10, 0);
	m_MOTSDK.SetDcMotorPositionControlPID (0, 600,30,600);
	m_MOTSDK.SetDcMotorPositionControlPID (1, 600,30,600);
	m_MOTSDK.DcMotorPositionTimeCtrAll (cmd1,cmd2,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,1000);
 
}
 
void CI90ControllerDlg::OnTurnRight() 
{
	// TODO: Add your control notification handler code here
	long cmd1,cmd2;
 
    cmd1 = m_Encoder1 - cWHOLE_RANGE / 3;
    cmd2 = m_Encoder2 - cWHOLE_RANGE / 3;
    
    // change cmd1, cmd2 to valid data range
    if (cmd1 < 0) cmd1 = cmd1 + cFULL_COUNT;
    if (cmd2 < 0) cmd2 = cmd2 + cFULL_COUNT;
    if (cmd1 > cFULL_COUNT) cmd1 = cmd1 - cFULL_COUNT;
    if (cmd2 > cFULL_COUNT) cmd2 = cmd2 - cFULL_COUNT;
 
 
	m_MOTSDK.SetDcMotorControlMode (0,M_POSITION);
	m_MOTSDK.SetDcMotorControlMode (1,M_POSITION);
	m_MOTSDK.SetDcMotorVelocityControlPID (0, 30, 10, 0);
	m_MOTSDK.SetDcMotorPositionControlPID (0, 600,30,600);
	m_MOTSDK.SetDcMotorPositionControlPID (1, 600,30,600);
	m_MOTSDK.DcMotorPositionTimeCtrAll (cmd1,cmd2,NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,1000);	
}
 
 
 
void CI90ControllerDlg::OnCancel() 
{
	// TODO: Add extra cleanup here
	m_MOTSDK.StopAudioPlay ();
	m_MOTSDK.DisableDcMotor (0);
	m_MOTSDK.DisableDcMotor (1);
	
	if (m_vitCtrl.GetControlStatus() != 0 &&
		m_vitCtrl.GetControlStatus() != 3)
	{
        m_vitCtrl.Disconnect();
	}
 
	delete []green_Dist;
	green_Dist = 0;
	delete []lila_Dist;
	lila_Dist = 0;
	delete []orange_Dist;
	orange_Dist = 0;
	delete []pink_Dist;
	pink_Dist = 0;
	delete []Back_Dist;
	Back_Dist = 0;
	delete []blue_Dist;
	blue_Dist = 0;
	CDialog::OnCancel();
}
 
void CI90ControllerDlg::OnTimer(UINT nIDEvent) 
{
	// TODO: Add your message handler code here and/or call default
	
	CDialog::OnTimer(nIDEvent);
	
	if(m_bCameraConnected)
	{
		UpdateData(true);
		//++m_iTimerCnt;
		//m_sTimerCnt.Format("%d",m_iTimerCnt);
 
		//OnForward();
		//Sleep(1000);
 
		UpdateData(false);
		
		//do upate the UI here
			// get color ID
			
			VARIANT pvData;
			 VariantInit(&pvData);
				VARIANT pvInfo; 
			   VariantInit(&pvInfo);
			  unsigned char *pData;	
			long captureresult=m_vitCtrl.GetSnapshot(4,&pvData,&pvInfo);
			//long captureresult=m_vitCtrl.GetSnapshot(2,&pvData,&pvInfo);
 
				CString strName,strName2;
				//CImage * image1,*image2,image3,image4;
				
				//strName = ".\\tempSnapShot1.bmp";
				strName = ".\\tempSnapShot1333.bmp";
				//strName2=".\\tempSnapShot.bmp";
				//image1 = new CImage();
				
				m_vitCtrl.SaveSnapshot(2,strName);
			   if (captureresult==0)
			   {
					pData=(unsigned char *)pvData.parray->pvData;
					double dx1,dy1,phi;
				
					//ReadMat();
					
					ImageSegmentation(pData,Back_Dist,backdoor_Dist,fore_dist,blue_Dist,green_Dist,pink_Dist,lila_Dist,orange_Dist);
			   }	
			//make a path for him to go 
			
			if ((colorposition.character=='b')&&(pinkland == true))
			{
				TurnDegree(360,2000);
			}
			if((colorposition.character=='p')&&(m_Sonar2 <100)&& (pinkland == false))
			{
				TurnDegree(-45,2000);
				Sleep(3000);
				//colorposition.character=='n';
				m_Encoder1 = m_MOTSDK.GetEncoderPulse1 ();
				m_Encoder2 = m_MOTSDK.GetEncoderPulse2 ();
 
				DriveForward(2.5,6000);
				pinkland = true;
				//Sleep(2000);
				
				/*if(m_Sonar2 > 100)
				{
					DriveForward(2,2000);
					Sleep(2000);
				}
				*/
 
			}
			else if ((m_Sonar2 > 100)&&(pinkland == false))
			{
				DriveForward(0.5,2000);
				
			}
	
		//here make decison based on color ID
 
		/*if((colorposition.character=='b')&&(colorposition.col[0]<176))
			{
				//we detected the blue landmark in the left side of the robot
				leftpixel=true;
				distanceL1=m_Sonar1;
				thetaL1=asin((currentyr-yL[0])/distanceL1);
 
			}
			else if ((colorposition.character=='b')&&(colorposition.col[0]>=176))
			{
				
				//we detected the blue landmark in the right side of the robot
				rightpixel=true;
				distanceL2=m_Sonar2;
				//calculate the angle between the robot and the landmarks 
				thetaL2=asin((currentyr-yL[0])/distanceL2);
 
			}
 
			else if((colorposition.character=='g')&&(colorposition.col[1]<176))
			{
				//we detected the green landmark in the left side of the robot
				leftpixel=true;
				distanceL1=m_Sonar1;
				thetaL1=asin((currentyr-yL[1])/distanceL1);
 
			}
			else if ((colorposition.character=='g')&&(colorposition.col[1]>=176))
			{
				
				//we detected the green landmark in the right side of the robot
				rightpixel=true;
				distanceL2=m_Sonar2;
				//calculate the angle between the robot and the landmarks 
				thetaL2=asin((currentyr-yL[1])/distanceL2);
 
			}
 
			else if((colorposition.character=='o')&&(colorposition.col[2]<176))
			{ 
				//we detected the orange landmark in the left side of the robot
				leftpixel=true;
				distanceL1=m_Sonar1;
				thetaL1=asin((currentyr-yL[5])/distanceL1);
 
			}
			else if ((colorposition.character=='o')&&(colorposition.col[2]>=176))
			{
				
				//we detected the orange landmark in the right side of the robot
				rightpixel=true;
				distanceL2=m_Sonar2;
				//calculate the angle between the robot and the landmarks 
				thetaL2=asin((currentyr-yL[5])/distanceL2);
 
			}
 
			else if((colorposition.character=='l')&&(colorposition.col[3]<42420))
			{
				//we detected the lila landmark in the left side of the robot
				leftpixel=true;
				distanceL1=m_Sonar1;
				thetaL1=asin((currentyr-yL[4])/distanceL1);
 
			}
			else if ((colorposition.character=='l')&&(colorposition.col[3]>=42420))
			{
				
				//we detected the lila landmark in the right side of the robot
				rightpixel=true;
				distanceL2=m_Sonar2;
				//calculate the angle between the robot and the landmarks 
				thetaL2=asin((currentyr-yL[4])/distanceL2);
 
			}
 
			else if((colorposition.character=='p')&&(colorposition.col[4]<42420))
			{
				//we detected the pink landmark in the left side of the robot
				leftpixel=true;
				distanceL1=m_Sonar1;
				thetaL1=asin((currentyr-yL[3])/distanceL1);
 
			}
			else if ((colorposition.character=='p')&&(colorposition.col[4]>=42420))
			{
				
				//we detected the pink landmark in the right side of the robot
				rightpixel=true;
				distanceL2=m_Sonar2;
				//calculate the angle between the robot and the landmarks 
				thetaL2=asin((currentyr-yL[3])/distanceL2);
 
			}	
			else if (colorposition.character=='n')
			{
			rightpixel=false;
			leftpixel=false;
			thetaL1=0;
			thetaL2=0;
			distanceL1=0;
			distanceL2=0;
			}
 
	
		
		if(ismoving && m_Encoderspeed1!=0 && m_Encoderspeed2!=0)
		{
				//xxt=calculatematrices(firstTime,thetaL1,thetaL2,distanceL1,distanceL2,rightpixel,leftpixel,currentxr,currentyr,angletravelled,xL,yL);
				
				//xxt=calculatematrices(firstTime,thetaL1,thetaL2,distanceL1,distanceL2,rightpixel,leftpixel,currentxr,currentyr,angletravelled,lastangletravelled,lastxr,lastyr,xL,yL);
 
				xxt=calculatematrices(firstTime,thetaL1,thetaL2,distanceL1,distanceL2,rightpixel,leftpixel,currentxr,currentyr, angletravelled, lastangletravelled, lastxr,lastyr, xL, yL, numberofcolors);
 
				variabilax =xxt.x[0][0];
				variabilay =xxt.x[1][0];
				variabilax1 = xxt.xt[0][0];
				variabilay1 = xxt.xt[1][0];
 
 
				CClientDC dc(this);
				CPoint p1(510+variabilax1,520-variabilay1);
				dc.MoveTo(lastpoint1);
				dc.LineTo(p1);
				lastpoint1=p1;
				
				CPen  myPen2,myPen3;
 
				//CPen penBlack(PS_COSMETIC,1,RGB(255,0,0));
				LOGBRUSH logBrush,logBrush1;
				logBrush.lbStyle = BS_SOLID;
				logBrush.lbColor = RGB(255,0,0);
				myPen2.CreatePen(PS_DOT|PS_GEOMETRIC|PS_ENDCAP_ROUND, 2, &logBrush); 
				dc.SelectObject(&myPen2);
				CPoint p3(510+variabilax,520-variabilay);
				dc.MoveTo(lastpoint2);
				dc.LineTo(p3);
				lastpoint2=p3;
				//angletravelled=angletravelled+lastangletravelled;
				//CPaintDC dc(this);
 
				logBrush1.lbStyle = BS_SOLID;
				logBrush1.lbColor = RGB(0,190,0);
				myPen3.CreatePen(PS_DOT|PS_GEOMETRIC|PS_ENDCAP_ROUND, 2, &logBrush1); 
				dc.SelectObject(&myPen3);
				dc.SetPixel(510 + m_Sonar1,520 - variabilay1,RGB(0,190,0));
				dc.SetPixel(510 + m_Sonar2,520 - variabilay1,RGB(200,0,0));
				dc.SetPixel(510 + m_Sonar3,520 - variabilay1,RGB(0,0,200));
 
				
 
 
 
								
 
		}
		
 
		lastdistravelled = currentdisttravelled;
		
		lastangletravelled=angletravelled;
		
		lastdistravelled=distravelled;
		lastxr=currentxr;
		lastyr=currentyr;
		lastRightEncoder = currentRightEncoder;
		lastLeftEncoder = currentLeftEncoder;
		*/
	}
}
 
void CI90ControllerDlg::OnRecord() 
{
	// TODO: Add your control notification handler code here
 
	
}
 
void CI90ControllerDlg::OnVoiceSegmentEventDrrobotsdkcontrolctrl1() 
{
	// TODO: Add your control notification handler code here
 
}
 
void CI90ControllerDlg::OnVoiceSegmentEventDrrobotsdkcontrolctrl2() 
{
	// TODO: Add your control notification handler code here
	
}
 
 
 
BOOL SaveAudioFile(LPCTSTR FileName) 
{
	
	return TRUE;
}
 
void CI90ControllerDlg::OnConnect() 
{
	// TODO: Add your control notification handler code here
	 if (m_vitCtrl.GetControlStatus() != 0 &&
		m_vitCtrl.GetControlStatus() != 3)
	{
        m_vitCtrl.Disconnect();
		return;
	}
 
	CString strServIP, strRootPwd, strUserName, strTemp;
 
	GetDlgItem(IDC_EDIT_SERV_IP)->GetWindowText(strServIP);
	GetDlgItem(IDC_EDIT_PORT)->GetWindowText(strTemp);
	GetDlgItem(IDC_EDIT_ROOT_ID)->GetWindowText(strUserName);
	GetDlgItem(IDC_EDIT_ROOT_PWD)->GetWindowText(strRootPwd);
 
	m_vitCtrl.SetPassword(strRootPwd);
	m_vitCtrl.SetUserName(strUserName);
	m_vitCtrl.SetRemoteIPAddr(strServIP);
	long lPort = atoi(strTemp);
	if (lPort <= 0 || lPort > 65535)
		lPort = 80;
	m_vitCtrl.SetHttpPort(lPort);
	
	//m_vitCtrl.SetMediaType(1);
 
	m_vitCtrl.Connect();
 
	GetDlgItem(IDC_EDIT_SERV_IP)->EnableWindow(FALSE);
	GetDlgItem(IDC_EDIT_PORT)->EnableWindow(FALSE);
	GetDlgItem(IDC_EDIT_ROOT_ID)->EnableWindow(FALSE);
	GetDlgItem(IDC_EDIT_ROOT_PWD)->EnableWindow(FALSE);
 
	GetDlgItem(IDC_Connect)->SetWindowText("Stop");
 
	GetDlgItem(IDC_CameraUp)->EnableWindow(true);
}
 
void CI90ControllerDlg::OnOnConnectionBrokenVitaminctrl1(long eConnType) 
{
	// TODO: Add your control notification handler code here
	GetDlgItem(IDC_EDIT_SERV_IP)->EnableWindow(TRUE);
	GetDlgItem(IDC_EDIT_PORT)->EnableWindow(TRUE);
	GetDlgItem(IDC_EDIT_ROOT_ID)->EnableWindow(TRUE);
	GetDlgItem(IDC_EDIT_ROOT_PWD)->EnableWindow(TRUE);
	GetDlgItem(IDC_Connect)->SetWindowText("Connect");
	
	m_bCameraConnected	= false;
}
 
void CI90ControllerDlg::OnCameraUp() 
{
	// TODO: Add your control notification handler code here
	m_vitCtrl.SendCameraCommand ("up",30000);	
}
 
 
void CI90ControllerDlg::OnCameraDown() 
{
	// TODO: Add your control notification handler code here
	m_vitCtrl.SendCameraCommand ("down",30000);	
}
 
void CI90ControllerDlg::OnCameraLeft() 
{
	// TODO: Add your control notification handler code here
	m_vitCtrl.SendCameraCommand ("left",30000);	
}
 
void CI90ControllerDlg::OnCameraRight() 
{
	// TODO: Add your control notification handler code here
	m_vitCtrl.SendCameraCommand ("right",30000);	
}
 
void CI90ControllerDlg::OnCameraReset() 
{
	// TODO: Add your control notification handler code here
	m_vitCtrl.SendCameraCommand ("home",30000);	
}
 
void CI90ControllerDlg::OnPan() 
{
	// TODO: Add your control notification handler code here
	m_vitCtrl.SendCameraCommand ("pan",30000);	
}
 
void CI90ControllerDlg::OnCameraStop() 
{
	// TODO: Add your control notification handler code here
	m_vitCtrl.SendCameraCommand ("stop",30000);	
}
 
void CI90ControllerDlg::OnCustomdrawPanSpeed(NMHDR* pNMHDR, LRESULT* pResult) 
{
	// TODO: Add your control notification handler code here
 
}
 
 
 
void CI90ControllerDlg::OnReleasedcapturePanSpeed(NMHDR* pNMHDR, LRESULT* pResult) 
{
	// TODO: Add your control notification handler code here
	
	int nPos;
	nPos = m_PanSlider.GetPos ();
 
	m_vitCtrl.SendCameraControlSpeed (1,nPos);
	*pResult = 0;
}
 
void CI90ControllerDlg::OnReleasedcaptureTiltSpeed(NMHDR* pNMHDR, LRESULT* pResult) 
{
	// TODO: Add your control notification handler code here
	
	int nPos;
	nPos = m_TiltSlider.GetPos ();
 
	m_vitCtrl.SendCameraControlSpeed (2,nPos);
	*pResult = 0;
}
 
void CI90ControllerDlg::OnOnConnectionOKVitaminctrl1(long eConnType) 
{
	// TODO: Add your control notification handler code here
	GetDlgItem(IDC_EDIT_SERV_IP)->EnableWindow(FALSE);
	GetDlgItem(IDC_EDIT_PORT)->EnableWindow(FALSE);
	GetDlgItem(IDC_EDIT_ROOT_ID)->EnableWindow(FALSE);
	GetDlgItem(IDC_EDIT_ROOT_PWD)->EnableWindow(FALSE);
	GetDlgItem(IDC_Connect)->SetWindowText("Stop");
 
	GetDlgItem(IDC_SnapShot)->EnableWindow(TRUE);
 
	m_bCameraConnected = true;
}
 
void CI90ControllerDlg::OnSnapShot() 
{
	// TODO: Add your control notification handler code here
	CString strName;
	strName = ".\\tempSnapShotdoor6.bmp";
	m_vitCtrl.SaveSnapshot(2, strName);
}
int* CI90ControllerDlg::MaxofVector(int *vect,int size)
{
	int *result = new int [2];
	maximum=vect[0];
	indexmax=0;
	for(int i=0;i<size;i++)
	{
		if(vect[i]>maximum)
		{
			maximum=vect[i];
			indexmax=i; 
			
		}
		
	}
	result[0]=maximum;
	result[1]=indexmax;
		
	return result;
}
int CI90ControllerDlg::MaxofVector1(int *vect,int size)
{
	int result;
	maximum=vect[0];
	//indexmax=0;
	for(int i=0;i<size;i++)
	{
		if(vect[i]>=maximum)
		{
			maximum=vect[i];
			//indexmax=i; 
			
		}
		
	}
	result=maximum;
	//result[1]=indexmax;
		
	return result;
}
void CI90ControllerDlg::ReadMat()// read data from matlab
{
 // TODO: Add your command handler code here
 
	MATFile *pmatFile,*pmatFile1,*pmatFile2,*pmatFile3,*pmatFile4,*pmatFile5,*pmatFile6,*pmatFile7;
	mxArray *pMxArray,*pMxArray1,*pMxArray2,*pMxArray3,*pMxArray4,*pMxArray5,*pMxArray6,*pMxArray7;
	
 
	const char *filename="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller_DRI\\BackDist1D.mat";
	
	pmatFile = matOpen(filename,"r");
	pMxArray = matGetVariable(pmatFile,"back_dist_1D");
	Back_Dist = (double *)mxGetData(pMxArray);
	
	matClose(pmatFile);
	mxFree(pMxArray);
	mxFree(pmatFile);
 
    const char *filename1="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller_DRI\\bluePattern1D.mat";
 
	pmatFile1 = matOpen(filename1,"r");
	pMxArray1 = matGetVariable(pmatFile1,"blue_dist_1D");
	blue_Dist = (double *)mxGetData(pMxArray1);
 
	matClose(pmatFile1);
	mxFree(pMxArray1);
	mxFree(pmatFile1);
 
	const char *filename2="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller_DRI\\greenPattern1D.mat";
 
	pmatFile2 = matOpen(filename2,"r");
	pMxArray2= matGetVariable(pmatFile2,"green_dist_1D");
	green_Dist = (double *)mxGetData(pMxArray2);
	
	matClose(pmatFile2);
	mxFree(pMxArray2);
	mxFree(pmatFile2);
 
 
	const char *filename3="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller_DRI\\lilaPattern1D.mat";
	
	pmatFile3 = matOpen(filename3,"r");
	pMxArray3 = matGetVariable(pmatFile3,"lila_dist_1D");
	lila_Dist = (double *)mxGetData(pMxArray3);
 
	matClose(pmatFile3);
	mxFree(pMxArray3);
	mxFree(pmatFile3);
	
	const char *filename4="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller_DRI\\orangePattern1D.mat";
	
	pmatFile4 = matOpen(filename4,"r");
	pMxArray4 = matGetVariable(pmatFile4,"orange_dist_1D");
	orange_Dist = (double *)mxGetData(pMxArray4);
	
	matClose(pmatFile4);
	mxFree(pMxArray4);
	mxFree(pmatFile4);
 
	const char *filename5="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller_DRI\\pinkPattern1D.mat";
	
	pmatFile5 = matOpen(filename5,"r");
	pMxArray5 = matGetVariable(pmatFile5,"pink_dist_1D");
	pink_Dist = (double *)mxGetData(pMxArray5);
 	
 
	matClose(pmatFile5);
	mxFree(pMxArray5);
	mxFree(pmatFile5);
 
	//door rec
	const char *filename6="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller_DRI\\BackDistdoor.mat";
	
	pmatFile6 = matOpen(filename6,"r");
	pMxArray6 = matGetVariable(pmatFile6,"back_dist11");
	backdoor_Dist = (double *)mxGetData(pMxArray6);
 	
 
	matClose(pmatFile6);
	mxFree(pMxArray6);
	mxFree(pmatFile6);
 
	const char *filename7="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller_DRI\\DoorDist.mat";
	
	pmatFile7 = matOpen(filename7,"r");
	pMxArray7 = matGetVariable(pmatFile7,"fore_dist11");
	fore_dist = (double *)mxGetData(pMxArray7);
 	
 
	matClose(pmatFile7);
	mxFree(pMxArray7);
	mxFree(pmatFile7);
 
}
detect CI90ControllerDlg::ImageSegmentation(const unsigned char* pBGR,double* Back_Dist,double* backdoor_Dist,double* fore_dist,double* blue_Dist,double* green_Dist,double* pink_Dist,double* lila_Dist,double* orange_Dist)
{
		//Define some parameters for segmentation
		
 
		int HIST_RES = 32;			//number of classes in each color band
		int HIST_SUB_FACTOR;
		double P_fg = 0.09;			//probability of foreground
		double K_TH = 0.0673;		// ratio of probability of foreground to probability of background
		HIST_SUB_FACTOR = 256 / HIST_RES;
		double P_bg;
		P_bg = 1 - P_fg;
 
		
		unsigned int* m_bdata=new unsigned int[m_Width*m_Height];
		unsigned int* m_gdata=new unsigned int[m_Width*m_Height];
		unsigned int* m_rdata=new unsigned int[m_Width*m_Height];
		unsigned int* index=new unsigned int[m_Width*m_Height];
		double* p_blue_pixel=new double[m_Width*m_Height];
		double* p_green_pixel=new double[m_Width*m_Height];
		double* p_pink_pixel=new double[m_Width*m_Height];
		double* p_orange_pixel=new double[m_Width*m_Height];
		double* p_lila_pixel=new double[m_Width*m_Height];
		double* p_back_pixel=new double[m_Width*m_Height];
		int *colorlist=new int [6];
		//door rec
		//double* p_backdoor_pixel=new double[m_Width*m_Height];
		//double* p_door_pixel=new double[m_Width*m_Height];
		
		for(int i=0;i<6;i++)
		{
			colorlist[i]=0;
		}
 
 
 
		//defined by gabi for detecting the color and returning the position of the pixel
		
		
 
		//Initialize the output
		 
		        
        //RGB version
		// Read in the frame
		for (unsigned int i = 0; i < m_Width*m_Height; i++) 
		{
                m_bdata[i] = *pBGR++;
                m_gdata[i] = *pBGR++;
                m_rdata[i] = *pBGR++;                
        }     
				
		 // Rescale R, G, B to be between 0 and HIST_RES
		 for (unsigned int z = 0; z < m_Width*m_Height;z++)
		 {
			 m_bdata[z] = m_bdata[z] / HIST_SUB_FACTOR + 1;
			 m_gdata[z] = m_gdata[z] / HIST_SUB_FACTOR + 1;
			 m_rdata[z] = m_rdata[z] / HIST_SUB_FACTOR + 1;
		 }
 
		 // Convert R, G, B into idices to be used in indexing the skin and background distributions
		 
 
		 for (unsigned int z = 0; z < m_Width*m_Height; z++) 
		 {
			 index[z] = (m_bdata[z] - 1) * HIST_RES*HIST_RES + (m_gdata[z] - 1) * HIST_RES + m_rdata[z];
			 //index[z] = (m_rdata[z] - 1) * HIST_RES*HIST_RES + (m_gdata[z] - 1) * HIST_RES + m_bdata[z];
		 }
 
		 // Get the probability of being foreground and background at each pixel
		 
		 int index_temp;
		 
		 colordetect1='a';
		 //for(int k=0;k<5;k++)
		 //{
		 //colordetect1[k]='a';
		 //}
		
		
 
		 for (unsigned int z = 0; z <m_Width*m_Height; z++) 
		 {
			 index_temp = index[z];
			 p_blue_pixel[z] = blue_Dist[index_temp-1];
			 p_green_pixel[z]= green_Dist[index_temp-1];	
			 p_pink_pixel[z] = pink_Dist[index_temp-1];
			 p_lila_pixel[z] = lila_Dist[index_temp-1];
			 p_orange_pixel[z] = orange_Dist[index_temp-1];
			 p_back_pixel[z] = Back_Dist[index_temp-1];
			 //door rec
			 //p_backdoor_pixel[z] = backdoor_Dist[index_temp-1];
			 //p_door_pixel[z]= fore_dist[index_temp-1];
		 }
 
		 // Find the indices of pixels that are classified as skin by Bayes'
		double suma=0;
		for(int i=0;i<m_Width*m_Height;i++)
		{
			suma=suma+p_lila_pixel[i];
		}
		double indice=0;
		 for (unsigned int z = 0; z < m_Width*m_Height; z++) 
		 {
			 p_temp = p_blue_pixel[z] / (K_TH * P_bg / P_fg);
 
			 if (p_temp > p_back_pixel[z]) 
			 {
				 colblue = z % m_Width-1;//this are the index of the pixel
				 rowblue = z / m_Width;
				
				 colorposition.col[0]=colblue;
				 colorposition.row[0]=rowblue;
				 numberofbluepixel++;
				 
			 }
			double suma11=0;
			p_temp1 = p_green_pixel[z] / (K_TH * P_bg / P_fg);
 
			 if (p_temp1 > p_back_pixel[z]) 
			 {
				 colgreen = z % m_Width-1;//this are the index of the pixel
				 rowgreen = z / m_Width;
				 indice = (IMAGE_HEIGHT - 1)*colgreen + IMAGE_WIDTH;
				
				 colorposition.col[1]=colgreen;
				 colorposition.row[1]=rowgreen;
				 numberofgreenpixel++;
 
				
			 }
 
			//for(int i=0;i<m_Width*m_Height;i++)
			//{
		//		suma11=suma11 + p_green_pixel[i];
		//	}
 
			 p_temp2 = p_orange_pixel[z] / (K_TH * P_bg / P_fg);
 
			 if (p_temp2 > p_back_pixel[z]) 
			 {
				colorange = z % m_Width-1;//this are the index of the pixel
				roworange = z / m_Width;
				 colorposition.col[2]=colorange;
				 colorposition.row[2]=roworange;
				numberoforangepixel++;
				
			 }
 
			p_temp3 = p_lila_pixel[z] / (K_TH * P_bg / P_fg);
 
			 if (p_temp3 > p_back_pixel[z]) 
			 {
				 collila = z % m_Width-1;//this are the index of the pixel
				 rowlila = z / m_Width;
				 colorposition.col[3]=collila;
				 colorposition.row[3]=rowlila;
				 numberoflilapixel++;
				
			 }
 
			 p_temp4 = p_pink_pixel[z] / (K_TH * P_bg / P_fg);
 
			 if (p_temp4 > p_back_pixel[z]) 
			 {
				 colpink = z % m_Width-1;//this are the index of the pixel
				 rowpink = z / m_Width;
				 colorposition.col[4]=colpink;
				 colorposition.row[4]=rowpink;
				 numberofpinkpixel++;
				
			 }
			 //door rec
			 /*p_temp5 = p_door_pixel[z] / (K_TH * P_bg / P_fg);
 
			 if (p_temp5 > p_backdoor_pixel[z]) 
			 {
				 //colpink = z % m_Width-1;//this are the index of the pixel
				 //rowpink = z / m_Width;
				 //colorposition.col[4]=colpink;
				 //colorposition.row[4]=rowpink;
				 numberofdoorpixel++;
				
			 }
			*/
		 }
		
		 colorlist[0]=numberofbluepixel;
		 colorlist[1]=numberofgreenpixel;
		 colorlist[2]=numberoforangepixel;
		 colorlist[3]=numberoflilapixel;
		 colorlist[4]=numberofpinkpixel;
		 //colorlist[5]=numberofdoorpixel;
 
		  int indexofcolors[6];
		
		int* maxcolorandindex;
		 maxcolorandindex=MaxofVector(colorlist,6);
		 if (maxcolorandindex[0]!=0)
		 {
			 if(maxcolorandindex[1]==0)
			 {
				colordetect1='b';
			 }
 
			  if(maxcolorandindex[1]==1)
			 {
				
				colordetect1='g';
			 }
			   if(maxcolorandindex[1]==2)
			 {
				 colordetect1='o';
			 }
			    if(maxcolorandindex[1]==3)
			 {
				  colordetect1='l';
			 }
				 if(maxcolorandindex[1]==4)
			 {
			 colordetect1='p';
			 }
				 
				 if(maxcolorandindex[1]==5)
			 {
			 colordetect1='d';
			 }
		 }
		 else
		 {
			 colordetect1='n';
		 }
 
	
		 colorposition.character=colordetect1;
		 
 
 		delete [] m_bdata;
		m_bdata = 0;
 
 
		delete [] m_gdata;
		m_gdata = 0;
		delete []m_rdata;
		m_rdata = 0;
		delete [] index;
		index = 0;
		
		delete [] p_blue_pixel;
		p_blue_pixel =0;
		delete [] p_green_pixel;
		p_green_pixel = 0;
		delete [] p_pink_pixel;
		p_pink_pixel = 0;
		delete [] p_orange_pixel;
		p_orange_pixel = 0;
		delete [] p_lila_pixel;
		p_lila_pixel = 0;
		delete [] p_back_pixel;
		p_back_pixel = 0;
 
		//delete [] p_backdoor_pixel;
		//p_backdoor_pixel = 0;
 
		//delete [] p_door_pixel;
		//p_door_pixel = 0;
		
		
		
		
 
		return colorposition;
		
		 
}
 double CI90ControllerDlg::Angle2PI(double tempAngle)
 
{
 
double numberPI;
 
numberPI = abs(tempAngle) / (3.14 * 2);
 
if (( abs(tempAngle) - numberPI * (3.14 * 2)) >= 0)
{
numberPI = numberPI ;
}
 
else
{
	numberPI =numberPI - 1;
}
 
tempAngle = (abs(tempAngle) - numberPI * (3.14 * 2));
 
return tempAngle;
 
} 
 
 double CI90ControllerDlg::AnglePI(double tempAngle)
 
{
 
if (tempAngle > 3.14)
 
tempAngle = tempAngle - (3.14 * 2);
 
if (tempAngle < -3.14)
 
tempAngle = tempAngle + (3.14* 2);
 
return tempAngle;
 
}
//CImage* CI90ControllerDlg::Sobel(CImage* image)
 
 
//function that loads in the header
bool CI90ControllerDlg::LoadP5Header(ifstream &infile, PIC &pic)
{
    bool rtv = true;
    char buf[16];
    int bufIndex;
    int width, height, maxval;
 
    infile.read(buf, 2); // get the magic number
    buf[2]='\0';
 
    if(buf[0] == 'P' && buf[1] == '5')
	//if(buf[0] == 'P' && buf[1] == '2')
	{
        infile.read(buf, 1);
        while(isspace(buf[0])) // Skip white space(s)
		{
            infile.read(buf,1);
		}
 
        // get width
        bufIndex = 0;
        while(bufIndex < 15 && !isspace(buf[bufIndex]))
		{
            bufIndex++;
            infile.read(buf+bufIndex, 1);
        }
        buf[bufIndex] = NULL;  // null terminated string
        width = atoi(buf);
 
        // get height
        infile.read(buf,1);
        while(isspace(buf[0])) // Skip white space(s)
		{
            infile.read(buf,1);
        }
        bufIndex = 0;  //line 
        while(bufIndex < 15 && !isspace(buf[bufIndex]))
		{
            bufIndex++;
            infile.read(buf+bufIndex, 1);
        }
        buf[bufIndex] = NULL;  // null terminated string
        height = atoi(buf);
 
       // get Maxval
		infile.read(buf,1);
        while(isspace(buf[0])) // Skip white space(s)
		{
            infile.read(buf,1);
        }
        bufIndex = 0;
        while(bufIndex < 15 && !isspace(buf[bufIndex]))
		{
            bufIndex++;
            infile.read(buf+bufIndex, 1);
        }
        buf[bufIndex] = NULL;  // null terminated string
        maxval = atoi(buf);
 
		// Skip white space(s)
		//infile.read(buf,1);
 
        // set the image information in the struct
        pic.InterLeaved = false;
        pic.Width = width;
        pic.Height = height;
		pic.Maxval = maxval;
 
    }
    else rtv = false;
 
    return rtv;
}; // end of LoadP5Header()
 
//function that accepts an infile object and a PIC Object
//and reads in the PIC object
void CI90ControllerDlg::LoadImage(ifstream &infile, PIC &pic)
{
    infile.read(reinterpret_cast<char *>(pic.img), pic.Width*pic.Height);
}
 
//function that accepts an outstream file and a PIC object to
//and writes the output stream to the PIC object
void CI90ControllerDlg::WritePGM(ofstream & outfile, PIC pic)
{
    //outfile << "P5" << endl;
	outfile << "P5" << endl;
    outfile << pic.Width << " " << pic.Height << endl;
	outfile << pic.Maxval << endl;
	pic.nChannel=1;
	pic.InterLeaved = false;
    outfile.write(reinterpret_cast<char *>(pic.img), pic.Width*pic.Height);
}
 
 void CI90ControllerDlg::Canny_Edge(PIC Pin_t, int Mask[5][5], char *outFileName_Pout)
//void CI90ControllerDlg::Canny_Edge(PIC Pin_t, double Mask[5][5], char *outFileName_Pout)
{
    
        ofstream outfile_Pout;
 
        outfile_Pout.open(outFileName_Pout, ios::binary);
       /* 
        int **IMG__gaussian_matrix = AllocateDynamicArray<int>(Pin_t.Width+6,Pin_t.Height+6);
        int **IMG_temp_matrix = AllocateDynamicArray<int>(Pin_t.Width,Pin_t.Height);
		int **IMG_temp_matrix_2 = AllocateDynamicArray<int>(Pin_t.Width+2,Pin_t.Height+2);
		int **IMG_final_matrix = AllocateDynamicArray<int>(Pin_t.Width,Pin_t.Height);
 
		int **IMG__sobel_matrix = AllocateDynamicArray<int>(Pin_t.Width+2,Pin_t.Height+2);
		float **IMG__gradient_matrix = AllocateDynamicArray<float>(Pin_t.Width+1,Pin_t.Height+1);
		int **IMG__direction_matrix = AllocateDynamicArray<int>(Pin_t.Width,Pin_t.Height);
		*/
		Array2DInt IMG__gaussian_matrix = CreateArray2D<int>(Pin_t.Width+6,Pin_t.Height+6);
        Array2DInt IMG_temp_matrix = CreateArray2D<int>(Pin_t.Width,Pin_t.Height);
        Array2DInt IMG_temp_matrix_2 = CreateArray2D<int>(Pin_t.Width+2,Pin_t.Height+2);
        Array2DInt IMG_final_matrix = CreateArray2D<int>(Pin_t.Width,Pin_t.Height); 
        Array2DInt IMG__sobel_matrix = CreateArray2D<int>(Pin_t.Width+2,Pin_t.Height+2);
        Array2DFloat IMG__gradient_matrix = CreateArray2D<float>(Pin_t.Width+1,Pin_t.Height+1);
        Array2DInt IMG__direction_matrix = CreateArray2D<int>(Pin_t.Width,Pin_t.Height);
 
		int Sobel_Filter_x [3][3] = {{-1,0,1},
								{-2,0,2},
								{-1,0,1}};
 
		int Sobel_Filter_y [3][3] = {{1,2,1},
								{0,0,0},
								{-1,-2,-1}};
   
        int Npixels, numberpixel,pixelCnt, gaussianValue, Gx, Gy, finalAngle;
		float tempAngle;
        int upperThresh, lowerThresh;
		upperThresh=60;
		lowerThresh=15;
		Npixels = Pin_t.Width*Pin_t.Height;
		Pin_t.Maxval = 255;
 
		//Create a two pixel layer of black border around the original image to apply the gaussian mask to boundary values
 
        for(int j = 0; j <= Pin_t.Height+3; j++)
        {
             IMG__gaussian_matrix[0][j] = 0;
             IMG__gaussian_matrix[1][j] = 0;
             IMG__gaussian_matrix[Pin_t.Width+2][j] = 0; 
             IMG__gaussian_matrix[Pin_t.Width+3][j] = 0; 
        }
        
        for(int k = 0; k <= Pin_t.Width+3; k++)
        {
             IMG__gaussian_matrix[k][0] = 0;
             IMG__gaussian_matrix[k][1] = 0;
             IMG__gaussian_matrix[k][Pin_t.Height+2] = 0;
             IMG__gaussian_matrix[k][Pin_t.Height+3] = 0;
        }
 
        pixelCnt = 0;
        
		//make 2D matrix of input
 
        for(int j = 2; j < Pin_t.Height + 2; j++)
        {
              for(int k = 2; k < Pin_t.Width + 2; k++)
              {
                    IMG__gaussian_matrix[k][j] = (int) Pin_t.img[pixelCnt];
                    pixelCnt++;
              }
         }
 
 
		// Gaussian Smooth the Original Image
 
        
        for(int j=2; j <= Pin_t.Height+1; j++)
        {           
            for(int k=2; k <= Pin_t.Width+1; k++)
            {      
 
                      gaussianValue = Mask[0][0]*IMG__gaussian_matrix[k-2][j-2] + Mask[0][1]*IMG__gaussian_matrix[k-2][j-1] + Mask[0][2]*IMG__gaussian_matrix[k-2][j] + Mask[0][3]*IMG__gaussian_matrix[k-2][j+1] + Mask[0][3]*IMG__gaussian_matrix[k-2][j+2]
					             + Mask[1][0]*IMG__gaussian_matrix[k-1][j-2] + Mask[1][1]*IMG__gaussian_matrix[k-1][j-1] + Mask[1][2]*IMG__gaussian_matrix[k-1][j] + Mask[1][3]*IMG__gaussian_matrix[k-1][j+1] + Mask[1][4]*IMG__gaussian_matrix[k-1][j+2]
								 + Mask[2][0]*IMG__gaussian_matrix[k][j-2] + Mask[2][1]*IMG__gaussian_matrix[k][j-1] + Mask[2][2]*IMG__gaussian_matrix[k][j] + Mask[2][3]*IMG__gaussian_matrix[k][j+1] + Mask[2][4]*IMG__gaussian_matrix[k][j+2]
                                 + Mask[3][0]*IMG__gaussian_matrix[k+1][j-2] + Mask[3][1]*IMG__gaussian_matrix[k+1][j-1] + Mask[3][2]*IMG__gaussian_matrix[k+1][j] + Mask[3][3]*IMG__gaussian_matrix[k+1][j+1] + Mask[3][4]*IMG__gaussian_matrix[k+1][j+2]
                                 + Mask[4][0]*IMG__gaussian_matrix[k+2][j-2] + Mask[4][1]*IMG__gaussian_matrix[k+2][j-1] + Mask[4][2]*IMG__gaussian_matrix[k+2][j] + Mask[4][3]*IMG__gaussian_matrix[k+2][j+1] + Mask[4][4]*IMG__gaussian_matrix[k+2][j+2];
								
 
                IMG_temp_matrix[k-2][j-2] = (int)(gaussianValue/159);
				  //IMG_temp_matrix[k-2][j-2] = (int)(gaussianValue);
            } 
        }
		
		//Create a single pixel layer of black border around the smoothed image to apply the sobel mask to boundary values
 
        for(int j = 0; j <= Pin_t.Height+1; j++)
        {
 
             IMG_temp_matrix_2[0][j] = 0;
             IMG_temp_matrix_2[Pin_t.Width+1][j] = 0; 
        }
        
        for(int k = 0; k <= Pin_t.Width+1; k++)
        {
             IMG_temp_matrix_2[k][0] = 0;
             IMG_temp_matrix_2[k][Pin_t.Height+1] = 0;
        }
		for(int j1=1; j1 <= Pin_t.Height;j1++)
		{
			for(int k0 =1; k0 <= Pin_t.Width;k0++)
			{
				IMG_temp_matrix_2[k0][j1]=IMG_temp_matrix[k0-1][j1-1];
			}
		}
 
		// Filter the Smoothed Image with both the x and y direction sobel masks and in each iteration determine the magnitude of the gradient as well as the direction
 
		for(int j=1; j <= Pin_t.Height; j++)
				{           
					for(int k=1; k <= Pin_t.Width; k++)
					{      
 
							 Gx = Sobel_Filter_x[1][1]*IMG_temp_matrix_2[k][j]        
											 + Sobel_Filter_x[2][1]*IMG_temp_matrix_2[k+1][j]
											 + Sobel_Filter_x[0][1]*IMG_temp_matrix_2[k-1][j]
											 + Sobel_Filter_x[1][2]*IMG_temp_matrix_2[k][j+1]
											 + Sobel_Filter_x[1][0]*IMG_temp_matrix_2[k][j-1]
											 + Sobel_Filter_x[0][0]*IMG_temp_matrix_2[k-1][j-1]
											 + Sobel_Filter_x[2][0]*IMG_temp_matrix_2[k+1][j-1]
											 + Sobel_Filter_x[0][2]*IMG_temp_matrix_2[k-1][j+1]
											 + Sobel_Filter_x[2][2]*IMG_temp_matrix_2[k+1][j+1];
 
 							 Gy = Sobel_Filter_y[1][1]*IMG_temp_matrix_2[k][j]        
											 + Sobel_Filter_y[2][1]*IMG_temp_matrix_2[k+1][j]
											 + Sobel_Filter_y[0][1]*IMG_temp_matrix_2[k-1][j]
											 + Sobel_Filter_y[1][2]*IMG_temp_matrix_2[k][j+1]
											 + Sobel_Filter_y[1][0]*IMG_temp_matrix_2[k][j-1]
											 + Sobel_Filter_y[0][0]*IMG_temp_matrix_2[k-1][j-1]
											 + Sobel_Filter_y[2][0]*IMG_temp_matrix_2[k+1][j-1]
											 + Sobel_Filter_y[0][2]*IMG_temp_matrix_2[k-1][j+1]
											 + Sobel_Filter_y[2][2]*IMG_temp_matrix_2[k+1][j+1];
						   
 
							// Compute gradient value
 
							//IMG__gradient_matrix[k-1][j-1] = sqrt(pow(Gx,2.0) + pow(Gy,2.0));
							IMG__gradient_matrix[0][0] = 0;				 
							IMG__gradient_matrix[k][j] = sqrt(pow(Gx,2.0) + pow(Gy,2.0));
 
							// Calculate direction of edge
 
							tempAngle = (atan2((float)Gx, (float)Gy)/3.14159) * 180.0;		
			
							// Convert edge direction to approximate value //
 
							if ( ( (tempAngle < 22.5) && (tempAngle > -22.5) ) || (tempAngle > 157.5) || (tempAngle < -157.5) )
								finalAngle = 0;
							if ( ( (tempAngle > 22.5) && (tempAngle < 67.5) ) || ( (tempAngle < -112.5) && (tempAngle > -157.5) ) )
								finalAngle = 45;
							if ( ( (tempAngle > 67.5) && (tempAngle < 112.5) ) || ( (tempAngle < -67.5) && (tempAngle > -112.5) ) )
								finalAngle = 90;
							if ( ( (tempAngle > 112.5) && (tempAngle < 157.5) ) || ( (tempAngle < -22.5) && (tempAngle > -67.5) ) )
								finalAngle = -45;
 
							IMG__direction_matrix[k-1][j-1] = finalAngle;
 
					} 
				}
		/*pixelCnt = 0;
        for(int j = 0; j < Pin_t.Height; j++)
        {
              for(int k = 0; k < Pin_t.Width; k++)
              {
                    Pin_t.img[pixelCnt] = (unsigned char) ( IMG__direction_matrix[k][j]);
                    pixelCnt++;
              }
         }
		WritePGM(outfile_Pout,Pin_t);
		*/
 
		// Perform Nonmaxima suppression 
 
		for(int j=0; j < Pin_t.Height; j++)
				{           
					for(int k=0; k < Pin_t.Width; k++)
					{  
 
						switch (IMG__direction_matrix[k][j]){		
							
							case 0:
								
								if(j == 0)
								{
									if(IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k][j+1])
									{
										IMG__gradient_matrix[k][j] = 0;
									}
 
								}
 
								else if( j == (Pin_t.Height - 1))
								{
									if(IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k][j-1])
									{
										IMG__gradient_matrix[k][j] = 0;
									}
								}
 
								else
								{
 
									if((IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k][j-1]) || (IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k][j+1]))
									{
										IMG__gradient_matrix[k][j] = 0;
									}
 
								}
 
								break;
 
							case 45:
								
								if((k == 0) && (j == (Pin_t.Height - 1)))
								{
									if(IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k+1][j-1])
									{
										IMG__gradient_matrix[k][j] = 0;
									}
								}
 
								else if((k == Pin_t.Width -1) && (j == 0))
								{
									if(IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k-1][j+1])
									{
										IMG__gradient_matrix[k][j] = 0;
									}
								}
 
								/*else
								{
 
									if((IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k-1][j+1]) || (IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k+1][j-1]))
									{
										IMG__gradient_matrix[k][j] = 0;
									}
 
								}*/
							
 
								break;
 
							case 90:
								
								if(k == 0)
								{
									if(IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k+1][j])
									{
										IMG__gradient_matrix[k][j] = 0;
									}
 
								}
 
								else if( k == (Pin_t.Width - 1))
								{
									if(IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k+1][j])
									{
										IMG__gradient_matrix[k][j] = 0;
									}
								}
 
								else
								{
 
									if((IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k-1][j]) || (IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k-1][j]))
									{
										IMG__gradient_matrix[k][j] = 0;
									}
 
								}
								break;
 
							case -45:
 
								if((k == 0) && (j == 0))
								{
									if(IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k+1][j+1])
									{
										IMG__gradient_matrix[k][j] = 0;
									}
								}
 
								else if((k == Pin_t.Width -1) && (j == (Pin_t.Height -1)))
								{
									if(IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k-1][j-1])
									{
										IMG__gradient_matrix[k][j] = 0;
									}
								}
 
								else
								{
 
									if((IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k+1][j+1]) || (IMG__gradient_matrix[k][j] < IMG__gradient_matrix[k-1][j-1]))
									{
										IMG__gradient_matrix[k][j] = 0;
									}
 
								}
 
								break;
							}
						
							if(IMG__gradient_matrix[k][j] >= upperThresh)
							{
								IMG__gradient_matrix[k][j] = 255;
							}
							else if (IMG__gradient_matrix[k][j] <= lowerThresh)
							{
								IMG__gradient_matrix[k][j] = 0;
								
 
							}
							
							/*if((IMG__gradient_matrix[k][j+1] == 255)||(IMG__gradient_matrix[k][j+2] == 255)||(IMG__gradient_matrix[k][j+3] == 255))
								{
									IMG__gradient_matrix[k][j] = 255;
								}
							*/	
 
 
							/*else
							{ 
								if((IMG__gradient_matrix[k-1][j-1] > upperThresh ) || (IMG__gradient_matrix[k][j-1] > upperThresh) || (IMG__gradient_matrix[k+1][j+1] > upperThresh) || (IMG__gradient_matrix[k+1][j] > upperThresh) || (IMG__gradient_matrix[k+1][j+1] > upperThresh) || (IMG__gradient_matrix[k][j+1] > upperThresh) || (IMG__gradient_matrix[k-1][j-1] > upperThresh) || (IMG__gradient_matrix[k-1][j] > upperThresh))
							{
								IMG__gradient_matrix[k][j] = 255;
							}
								else 
								{
									IMG__gradient_matrix[k][j] = 0;
								}
							}
							*/
 
 
					}
 
		}
 
		// Insert Hysterisys Algorithm here //
	/*	int f,sts;
		sts=0;
		f=0;
		int knumber[2000];
			for(int k=0; k < Pin_t.Width; k++)
				{           
					
					for(int st=0;st<2000;st++)
					{
						knumber[st]=0;
					}
					for(int j=0; j < Pin_t.Height; j++)
					{  
							if(IMG__gradient_matrix[k][j]==255)	
							{
								knumber[sts]= j;
								sts++;
				
							}
							
							for(int pra = 0;pra<sts;pra++)
							{
								if( ((knumber[pra+1]-knumber[pra])>3) &&((knumber[pra+1]-knumber[pra])<5) )
								{
									f = knumber[pra];
									IMG__gradient_matrix[k][f]=255;
								}
								else
								continue;
							}
							
					}
			}
      */  
        //output the calculated 2D matrix to original 1D
		numberpixel = 0;
        pixelCnt = 0;
		int f,sts;
		sts=0;
		int knumber[300];
		//for(int st=0;st<300;st++)
		//		{
		//				knumber[st]=0;
		//			}
        for(int j = 0; j < Pin_t.Height; j++)
        {
			
              for(int k = 0; k < Pin_t.Width; k++)
              {		
					if ((IMG__gradient_matrix[k][j]==255)&&(IMG__gradient_matrix[k+1][j]==255)&&(IMG__gradient_matrix[k+2][j]==255) && (IMG__gradient_matrix[k+3][j]==255) && (IMG__gradient_matrix[k+4][j]==255)&& (IMG__gradient_matrix[k+5][j]==255))// && (IMG__gradient_matrix[k-1][j+1]==255))
					{
						numberpixel ++;
						if((numberpixel > 50) && (numberpixel <150))
						{
						//knumber[sts] = j;
						//sts++;
						landmark = true;
						
						//CPoint p5(577+xL[0]*0.1,467+yL[0]*0.1);
						//dc.MoveTo(lastpoint11);
						//dc.LineTo(p5);
						//lastpoint11=p5;
						
						
					/*	CPoint p6(510+xL[0]*0.5,510-yL[0]*0.2);
						//blue landmark
						
						dc.SetPixel(511+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
						dc.SetPixel(509+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
						dc.SetPixel(512+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
						dc.SetPixel(508+xL[0]*0.5,520-yL[0]*0.2,RGB(0,0,255));
						dc.SetPixel(510+xL[0]*0.5,521-yL[0]*0.2,RGB(0,0,255));
						dc.SetPixel(510+xL[0]*0.5,522-yL[0]*0.2,RGB(0,0,255));
						dc.SetPixel(510+xL[0]*0.5,518-yL[0]*0.2,RGB(0,0,255));
						dc.SetPixel(510+xL[0]*0.5,519-yL[0]*0.2,RGB(0,0,255));
 
						dc.MoveTo(lastpoint11);
						dc.LineTo(p6);
						lastpoint11=p6;
						*/
											}
 
						/*if ((numberpixel > 150)&& (numberpixel < 600))
							{
								IMG__gradient_matrix[k][j]=0;
								landmark = true;	
							}
							*/							
						if (landmark)
							break;
					}
					
					
                    Pin_t.img[pixelCnt] = (unsigned char) (IMG__gradient_matrix[k][j]);
                    pixelCnt++;
              }
         }
		// outfile_Pout.nChannel = 1;
        //outfile_Pout.InterLeaved = false;
 
 	//outfile_Pout.close();
        WritePGM(outfile_Pout, Pin_t);
	
		
	//delete []knumber;
	//knumber=NULL;
 
 }
void CI90ControllerDlg::TurnDegree(double degree, short time)
{
	double Dis = WheelDis/2*(degree/180.0*3.14);
	m_Encoder1 = m_MOTSDK.GetEncoderPulse1 ();
	m_Encoder2 = m_MOTSDK.GetEncoderPulse2 ();
 
	int diffEncoder = (int) (Dis / (2*3.14*wheelR) *  CIRCLE_CNT);
 
	int LeftCmd = m_Encoder1 - diffEncoder;
	
		if(LeftCmd < 0)
		{
			LeftCmd = 32767 + LeftCmd;
		}
 
		else if (LeftCmd > 32767)
		{
			LeftCmd = LeftCmd - 32767;
		}
 
		int RightCmd = m_Encoder2 - diffEncoder;
 
			if(RightCmd < 0)
		{
			RightCmd = 32767 + RightCmd;
		}
 
		else if (RightCmd > 32767)
		{
			RightCmd = RightCmd - 32767;
		}
		m_MOTSDK.SetDcMotorPositionControlPID(0,1000,5,10000);
		m_MOTSDK.SetDcMotorPositionControlPID(1,1000,5,10000);
		
		m_MOTSDK.DcMotorPositionTimeCtrAll (short(LeftCmd),short(RightCmd),NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,time);
		
 
}
void CI90ControllerDlg::DriveForward(double Dis, short time)
{
	int diffEncoder = (int) (Dis / (2*3.14*wheelR) *  CIRCLE_CNT);
	int LeftCmd = m_Encoder1 - diffEncoder;
	
		if(LeftCmd < 0)
		{
			LeftCmd = 32767 + LeftCmd;
		}
 
		else if (LeftCmd > 32767)
		{
			LeftCmd = LeftCmd - 32767;
		}
 
		int RightCmd = m_Encoder2 + diffEncoder;
 
			if(RightCmd < 0)
		{
			RightCmd = 32767 + RightCmd;
		}
 
		else if (RightCmd > 32767)
		{
			RightCmd = RightCmd - 32767;
		}
		m_MOTSDK.DcMotorPositionTimeCtrAll (short(LeftCmd),short(RightCmd),NO_CONTROL,NO_CONTROL,NO_CONTROL,NO_CONTROL,time);
}

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:
125:
126:
127:
128:
129:
130:
131:
132:
133:
134:
135:
136:
137:
138:
139:
140:
141:
142:
143:
144:
145:
146:
147:
148:
149:
150:
151:
152:
153:
154:
155:
156:
157:
158:
159:
160:
161:
162:
163:
164:
165:
166:
167:
168:
169:
170:
171:
172:
173:
174:
175:
176:
177:
178:
179:
180:
181:
182:
183:
184:
185:
186:
187:
188:
189:
190:
191:
192:
193:
194:
195:
196:
197:
198:
199:
200:
201:
202:
203:
204:
205:
206:
207:
208:
209:
210:
211:
212:
213:
214:
215:
216:
217:
218:
219:
220:
221:
222:
223:
224:
225:
226:
227:
228:
229:
230:
231:
232:
233:
234:
235:
236:
237:
238:
239:
240:
241:
242:
243:
244:
245:
246:
247:
248:
249:
250:
251:
252:
253:
254:
255:
256:
257:
258:
259:
260:
261:
262:
263:
264:
265:
266:
267:
268:
269:
270:
271:
272:
273:
274:
275:
276:
277:
278:
279:
280:
281:
282:
283:
284:
285:
286:
287:
288:
289:
290:
291:
292:
293:
294:
295:
296:
297:
298:
299:
300:
301:
302:
303:
304:
305:
306:
307:
308:
309:
310:
311:
312:
313:
314:
315:
316:
317:
318:
319:
320:
321:
322:
323:
324:
325:
326:
327:
328:
329:
330:
331:
332:
333:
334:
335:
336:
337:
338:
339:
340:
341:
342:
343:
344:
345:
346:
347:
348:
349:
350:
351:
352:
353:
354:
355:
356:
357:
358:
359:
360:
361:
362:
363:
364:
365:
366:
367:
368:
369:
370:
371:
372:
373:
374:
375:
376:
377:
378:
379:
380:
381:
382:
383:
384:
385:
386:
387:
388:
389:
390:
391:
392:
393:
394:
395:
396:
397:
398:
399:
400:
401:
402:
403:
404:
405:
406:
407:
408:
409:
410:
411:
412:
413:
414:
415:
416:
417:
418:
419:
420:
421:
422:
423:
424:
425:
426:
427:
428:
429:
430:
431:
432:
433:
434:
435:
436:
437:
438:
439:
440:
441:
442:
443:
444:
445:
446:
447:
448:
449:
450:
451:
452:
453:
454:
455:
456:
457:
458:
459:
460:
461:
462:
463:
464:
465:
466:
467:
468:
469:
470:
471:
472:
473:
474:
475:
476:
477:
478:
479:
480:
481:
482:
483:
484:
485:
486:
487:
488:
489:
490:
491:
492:
493:
494:
495:
496:
497:
498:
499:
500:
501:
502:
503:
504:
505:
506:
507:
508:
509:
510:
511:
512:
513:
514:
515:
516:
517:
518:
519:
520:
521:
522:
523:
524:
525:
526:
527:
528:
529:
530:
531:
532:
533:
534:
535:
536:
537:
538:
539:
540:
541:
542:
543:
544:
545:
546:
547:
548:
549:
550:
551:
552:
553:
554:
555:
556:
557:
558:
559:
560:
561:
562:
563:
564:
565:
566:
567:
568:
569:
570:
571:
572:
573:
574:
575:
576:
577:
578:
579:
580:
581:
582:
583:
584:
585:
586:
587:
588:
589:
590:
591:
592:
593:
594:
595:
596:
597:
598:
599:
600:
601:
602:
603:
604:
605:
606:
607:
608:
609:
610:
611:
612:
613:
614:
615:
616:
617:
618:
619:
620:
621:
622:
623:
624:
625:
626:
627:
628:
629:
630:
631:
632:
633:
634:
635:
636:
637:
638:
639:
640:
641:
642:
643:
644:
645:
646:
647:
648:
649:
650:
651:
652:
653:
654:
655:
656:
657:
658:
659:
660:
661:
662:
663:
664:
665:
666:
667:
668:
669:
670:
671:
672:
673:
674:
675:
676:
677:
678:
679:
680:
681:
682:
683:
684:
685:
686:
687:
688:
689:
690:
691:
692:
693:
694:
695:
696:
697:
698:
699:
700:
701:
702:
703:
704:
705:
706:
707:
708:
709:
710:
711:
712:
713:
714:
715:
716:
717:
718:
719:
720:
721:
722:
723:
724:
725:
726:
727:
728:
729:
730:
731:
732:
733:
734:
735:
736:
737:
738:
739:
740:
741:
742:
743:
744:
745:
746:
747:
748:
749:
750:
751:
752:
753:
754:
755:
756:
757:
758:
759:
760:
761:
762:
763:
764:
765:
766:
767:
768:
769:
770:
771:
772:
773:
774:
775:
776:
777:
778:
779:
780:
781:
782:
783:
784:
785:
786:
787:
788:
789:
790:
791:
792:
793:
794:
795:
796:
797:
798:
799:
800:
801:
802:
803:
804:
805:
806:
807:
808:
809:
810:
811:
812:
813:
814:
815:
816:
817:
818:
819:
820:
821:
822:
823:
824:
825:
826:
827:
828:
829:
830:
831:
832:
833:
834:
835:
836:
837:
838:
839:
840:
841:
842:
843:
844:
845:
846:
847:
848:
849:
850:
851:
852:
853:
854:
855:
856:
857:
858:
859:
860:
861:
862:
863:
864:
865:
866:
867:
868:
869:
870:
871:
872:
873:
874:
875:
876:
877:
878:
879:
880:
881:
882:
883:
884:
885:
886:
887:
888:
889:
890:
891:
892:
893:
894:
895:
896:
897:
898:
899:
900:
901:
902:
903:
904:
905:
906:
907:
908:
909:
910:
911:
912:
913:
914:
915:
916:
917:
918:
919:
920:
921:
922:
923:
924:
925:
926:
927:
928:
929:
930:
931:
932:
933:
934:
935:
936:
937:
938:
939:
940:
941:
942:
943:
944:
945:
946:
947:
948:
949:
950:
951:
952:
953:
954:
955:
956:
957:
958:
959:
960:
961:
962:
963:
964:
965:
966:
967:
968:
969:
970:
971:
972:
973:
974:
975:
976:
977:
978:
979:
980:
981:
982:
983:
984:
985:
986:
987:
988:
989:
990:
991:
992:
993:
994:
995:
996:
997:
998:
999:
1000:
1001:
1002:
1003:
1004:
1005:
1006:
1007:
1008:
1009:
1010:
1011:
1012:
1013:
1014:
1015:
1016:
1017:
1018:
1019:
1020:
1021:
1022:
1023:
1024:
1025:
1026:
1027:
1028:
1029:
1030:
1031:
1032:
1033:
1034:
1035:
1036:
1037:
1038:
1039:
1040:
1041:
1042:
1043:
1044:
1045:
1046:
1047:
1048:
1049:
1050:
1051:
1052:
1053:
1054:
1055:
1056:
1057:
1058:
1059:
1060:
1061:
1062:
1063:
1064:
1065:
1066:
1067:
1068:
1069:
1070:
1071:
1072:
1073:
1074:
1075:
1076:
1077:
1078:
1079:
1080:
1081:
1082:
1083:
1084:
1085:
1086:
1087:
1088:
1089:
1090:
1091:
1092:
1093:
1094:
1095:
1096:
1097:
1098:
1099:
1100:
1101:
1102:
1103:
1104:
1105:
1106:
1107:
1108:
1109:
1110:
1111:
1112:
1113:
1114:
1115:
1116:
1117:
1118:
1119:
1120:
1121:
1122:
1123:
1124:
1125:
1126:
1127:
1128:
1129:
1130:
1131:
1132:
1133:
1134:
1135:
1136:
1137:
1138:
1139:
1140:
1141:
1142:
1143:
1144:
1145:
1146:
1147:
1148:
1149:
1150:
1151:
1152:
1153:
1154:
1155:
1156:
1157:
1158:
1159:
1160:
1161:
1162:
1163:
1164:
1165:
1166:
1167:
1168:
1169:
1170:
1171:
1172:
1173:
1174:
1175:
1176:
1177:
1178:
1179:
1180:
1181:
1182:
1183:
1184:
1185:
1186:
1187:
1188:
1189:
1190:
1191:
1192:
1193:
1194:
1195:
1196:
1197:
1198:
1199:
1200:
1201:
1202:
1203:
1204:
1205:
1206:
1207:
1208:
1209:
1210:
1211:
1212:
1213:
1214:
1215:
1216:
1217:
1218:
1219:
1220:
1221:
1222:
1223:
1224:
1225:
1226:
1227:
1228:
1229:
1230:
1231:
1232:
1233:
1234:
1235:
1236:
1237:
1238:
1239:
1240:
1241:
1242:
1243:
1244:
1245:
1246:
1247:
1248:
1249:
1250:
1251:
1252:
1253:
1254:
1255:
1256:
1257:
1258:
1259:
1260:
1261:
1262:
1263:
1264:
1265:
1266:
1267:
1268:
1269:
1270:
1271:
1272:
1273:
1274:
1275:
1276:
1277:
1278:
1279:
1280:
1281:
1282:
1283:
1284:
1285:
1286:
1287:
1288:
1289:
1290:
1291:
1292:
1293:
1294:
1295:
1296:
1297:
1298:
1299:
1300:
1301:
1302:
1303:
1304:
1305:
1306:
1307:
1308:
1309:
1310:
1311:
1312:
1313:
1314:
1315:
1316:
1317:
1318:
1319:
1320:
1321:
1322:
1323:
1324:
1325:
1326:
1327:
1328:
1329:
1330:
1331:
1332:
1333:
1334:
1335:
1336:
1337:
1338:
1339:
1340:
1341:
1342:
1343:
1344:
1345:
1346:
1347:
1348:
1349:
1350:
1351:
1352:
1353:
1354:
1355:
1356:
1357:
1358:
1359:
1360:
1361:
1362:
1363:
1364:
1365:
1366:
1367:
1368:
1369:
1370:
1371:
1372:
1373:
1374:
1375:
1376:
1377:
1378:
1379:
1380:
1381:
1382:
1383:
1384:
1385:
1386:
1387:
1388:
1389:
1390:
1391:
1392:
1393:
1394:
1395:
1396:
1397:
1398:
1399:
1400:
1401:
1402:
1403:
1404:
1405:
1406:
1407:
1408:
1409:
1410:
1411:
1412:
1413:
1414:
1415:
1416:
1417:
1418:
1419:
1420:
1421:
1422:
1423:
1424:
1425:
1426:
1427:
1428:
1429:
1430:
1431:
1432:
1433:
1434:
1435:
1436:
1437:
1438:
1439:
1440:
1441:
1442:
1443:
1444:
1445:
1446:
1447:
1448:
1449:
1450:
1451:
1452:
1453:
1454:
1455:
1456:
1457:
1458:
1459:
1460:
1461:
1462:
1463:
1464:
1465:
1466:
1467:
1468:
1469:
1470:
1471:
1472:
1473:
1474:
1475:
1476:
1477:
1478:
1479:
1480:
1481:
1482:
1483:
1484:
1485:
1486:
1487:
1488:
1489:
1490:
1491:
1492:
1493:
1494:
1495:
1496:
1497:
1498:
1499:
1500:
1501:
1502:
1503:
1504:
1505:
1506:
1507:
1508:
1509:
1510:
1511:
1512:
1513:
1514:
1515:
1516:
1517:
1518:
1519:
1520:
1521:
1522:
1523:
1524:
1525:
1526:
1527:
1528:
1529:
1530:
1531:
1532:
1533:
1534:
1535:
1536:
1537:
1538:
1539:
1540:
1541:
1542:
1543:
1544:
1545:
1546:
1547:
1548:
1549:
1550:
1551:
1552:
1553:
1554:
1555:
1556:
1557:
1558:
1559:
1560:
1561:
1562:
1563:
1564:
1565:
1566:
1567:
1568:
1569:
1570:
1571:
1572:
1573:
1574:
1575:
1576:
1577:
1578:
1579:
1580:
1581:
1582:
1583:
1584:
1585:
1586:
1587:
1588:
1589:
1590:
1591:
1592:
1593:
1594:
1595:
1596:
1597:
1598:
1599:
1600:
1601:
1602:
1603:
1604:
1605:
1606:
1607:
1608:
1609:
1610:
1611:
1612:
1613:
1614:
1615:
1616:
1617:
1618:
1619:
1620:
1621:
1622:
1623:
1624:
1625:
1626:
1627:
1628:
1629:
1630:
1631:
1632:
1633:
1634:
1635:
1636:
1637:
1638:
1639:
1640:
1641:
1642:
1643:
1644:
1645:
1646:
1647:
1648:
1649:
1650:
1651:
1652:
1653:
1654:
1655:
1656:
1657:
1658:
1659:
1660:
1661:
1662:
1663:
1664:
1665:
1666:
1667:
1668:
1669:
1670:
1671:
1672:
1673:
1674:
1675:
1676:
1677:
1678:
1679:
1680:
1681:
1682:
1683:
1684:
1685:
1686:
1687:
1688:
1689:
1690:
1691:
1692:
1693:
1694:
1695:
1696:
1697:
1698:
1699:
1700:
1701:
1702:
1703:
1704:
1705:
1706:
1707:
1708:
1709:
1710:
1711:
1712:
1713:
1714:
1715:
1716:
1717:
1718:
1719:
1720:
1721:
1722:
1723:
1724:
1725:
1726:
1727:
1728:
1729:
1730:
1731:
1732:
1733:
1734:
1735:
1736:
1737:
1738:
1739:
1740:
1741:
1742:
1743:
1744:
1745:
1746:
1747:
1748:
1749:
1750:
1751:
1752:
1753:
1754:
1755:
1756:
1757:
1758:
1759:
1760:
1761:
1762:
1763:
1764:
1765:
1766:
1767:
1768:
1769:
1770:
1771:
1772:
1773:
1774:
1775:
1776:
1777:
1778:
1779:
1780:
1781:
1782:
1783:
1784:
1785:
1786:
1787:
1788:
1789:
1790:
1791:
1792:
1793:
1794:
1795:
1796:
1797:
1798:
1799:
1800:
1801:
1802:
1803:
1804:
1805:
1806:
1807:
1808:
1809:
1810:
1811:
1812:
1813:
1814:
1815:
1816:
1817:
1818:
1819:
1820:
1821:
1822:
1823:
1824:
1825:
1826:
1827:
1828:
1829:
1830:
1831:
1832:
1833:
1834:
1835:
1836:
1837:
1838:
1839:
1840:
1841:
1842:
1843:
1844:
1845:
1846:
1847:
1848:
1849:
1850:
1851:
1852:
1853:
1854:
1855:
1856:
1857:
1858:
1859:
1860:
1861:
1862:
1863:
1864:
1865:
1866:
1867:
1868:
1869:
1870:
1871:
1872:
1873:
1874:
1875:
1876:
1877:
1878:
1879:
1880:
1881:
1882:
1883:
1884:
1885:
1886:
1887:
1888:
1889:
1890:
1891:
1892:
1893:
1894:
1895:
1896:
1897:
1898:
1899:
1900:
1901:
1902:
1903:
1904:
1905:
1906:
1907:
1908:
1909:
1910:
1911:
1912:
1913:
1914:
1915:
1916:
1917:
1918:
1919:
1920:
1921:
1922:
1923:
1924:
1925:
1926:
1927:
1928:
1929:
1930:
1931:
1932:
1933:
1934:
1935:
1936:
1937:
1938:
1939:
1940:
1941:
1942:
1943:
1944:
1945:
1946:
1947:
1948:
1949:
1950:
1951:
1952:
1953:
1954:
1955:
1956:
1957:
1958:
1959:
1960:
1961:
1962:
1963:
1964:
1965:
1966:
1967:
1968:
1969:
1970:
1971:
1972:
1973:
1974:
1975:
1976:
1977:
1978:
1979:
1980:
1981:
1982:
1983:
1984:
1985:
1986:
1987:
1988:
1989:
1990:
1991:
1992:
1993:
1994:
1995:
1996:
1997:
1998:
1999:
2000:
2001:
2002:
2003:
2004:
2005:
2006:
2007:
2008:
2009:
2010:
2011:
2012:
2013:
2014:
2015:
2016:
2017:
2018:
2019:
2020:
2021:
2022:
2023:
2024:
2025:
2026:
2027:
2028:
2029:
2030:
2031:
2032:
2033:
2034:
2035:
2036:
2037:
2038:
2039:
2040:
2041:
2042:
2043:
2044:
2045:
2046:
2047:
2048:
2049:
2050:
2051:
2052:
2053:
2054:
2055:
2056:
2057:
2058:
2059:
2060:
2061:
2062:
2063:
2064:
2065:
2066:
2067:
2068:
2069:
2070:
2071:
2072:
2073:
2074:
2075:
2076:
2077:
2078:
2079:
2080:
2081:
2082:
2083:
2084:
2085:
2086:
2087:
2088:
2089:
2090:
2091:
2092:
2093:
2094:
2095:
2096:
2097:
2098:
2099:
2100:
2101:
2102:
2103:
2104:
2105:
2106:
2107:
2108:
2109:
2110:
2111:
2112:
2113:
2114:
2115:
2116:
2117:
2118:
2119:
2120:
2121:
2122:
2123:
2124:
2125:
2126:
2127:
2128:
2129:
2130:
2131:
2132:
2133:
2134:
2135:
2136:
2137:
2138:
2139:
2140:
2141:
2142:
2143:
2144:
2145:
2146:
2147:
2148:
2149:
2150:
2151:
2152:
2153:
2154:
2155:
2156:
2157:
2158:
2159:
2160:
2161:
2162:
2163:
2164:
2165:
2166:
2167:
2168:
2169:
2170:
2171:
2172:
2173:
2174:
2175:
2176:
2177:
2178:
2179:
2180:
2181:
2182:
2183:
2184:
2185:
2186:
2187:
2188:
2189:
2190:
2191:
2192:
2193:
2194:
2195:
2196:
2197:
2198:
2199:
2200:
2201:
2202:
2203:
2204:
2205:
2206:
2207:
2208:
2209:
2210:
2211:
2212:
2213:
2214:
2215:
2216:
2217:
2218:
2219:
2220:
2221:
2222:
2223:
2224:
2225:
2226:
2227:
2228:
2229:
2230:
2231:
2232:
2233:
2234:
2235:
2236:
2237:
2238:
2239:
2240:
2241:
2242:
2243:
2244:
2245:
2246:
2247:
2248:
2249:
2250:
2251:
2252:
2253:
2254:
2255:
2256:
2257:
2258:
2259:
2260:
2261:
2262:
2263:
2264:
2265:
2266:
2267:
2268:
2269:
2270:
2271:
2272:
2273:
2274:
2275:
2276:
2277:
2278:
2279:
2280:
2281:
2282:
2283:
2284:
2285:
2286:
2287:
2288:
2289:
2290:
2291:
2292:
2293:
2294:
2295:
2296:
2297:
2298:
2299:
2300:
2301:
2302:
2303:
2304:
2305:
2306:
2307:
2308:
2309:
2310:
2311:
2312:
2313:
2314:
2315:
2316:
2317:
2318:
2319:
2320:
2321:
2322:
2323:
2324:
2325:
2326:
2327:
2328:
2329:
2330:
2331:
2332:
2333:
2334:
2335:
2336:
2337:
2338:
2339:
2340:
2341:
2342:
2343:
2344:
2345:
2346:
2347:
2348:
2349:
2350:
2351:
2352:
2353:
2354:
2355:
2356:
2357:
2358:
2359:
2360:
2361:
2362:
2363:
2364:
2365:
2366:
2367:
2368:
2369:
2370:
2371:
2372:
2373:
2374:
2375:
2376:
2377:
2378:
2379:
2380:
2381:
2382:
2383:
2384:
2385:
2386:
2387:
2388:
2389:
2390:
2391:
2392:
2393:
2394:
2395:
2396:
2397:
2398:
2399:
2400:
2401:
2402:
2403:
2404:
2405:
2406:
2407:
2408:
2409:
2410:
2411:
2412:
2413:
2414:
2415:
2416:
2417:
2418:
2419:
2420:
2421:
2422:
2423:
2424:
2425:
2426:
2427:
2428:
2429:
2430:
2431:
2432:
2433:
2434:
2435:
2436:
2437:
2438:
2439:
2440:
2441:
2442:
2443:
2444:
2445:
2446:
2447:
2448:
2449:
2450:
2451:
2452:
2453:
2454:
2455:
2456:
2457:
2458:
2459:
2460:
2461:
2462:
2463:
2464:
2465:
2466:
2467:
2468:
2469:
2470:
2471:
2472:
2473:
2474:
2475:
2476:
2477:
2478:
2479:
2480:
2481:
2482:
2483:
2484:
2485:
2486:
2487:
2488:
2489:
2490:
2491:
2492:
2493:
2494:
2495:
2496:
2497:
2498:
2499:
2500:
2501:
2502:
2503:
2504:
2505:
2506:
2507:
2508:
2509:
2510:
2511:
2512:
2513:
2514:
2515:
2516:
2517:
2518:
2519:
2520:
2521:
2522:
2523:
2524:
2525:
2526:
2527:
2528:
2529:
2530:
2531:
2532:
2533:
2534:
2535:
2536:
2537:
2538:
2539:
2540:
2541:
2542:
2543:
2544:
2545:
2546:
2547:
2548:
2549:
2550:
2551:
2552:
2553:
2554:
2555:
2556:
2557:
2558:
2559:
2560:
2561:
2562:
2563:
2564:
2565:
2566:
2567:
2568:
2569:
2570:
2571:
2572:
2573:
2574:
2575:
2576:
2577:
2578:
2579:
2580:
2581:
2582:
2583:
2584:
2585:
2586:
2587:
2588:
2589:
2590:
2591:
2592:
2593:
2594:
2595:
2596:
2597:
2598:
2599:
2600:
2601:
2602:
2603:
2604:
2605:
2606:
2607:
2608:
2609:
2610:
2611:
2612:
2613:
2614:
2615:
2616:
2617:
2618:
2619:
2620:
2621:
2622:
2623:
2624:
2625:
2626:
2627:
2628:
2629:
2630:
2631:
2632:
2633:
2634:
2635:
2636:
2637:
2638:
2639:
2640:
2641:
2642:
2643:
2644:
2645:
2646:
2647:
2648:
2649:
2650:
2651:
2652:
2653:
2654:
2655:
2656:
2657:
2658:
2659:
2660:
2661:
2662:
2663:
2664:
2665:
2666:
2667:
2668:
2669:
2670:
2671:

// Copyright (C) 2003
// Gerhard Neumann (gerhard@igi.tu-graz.ac.at)
 
//                
// This file is part of RL Toolbox.
// http://www.igi.tugraz.at/ril_toolbox
//
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "stdafx.h"
#include "ril_debug.h"
#include "cgridworldmodel.h"
 
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string>
#include <stdexcept>
 
CGridWorld::CGridWorld(char* filename)
{
	
	start_values = new std::set<char>();
	target_values = new std::set<char>();
	prohibited_values = new std::set<char>();
	grid = new std::vector<char *>();
	size_x = 0;
	size_y = 0;
	load(filename);
	
}
 
CGridWorld::CGridWorld(unsigned int size_x, unsigned int size_y)
{
	
	start_values = new std::set<char>();
	target_values = new std::set<char>();
	prohibited_values = new std::set<char>();
	grid = new std::vector<char *>();
	this->size_x = size_x;
	this->size_y = size_y;
	initGrid();
	
}
 
CGridWorld::~CGridWorld()
{
	deallocGrid();
	start_values->clear();
	delete start_values;
	target_values->clear();
	delete target_values;
	prohibited_values->clear();
	delete prohibited_values;
	delete grid;
}
 
void CGridWorld::allocGrid()
{
	deallocGrid();
    for (int i = 0; i < size_y; i++)
	{
		grid->push_back(new char[size_x]);
	}
	is_allocated = true;
}
 
void CGridWorld::deallocGrid()
{
    if (is_allocated)
	{
		for (int i = 0; i < size_y; i++)
		{
			delete (*grid)[i];
		}
		grid->clear();
		is_allocated = false;
	}
}
 
void CGridWorld::initGrid()
{
	if (!is_allocated && size_x > 0 && size_y > 0)
	{
		allocGrid();
	}
}
 
bool CGridWorld::isValid()
{
	return is_allocated;
}
 
void CGridWorld::load(char* filename)
{
    FILE* stream = fopen(filename, "r");
	if (!stream) 
	{
		throw new std::runtime_error("Gridworld: Datei konnte nicht gefunden werden!");
	}
	load(stream);
	fclose(stream);
}
 
void CGridWorld::load(FILE *stream)
{
    int xsize = 0, ysize = 0, i = 0;
	bool flg_grid_world = false;
	char buffer[1024];
	std::string text;
	std::set<char>* tmpset;
	char temp;
 
    if (fgets(buffer, 1024, stream) == NULL)
	{
		throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltige Gridworld! (unerwartetes Dateiende)");
	}
    
	while (!((buffer[0] <= 57 && buffer[0] >= 48) || (buffer[0] <= 70 && buffer[0] >= 65))) // != 1..9,A..F
	{
		if (!(buffer[0] == '#' || buffer[0] == 13 || buffer[0] == 10)) // ignore # and enter
		{
			text = buffer;
			if(text.find("Gridworld") == 0)
			{
				flg_grid_world = true;
			}
			else if (text.find("Size: ") == 0)
			{
				if (sscanf(buffer, "Size: %dx%d", &xsize, &ysize) != 2)
					throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltigen Gridworldgroessenangaben!");
				this->size_x = xsize;
				this->size_y = ysize;
			}
			else
			{
				if (text.find("StartValues: ") == 0)
				{
					tmpset = start_values;
					i = strlen("StartValues:");
				}
				else if (text.find("TargetValues: ") == 0)
				{
					tmpset = target_values;
					i = strlen("TargetValues:");
				}
				else if (text.find("ProhibitedValues: ") == 0)
				{
					tmpset = prohibited_values;
					i = strlen("ProhibitedValues:");
				}
				else 
				{
					throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltige Gridworld! (ungueltiges Token)");
				}
 
				while (i > 0)
				{
					if (sscanf(text.substr(i + 1).c_str(), "%c", &temp) != 1)
					{
						throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltige Gridworld! (ungueltiger Wert)");
					}
					tmpset->insert((char)temp);
					i = text.find(",", i + 1);
				}
			}
		}
 
		if (fgets(buffer, 1024, stream) == NULL)
			throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltige Gridworld! (unerwartetes Dateiende)");
	}
 
    if (!flg_grid_world)
		throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltige Gridworld!");
 
	i = 0;
	if (size_x == 0) size_x = strlen(buffer) - 1;
		
	do 
	{
		if (strlen(buffer) < (unsigned int)(size_x))
			throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltige Gridworld! (size_x != Gridgroesse)");
		grid->push_back(new char[size_x]);
		for (int j = size_x - 1; j >= 0; j--)
		{
			/*if (sscanf (&buffer[j], "%X", &temp) != 1)
				throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltige Gridworld! (ungueltiger Gridwert)");*/
			(*grid)[i][j] = (char)buffer[j];
			buffer[j] = 0;
		}
		i++;
	}
	while (fgets(buffer, 1024, stream) != NULL);
 
	if (size_y == 0)
		size_y = i;
	if (i != size_y)
		throw new std::runtime_error("Gridworld: Datei enthaelt keine gueltige Gridworld! (size_y != Gridgroesse)");
 
	is_allocated = true;
}
 
void CGridWorld::save(char* filename)
{
    FILE* stream = fopen(filename, "w");
	if (!stream)
		throw new std::runtime_error("Gridworld: Datei konnte nicht erstellt werden!");
	save(stream);
	fclose(stream);
}
 
void CGridWorld::save(FILE *stream)
{
    fprintf(stream, "Gridworld\n");
	fprintf(stream, "Size: %dx%d\n", size_x, size_y);
 
	if (!start_values->empty())
	{
		std::set<char>::iterator it = start_values->begin();
		fprintf(stream, "StartValues: %1X", *(it++));
		while (it != start_values->end())
		{
			fprintf(stream, ", %1X", *(it++));
		}
		fprintf(stream, "\n");
	}
 
	if (!target_values->empty())
	{
		std::set<char>::iterator it = target_values->begin();
		fprintf(stream, "TargetValues: %1X", *(it++));
		while (it != target_values->end())
		{
			fprintf(stream, ", %1X", *(it++));
		}
		fprintf(stream, "\n");
	}
 
	if (!prohibited_values->empty())
	{
		std::set<char>::iterator it = prohibited_values->begin();
		fprintf(stream, "ProhibitedValues: %1X", *(it++));
		while (it != prohibited_values->end())
		{
			fprintf(stream, ", %1X", *(it++));
		}
		fprintf(stream, "\n");
	}
 
	fprintf(stream, "\n");
    for (int i = 0; i < size_y; i++)
	{
        for (int j = 0; j < size_x; j++)
		{
            fprintf(stream, "%1X", (*grid)[i][j]);
        }
        fprintf(stream, "\n");			
    }
}
 
void CGridWorld::setGridValue(unsigned int pos_x, unsigned int pos_y, char value)
{
	if (is_allocated && ((int)pos_x < size_x) && ((int)pos_y < size_y))
	{
		(*grid)[pos_y][pos_x] = value;
	}
}
 
char CGridWorld::getGridValue(unsigned int pos_x, unsigned int pos_y)
{
	if (is_allocated && ((int)pos_x < size_x) && ((int)pos_y < size_y))
	{
		return (*grid)[pos_y][pos_x];
	}
	else throw new std::invalid_argument("Gridworld_getGridValue: ungueltiger Parameter oder Grid nicht initialisiert)");
}
 
void CGridWorld::addStartValue(char value)
{
	start_values->insert(value);
}
 
void CGridWorld::removeStartValue(char value)
{
	start_values->erase(value);
}
 
std::set<char> *CGridWorld::getStartValues()
{
	return start_values;
}
 
void CGridWorld::addTargetValue(char value)
{
	target_values->insert(value);
}
 
void CGridWorld::removeTargetValue(char value)
{
	target_values->erase(value);
}
 
std::set<char> *CGridWorld::getTargetValues()
{
	return target_values;
}
 
void CGridWorld::addProhibitedValue(char value)
{
	prohibited_values->insert(value);
}
 
void CGridWorld::removeProhibitedValue(char value)
{
	prohibited_values->erase(value);
}
 
std::set<char> *CGridWorld::getProhibitedValues()
{
	return prohibited_values;
}
 
void CGridWorld::setSize(unsigned int size_x, unsigned int size_y)
{
	this->deallocGrid();
	this->size_x = size_x;
	this->size_y = size_y;
}
 
unsigned int CGridWorld::getSizeX()
{
	return this->size_x;
}
 
unsigned int CGridWorld::getSizeY()
{
	return this->size_y;
}
 
std::set<char> *CGridWorld::getUsedValues()
{
	static std::set<char> *values = new std::set<char>();
	values->clear();
	char tmp;
	for (int j = 0; j < size_y; j++)
	{
		for (int i = 0; i < size_x; i++)
		{
			tmp = (*grid)[j][i];
			values->insert((*grid)[j][i]);
		}
	}
	return values;
}
 
 
CGridWorldModel::CGridWorldModel(char* filename, unsigned int max_bounces) : CGridWorld(filename), CTransitionFunction(new CStateProperties(0,3), new CActionSet()) {
	this->max_bounces = max_bounces;
	this->reward_standard = -1.0;
	this->reward_bounce = -10;
	this->reward_success = 100.0;
	this->is_parsed = false;
	this->start_points = new std::vector<std::pair<int, int>* >();
	this->rewards = new std::map<char, rlt_real>();
}
 
CGridWorldModel::CGridWorldModel(unsigned int size_x, unsigned int size_y, unsigned int max_bounces) : CGridWorld(size_x, size_y), CTransitionFunction(new CStateProperties(0,3), new CActionSet()) {
	this->max_bounces = max_bounces;
	this->reward_standard = -1.0;
	this->reward_bounce = -10;
	this->reward_success = 100.0;
	this->is_parsed = false;
	this->start_points = new std::vector<std::pair<int, int>* >();
	this->rewards = new std::map<char, rlt_real>();
}
 
CGridWorldModel::~CGridWorldModel()
{
    for(unsigned int j = 0; j < start_points->size(); j++)
	{
		delete (*start_points)[j];
	}
	delete start_points;
 
	delete properties;
	delete actions;
	delete rewards;
}
 
void CGridWorldModel::setMaxBounces(unsigned int value)
{
	this->max_bounces = value;
}
 
unsigned int CGridWorldModel::getMaxBounces()
{
	return max_bounces;
}
/*
void CGridWorldModel::setActualBounces(unsigned int value)
{
	this->actual_bounces = value;
}
 
unsigned int CGridWorldModel::getActualBounces()
{
	return actual_bounces;
}
 
void CGridWorldModel::setPosX(unsigned int value)
{
	if (value >= (unsigned int)size_x)
		throw new std::invalid_argument("GridWorldModel_setPosX: ungueltiger Parameter!");
	pos_x = value;
}
 
int CGridWorldModel::getPosX()
{
	return pos_x;
}
 
void CGridWorldModel::setPosY(unsigned int value)
{
	if (value >= (unsigned int)size_y)
		throw new std::invalid_argument("GridWorldModel_setPosY: ungueltiger Parameter!");
	pos_y = value;
}
 
int CGridWorldModel::getPosY()
{
	return pos_y;
}
*/
void CGridWorldModel::setRewardStandard(rlt_real value)
{
	this->reward_standard = value;
	
}
 
rlt_real CGridWorldModel::getRewardStandard()
{
	return this->reward_standard;
}
 
void CGridWorldModel::setRewardSuccess(rlt_real value)
{
	this->reward_success = value;
 
 
}
 
rlt_real CGridWorldModel::getRewardSuccess()
{
	return this->reward_success;
}
 
void CGridWorldModel::setRewardBounce(rlt_real value)
{
	reward_bounce = value;
}
 
 
rlt_real CGridWorldModel::getRewardBounce()
{
	return reward_bounce;
}
 
void CGridWorldModel::setRewardForSymbol(char symbol, rlt_real reward)
{
	(*rewards)[symbol] = reward;
}
 
rlt_real CGridWorldModel::getRewardForSymbol(char symbol)
{
	std::map<char, rlt_real>::iterator it = rewards->find(symbol);
	rlt_real rew = 0.0;
 
	if (it != rewards->end())
	{
		rew = (*it).second;
	}
	else
	{
		if (target_values->find(symbol) != target_values->end())
		{
			rew = reward_success;
		}
		else
		{
			rew = reward_standard;
		}
	}
	return rew;
}
 
 
 
void CGridWorldModel::parseGrid()
{
	if (isValid() && !is_parsed)
	{
		for(unsigned int h = 0; h < start_points->size(); h++)
		{
			delete (*start_points)[h];
		}
		start_points->clear();
	    for (int j = 0; j < size_y; j++)
		{
			for (int i = 0; i < size_x; i++)
			{
				if(start_values->find(getGridValue(i, j)) != start_values->end())
					start_points->push_back(new std::pair<int, int>(j, i));
			}
		}
	}
	is_parsed = true;
}
 
void CGridWorldModel::load(FILE *stream)
{
	CGridWorld::load(stream);
	is_parsed = false;
}
 
void CGridWorldModel::initGrid()
{
	CGridWorld::initGrid();
	is_parsed = false;
}
 
void CGridWorldModel::setGridValue(unsigned int pos_x, unsigned int pos_y, char value)
{
	CGridWorld::setGridValue(pos_x, pos_y, value);
	is_parsed = false;
}
 
void CGridWorldModel::addStartValue(char value)
{
	CGridWorld::addStartValue(value);
	is_parsed = false;
}
 
void CGridWorldModel::removeStartValue(char value)
{
	CGridWorld::removeStartValue(value);
	is_parsed = false;
}
 
void CGridWorldModel::transitionFunction(CState *oldstate, CAction *action, CState *newState, CActionData *data)
{
	int pos_x = oldstate->getDiscreteState(0);
	int pos_y = oldstate->getDiscreteState(1);
	int actual_bounces = oldstate->getDiscreteState(2);
 
	CGridWorldAction* gridAction = dynamic_cast<CGridWorldAction*>(action);
 
	int tmp_pos_x = pos_x + gridAction->getXMove();
	int tmp_pos_y = pos_y + gridAction->getYMove();
 
	if (tmp_pos_x < 0 || tmp_pos_x >= size_x || tmp_pos_y < 0 || tmp_pos_y >= size_y)
	{
		actual_bounces++;
	}
	else if (prohibited_values->find(getGridValue(tmp_pos_x, tmp_pos_y)) != prohibited_values->end())
	{
		actual_bounces++;
	}
	else
	{
		pos_x = tmp_pos_x;
		pos_y = tmp_pos_y;
	}
 
	newState->setDiscreteState(0, pos_x);
	newState->setDiscreteState(1, pos_y);
	newState->setDiscreteState(2,actual_bounces);
}
 
bool CGridWorldModel::isResetState(CState *state)
{
	return ((unsigned int) state->getDiscreteState(2) > this->max_bounces) || (target_values->find(getGridValue(state->getDiscreteState(0), state->getDiscreteState(1))) != target_values->end());
}
 
bool CGridWorldModel::isFailedState(CState *state)
{
	return ((unsigned int) state->getDiscreteState(2) > this->max_bounces);
}
 
void CGridWorldModel::getResetState(CState *resetState)
{
	if (!is_parsed)
		parseGrid();
	if (start_points->size() > 0)
	{
		int i = rand() % start_points->size();
		resetState->setDiscreteState(0, (*start_points)[i]->second);
		resetState->setDiscreteState(1, (*start_points)[i]->first);
	}
	else
	{
		resetState->setDiscreteState(0, 0);
		resetState->setDiscreteState(1, 0);
	}
	resetState->setDiscreteState(2, 0);
}
 
rlt_real CGridWorldModel::getReward(CStateCollection *oldState, CAction *action, CStateCollection *newState) {
	rlt_real rew = 0.0;
	if (newState->getState()->getDiscreteState(2) > oldState->getState()->getDiscreteState(2))
	{
		rew = reward_bounce;
	}
	else
	{
		int x = newState->getState()->getDiscreteState(0);
		int y = newState->getState()->getDiscreteState(1);
 
		rew = getRewardForSymbol(getGridValue(x, y));
	}
    return rew;
} 
 
CLocal4GridWorldState::CLocal4GridWorldState(CGridWorld* grid_world) : CStateModifier(0, 4)
{
	this->grid_world = grid_world;
	for (int i = 0; i < 4; i++)
	{
		setDiscreteStateSize(i, grid_world->getUsedValues()->size());
	}
}
 
CLocal4GridWorldState::~CLocal4GridWorldState()
{
}
 
void CLocal4GridWorldState::getModifiedState(CStateCollection *originalState, CState *state)
{
	int pos_x = originalState->getState()->getDiscreteState(0);
	int pos_y = originalState->getState()->getDiscreteState(1);
	
	if (pos_y > 0)
		state->setDiscreteState(0, grid_world->getGridValue(pos_x, pos_y - 1));
	else
		state->setDiscreteState(0, grid_world->getGridValue(pos_x, pos_y));
 
	if (pos_x < (int)grid_world->getSizeX() - 1)
		state->setDiscreteState(1, grid_world->getGridValue(pos_x + 1, pos_y));
	else
		state->setDiscreteState(1, grid_world->getGridValue(pos_x, pos_y));
 
	if (pos_y < (int)grid_world->getSizeY() - 1)
		state->setDiscreteState(2, grid_world->getGridValue(pos_x, pos_y + 1));
	else
		state->setDiscreteState(2, grid_world->getGridValue(pos_x, pos_y));
 
	if (pos_x > 0)
		state->setDiscreteState(3, grid_world->getGridValue(pos_x - 1, pos_y));
	else
		state->setDiscreteState(3, grid_world->getGridValue(pos_x, pos_y));
} 
 
 
CLocal4XGridWorldState::CLocal4XGridWorldState(CGridWorld* grid_world) : CStateModifier(0, 4)
{
	this->grid_world = grid_world;
	for (int i = 0; i < 4; i++)
	{
		setDiscreteStateSize(i, grid_world->getUsedValues()->size());
	}
}
 
CLocal4XGridWorldState::~CLocal4XGridWorldState()
{
}
 
void CLocal4XGridWorldState::getModifiedState(CStateCollection *originalState, CState *state)
{
	int pos_x = originalState->getState()->getDiscreteState(0);
	int pos_y = originalState->getState()->getDiscreteState(1);
	
	if ((pos_y > 0) && (pos_x > 0))
		state->setDiscreteState(0, grid_world->getGridValue(pos_x - 1, pos_y - 1));
	else
		state->setDiscreteState(0, grid_world->getGridValue(pos_x, pos_y));
 
	if ((pos_y > 0) && (pos_x < (int)grid_world->getSizeX() - 1))
		state->setDiscreteState(1, grid_world->getGridValue(pos_x + 1, pos_y - 1));
	else
		state->setDiscreteState(1, grid_world->getGridValue(pos_x, pos_y));
 
	if ((pos_y < (int)grid_world->getSizeY() - 1) && (pos_x < (int)grid_world->getSizeX() - 1))
		state->setDiscreteState(2, grid_world->getGridValue(pos_x + 1, pos_y + 1));
	else
		state->setDiscreteState(2, grid_world->getGridValue(pos_x, pos_y));
 
	if ((pos_y < (int)grid_world->getSizeY() - 1) && (pos_x > 0))
		state->setDiscreteState(3, grid_world->getGridValue(pos_x - 1, pos_y + 1));
	else
		state->setDiscreteState(3, grid_world->getGridValue(pos_x, pos_y));
} 
 
 
CLocal8GridWorldState::CLocal8GridWorldState(CGridWorld* grid_world) : CStateModifier(0, 8)
{
	this->grid_world = grid_world;
	for (int i = 0; i < 8; i++)
	{
		setDiscreteStateSize(i, grid_world->getUsedValues()->size());
	}
}
 
CLocal8GridWorldState::~CLocal8GridWorldState()
{
}
 
void CLocal8GridWorldState::getModifiedState(CStateCollection *originalState, CState *state)
{
	int pos_x = originalState->getState()->getDiscreteState(0);
	int pos_y = originalState->getState()->getDiscreteState(1);
 
	if (pos_y > 0)
		state->setDiscreteState(0, grid_world->getGridValue(pos_x, pos_y - 1));
	else
		state->setDiscreteState(0, grid_world->getGridValue(pos_x, pos_y));
 
	if ((pos_y > 0) && (pos_x < (int)grid_world->getSizeX() - 1))
		state->setDiscreteState(1, grid_world->getGridValue(pos_x + 1, pos_y - 1));
	else
		state->setDiscreteState(1, grid_world->getGridValue(pos_x, pos_y));
 
	if (pos_x < (int)grid_world->getSizeX() - 1)
		state->setDiscreteState(2, grid_world->getGridValue(pos_x + 1, pos_y));
	else
		state->setDiscreteState(2, grid_world->getGridValue(pos_x, pos_y));
 
	if ((pos_y < (int)grid_world->getSizeY() - 1) && (pos_x < (int)grid_world->getSizeX() - 1))
		state->setDiscreteState(3, grid_world->getGridValue(pos_x + 1, pos_y + 1));
	else
		state->setDiscreteState(3, grid_world->getGridValue(pos_x, pos_y));
 
	if (pos_y < (int)grid_world->getSizeY() - 1)
		state->setDiscreteState(4, grid_world->getGridValue(pos_x, pos_y + 1));
	else
		state->setDiscreteState(4, grid_world->getGridValue(pos_x, pos_y));
 
	if ((pos_y < (int)grid_world->getSizeY() - 1) && (pos_x > 0))
		state->setDiscreteState(5, grid_world->getGridValue(pos_x - 1, pos_y + 1));
	else
		state->setDiscreteState(5, grid_world->getGridValue(pos_x, pos_y));
 
	if (pos_x > 0)
		state->setDiscreteState(6, grid_world->getGridValue(pos_x - 1, pos_y));
	else
		state->setDiscreteState(6, grid_world->getGridValue(pos_x, pos_y));
 
	if ((pos_y > 0) && (pos_x > 0))
		state->setDiscreteState(7, grid_world->getGridValue(pos_x - 1, pos_y - 1));
	else
		state->setDiscreteState(7, grid_world->getGridValue(pos_x, pos_y));
} 
 
 
CGlobalGridWorldDiscreteState::CGlobalGridWorldDiscreteState(unsigned int size_x, unsigned int size_y) : CAbstractStateDiscretizer(size_x * size_y + 1)
{
	this->size_x = size_x;
	this->size_y = size_y;
}
 
unsigned int CGlobalGridWorldDiscreteState::getDiscreteStateNumber(CStateCollection *state) {
    unsigned int discstate;
	int x = state->getState()->getDiscreteState(0);
	int y = state->getState()->getDiscreteState(1);
 
	if (x < 0 || (unsigned int)x >= size_x || y < 0 || (unsigned int)y >= size_y)
	{
		discstate = 0;
    }
	else
	{
		discstate = y * size_x + x + 1;
	}
	return discstate;
}
 
 
CLocalGridWorldDiscreteState::CLocalGridWorldDiscreteState(CStateModifier* orig_state, unsigned int neighbourhood, std::set<char> *possible_values) : CAbstractStateDiscretizer((int)pow((rlt_real) possible_values->size(), (rlt_real) neighbourhood))
{
	this->orig_state = orig_state;
	valuemap = new std::map<char, short>();
	std::set<char>::iterator it = possible_values->begin();
	for(short i = 0; it != possible_values->end(); i++, it++)
	{
		(*valuemap)[(*it)] = i;
	}
}
 
CLocalGridWorldDiscreteState::~CLocalGridWorldDiscreteState()
{
	valuemap->clear();
	delete valuemap;
}
 
unsigned int CLocalGridWorldDiscreteState::getDiscreteStateNumber(CStateCollection *state)
{
    CState *source_state = state->getState(orig_state);
    unsigned int discstate = 0;
	for (unsigned int i = 0; i < source_state->getNumDiscreteStates() - 1; i++)
	{
		discstate = discstate * valuemap->size() + (unsigned int)((*valuemap)[(char)source_state->getDiscreteState(i)]);
	}
	return discstate;
}
 
 
CSmallLocalGridWorldDiscreteState::CSmallLocalGridWorldDiscreteState(CStateModifier* orig_state, unsigned int neighbourhood, CGridWorld *gridworld) : CAbstractStateDiscretizer((unsigned int)pow((rlt_real) 3.0,(rlt_real) neighbourhood))
{
	this->orig_state = orig_state;
	this->gridworld = gridworld;
}
 
CSmallLocalGridWorldDiscreteState::~CSmallLocalGridWorldDiscreteState()
{
}
 
unsigned int CSmallLocalGridWorldDiscreteState::getDiscreteStateNumber(CStateCollection *state)
{
    CState *source_state = state->getState(orig_state);
    unsigned int discstate = 0;
	unsigned int temp;
	for (unsigned int i = 0; i < source_state->getNumDiscreteStates() - 1; i++)
	{
		if (gridworld->getTargetValues()->find((char)source_state->getDiscreteState(i)) != gridworld->getTargetValues()->end())
			temp = 2;
		else if (gridworld->getProhibitedValues()->find((char)source_state->getDiscreteState(i)) != gridworld->getProhibitedValues()->end())
			temp = 1;
		else
			temp = 0;
		discstate = discstate * 3 + temp;
	}
	return discstate;
}
 
 
CGridWorldAction::CGridWorldAction(int x_move, int y_move)
{
	addType(GRIDWORLDACTION);
 
    this->x_move = x_move;
	this->y_move = y_move;
}
 
int CGridWorldAction::getXMove()
{
    return this->x_move;
}
 
int CGridWorldAction::getYMove()
{
    return this->y_move;
}
 
 
#ifdef WIN32
 
CGridWorldController::CGridWorldController(CGridWorld *gridworld, CActionSet *actions) : CAgentStatisticController(actions)
{
    this->gridworld = gridworld;
	target_points = new std::set<std::pair<unsigned int, unsigned int>*>();
	record = new std::vector<GridControllerRecord>();
	record->resize(actions->size());
	lastXMove = 0;
	lastYMove = 0;
	init();
}
 
CGridWorldController::~CGridWorldController()
{
	record->clear();
	delete record;
	if (target_points->size() > 0)
	{
		std::set<std::pair<unsigned int, unsigned int> *>::iterator it = target_points->begin();
        for(;it != target_points->end(); it++)
		{
			delete *it;
		}
		target_points->clear();
	}
    delete target_points;
}
 
void CGridWorldController::init()
{
	std::pair<unsigned int, unsigned int> *target_point;
	if (target_points->size() > 0)
	{
		std::set<std::pair<unsigned int, unsigned int> *>::iterator it = target_points->begin();
        for(;it != target_points->end(); it++)
		{
			delete *it;
		}
		target_points->clear();
	}
 
	for (unsigned int j = 0; j < gridworld->getSizeY(); j++)
	{
		for (unsigned int i = 0; i < gridworld->getSizeX(); i++)
		{
			if (gridworld->getTargetValues()->find(gridworld->getGridValue(i ,j)) != gridworld->getTargetValues()->end())
			{
				target_point = new std::pair<unsigned int, unsigned int>(i, j);
				target_points->insert(target_point);
			}
		}
	}
}
 
void CGridWorldController::newEpisode()
{
	for (unsigned int i = 0; i < record->size(); i++)
	{
		(*record)[i].factor = 1.0;
	}
}
 
CAction* CGridWorldController::getNextAction(CStateCollection *state, CActionStatistics *stat)
{
	std::set<std::pair<unsigned int, unsigned int> *>::iterator itp;
	int x = state->getState()->getDiscreteState(0);
	int y = state->getState()->getDiscreteState(1);
	unsigned int bestind = 0;
	rlt_real dist;
	rlt_real maxdist = gridworld->getSizeX() * gridworld->getSizeX() + gridworld->getSizeY() * gridworld->getSizeY();
	CActionSet::iterator it = actions->begin();
	for (int i = 0; it != actions->end(); i++, it++)
	{
		CGridWorldAction *gridAction = dynamic_cast<CGridWorldAction*>(*it);
		(*record)[i].pos_x = x + gridAction->getXMove();
		(*record)[i].pos_y = y + gridAction->getYMove();
		(*record)[i].action = gridAction;
		dist = maxdist * 2.0;
		if (gridworld->getProhibitedValues()->find(gridworld->getGridValue((*record)[i].pos_x ,(*record)[i].pos_y)) == gridworld->getProhibitedValues()->end())
		{ // kein verbotener wert
			for (itp = target_points->begin(); itp != target_points->end(); itp++)
			{
		        dist = min(dist, ((*record)[i].pos_x - (*itp)->first) * ((*record)[i].pos_x - (*itp)->first) + ((*record)[i].pos_y - (*itp)->second) * ((*record)[i].pos_y - (*itp)->second));
			}
			dist *= (*record)[i].factor;
		}
		else
		{
			//(*record)[i].factor = 1.0;
		}
		if ((lastXMove == -gridAction->getXMove()) && (lastYMove == -gridAction->getYMove()))
		{ // wir wollen doch nicht den gleichen weg zurueck gehen
			(*record)[i].distance = max(maxdist - 0.5, dist);
		}
		else
		{
			(*record)[i].distance = dist;
		}
		if ((*record)[i].distance < (*record)[bestind].distance)
			bestind = i;
	}
	lastXMove = (*record)[bestind].action->getXMove();
	lastYMove = (*record)[bestind].action->getYMove();
 
	for (unsigned int j = 0; j < record->size(); j++)
	{
		if (j == bestind)
			(*record)[j].factor = max(0.7, (*record)[j].factor * 0.99);
		else
			(*record)[j].factor = min(1.3, (*record)[j].factor * 1.01);
	}
 
	if (stat != NULL)
	{
		stat->action = (*record)[bestind].action;
		stat->owner = this;
		stat->equal = 1;
		stat->probability = 0.5;
		stat->superior = 0;
		return stat->action;
	}
	else
	{
		return (*record)[bestind].action;
	}
}
 
 
CGridWorldVisualizer::CGridWorldVisualizer(CGridWorldModel *gridworld)
{
	this->gridworld = gridworld;
	this->console = GetStdHandle(STD_OUTPUT_HANDLE);
	this->flgDisplay = true;
	this->flgTranspose = false;
	this->xpos = 0;
    this->ypos = 0;
	this->xoffset = 1;
	this->yoffset = 3;
}
 
CGridWorldVisualizer::~CGridWorldVisualizer()
{
}
 
void CGridWorldVisualizer::nextStep(CStateCollection *oldStateCol, CAction *action, CStateCollection *nextStateCol)
{
	COORD coord;
    const WORD attribute = BACKGROUND_RED;
	DWORD dummy;
 
	CState *nextState = nextStateCol->getState();
	CState *oldState = oldStateCol->getState();
 
	if (flgTranspose)
	{
		xpos = (short)oldState->getDiscreteState(1);
		ypos = (short)oldState->getDiscreteState(0);
	}
	else
	{
		xpos = (short)oldState->getDiscreteState(0);
		ypos = (short)oldState->getDiscreteState(1);
	}
 
	if (flgDisplay)
	{
		coord.X = xpos + xoffset;
		coord.Y = ypos + yoffset;
		SetConsoleCursorPosition(console, coord);
		if (gridworld->getGridValue(xpos, ypos) != 0)
		{
			printf("%1X", gridworld->getGridValue(xpos, ypos));
		}
		else
		{
			printf(" ");
		}
		WriteConsoleOutputAttribute(console, &attribute, 1, coord, &dummy);
 
		if (flgTranspose)
		{
			xpos = (short)nextState->getDiscreteState(1);
			ypos = (short)nextState->getDiscreteState(0);
		}
		else
		{
			xpos = (short)nextState->getDiscreteState(0);
			ypos = (short)nextState->getDiscreteState(1);
		}
		coord.X = xpos + xoffset;
		coord.Y = ypos + yoffset;
		SetConsoleCursorPosition(console, coord);
		printf("#");
	}
}
 
void CGridWorldVisualizer::newEpisode()
{
	DWORD dummy;
	COORD coord;
	CONSOLE_SCREEN_BUFFER_INFO sInfo;
	GetConsoleScreenBufferInfo(console, &sInfo);
	coord.X = 0;
	coord.Y = 0;
	FillConsoleOutputCharacter(console, ' ', (1 + sInfo.srWindow.Right - sInfo.srWindow.Left) * (1 + sInfo.srWindow.Bottom - sInfo.srWindow.Top), coord, &dummy);
	
	if (flgDisplay)
	{
		flgTranspose = (gridworld->getSizeX() < gridworld->getSizeY());
		if (flgTranspose)
		{
			coord.X = xoffset;
			for (unsigned int h = 0; h < gridworld->getSizeX(); h++)
			{
				coord.Y = (short)h + yoffset;
				SetConsoleCursorPosition(console, coord);
				for (unsigned int j = 0; j < gridworld->getSizeY(); j++)
				{
					if (gridworld->getGridValue(h, j) != 0)
					{
						printf("%1X", gridworld->getGridValue(h, j));
					}
					else
					{
						printf(" ");
					}
				}
			}
			//xpos = (short)gridworld->getPosY();
			//ypos = (short)gridworld->getPosX();
		}
		else
		{
			coord.X = xoffset;
			for (unsigned int h = 0; h < gridworld->getSizeY(); h++)
			{
				coord.Y = (short)h + yoffset;
				SetConsoleCursorPosition(console, coord);
				for (unsigned int j = 0; j < gridworld->getSizeX(); j++)
				{
					if (gridworld->getGridValue(j, h) != 0)
					{
						printf("%1X", gridworld->getGridValue(j, h));
					}
					else
					{
						printf(" ");
					}
				}
			}
			//xpos = (short)gridworld->getPosX();
			//ypos = (short)gridworld->getPosY();
		}
		coord.X = xpos + xoffset;
		coord.Y = ypos + yoffset;
		//SetConsoleCursorPosition(console, coord);
		//printf("#");
	}
}
 
bool CGridWorldVisualizer::getDisplay()
{
	return flgDisplay;
}
 
void CGridWorldVisualizer::setDisplay(bool flgDisplay)
{
	this->flgDisplay = flgDisplay;
}
 
#endif // WIN32
 
 
void CRaceTrack::generateRaceTrack(CGridWorld *gridworld, unsigned int width, unsigned int length, unsigned int h_max, unsigned int dx_min, unsigned int dx_max)
{
	unsigned int i, j, x, dx, tmp1, tmp2, y1, y2, h;
	gridworld->setSize(length, width);
	gridworld->initGrid();
	gridworld->addProhibitedValue(1);
	gridworld->addProhibitedValue(2);
	gridworld->addStartValue(3);
	gridworld->addTargetValue(4);
 
	for (j = 1; j < width - 1; j++)
	{
		gridworld->setGridValue(0, j, 2);
		gridworld->setGridValue(1, j, 3);
		gridworld->setGridValue(length - 2, j, 4);
		gridworld->setGridValue(length - 1, j, 2);
		for (i = 2; i < length - 2; i++)
		{
			gridworld->setGridValue(i, j, 0);
		}
 
	}
	for (i = 0; i < length; i++)
	{
		gridworld->setGridValue(i, 0, 2);
		gridworld->setGridValue(i, width - 1, 2);
	}
 
	dx = (int)ceil((rlt_real)rand() / (rlt_real)RAND_MAX * (rlt_real)dx_max);
	x = 2 + dx;
	while (x < length - 2)
	{
		tmp1 = (int)ceil((rlt_real)rand() / (rlt_real)RAND_MAX * (rlt_real)(width - 1));
		tmp2 = (int)ceil((rlt_real)rand() / (rlt_real)RAND_MAX * (rlt_real)(width - 1));
		y1 = min(tmp1, tmp2);
		y2 = max(tmp1, tmp2);
		if (( y1 < 2) && (y2 > width - 3))
		{
			if (rand() < (RAND_MAX / 2))
			{
				y1 = 2;
				y2 = width - 2;
			}
			else
			{
				y1 = 1;
				y2 = width - 3;
			}
		}
		h = min(dx, (unsigned int)ceil((rlt_real)rand() / (rlt_real)RAND_MAX * (rlt_real)h_max)) - 1;
		for(i = y1; i <= y2; i++)
		{
			gridworld->setGridValue(x, i, 1);
		}
        for(i = x-h+1; i <= x; i++)
		{
			gridworld->setGridValue(i, y1, 1);
			gridworld->setGridValue(i, y2, 1);
		}
		dx = dx_min + (int)ceil((rlt_real)rand() / (rlt_real)RAND_MAX * (rlt_real)dx_max);
		x += dx;
	}
}
 
 
CRaceTrackDiscreteState::CRaceTrackDiscreteState(CStateModifier* orig_state, unsigned int neighbourhood, CGridWorld *gridworld) : CAbstractStateDiscretizer((unsigned int)pow((rlt_real) 2.0, (rlt_real) neighbourhood))
{
	this->orig_state = orig_state;
	this->gridworld = gridworld;
}
 
CRaceTrackDiscreteState::~CRaceTrackDiscreteState()
{
}
 
unsigned int CRaceTrackDiscreteState::getDiscreteStateNumber(CStateCollection *state)
{
    CState *source_state = state->getState(orig_state);
    unsigned int discstate = 0;
	unsigned int temp;
	for (unsigned int i = 0; i < source_state->getNumDiscreteStates() - 1; i++)
	{
        if (gridworld->getProhibitedValues()->find((char)source_state->getDiscreteState(i)) != gridworld->getProhibitedValues()->end())
			temp = 1;
		else
			temp = 0;
		discstate = discstate * 2 + temp;
	}
	return discstate;
}
                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:
125:
126:
127:
128:
129:
130:
131:
132:
133:
134:
135:
136:
137:
138:
139:
140:
141:
142:
143:
144:
145:
146:
147:
148:
149:
150:
151:
152:
153:
154:
155:
156:
157:
158:
159:
160:
161:
162:
163:
164:
165:
166:
167:
168:
169:
170:
171:
172:
173:
174:
175:
176:
177:
178:
179:
180:
181:
182:
183:
184:
185:
186:
187:
188:
189:
190:
191:
192:
193:
194:
195:
196:
197:
198:
199:
200:
201:
202:
203:
204:
205:
206:
207:
208:
209:
210:
211:
212:
213:
214:
215:
216:
217:
218:
219:
220:
221:
222:
223:
224:
225:
226:
227:
228:
229:
230:
231:
232:
233:
234:
235:
236:
237:
238:
239:
240:
241:
242:
243:
244:
245:
246:
247:
248:
249:
250:
251:
252:
253:
254:
255:
256:
257:
258:
259:
260:
261:
262:
263:
264:
265:
266:
267:
268:
269:
270:
271:
272:
273:
274:
275:
276:
277:
278:
279:
280:
281:
282:
283:
284:
285:
286:
287:
288:
289:
290:
291:
292:
293:
294:
295:
296:
297:
298:
299:
300:
301:
302:
303:
304:
305:
306:
307:
308:
309:
310:
311:
312:
313:
314:
315:
316:
317:
318:
319:
320:
321:
322:
323:
324:
325:
326:
327:
328:
329:
330:
331:
332:
333:
334:
335:
336:
337:
338:
339:
340:
341:
342:
343:
344:
345:
346:
347:
348:
349:
350:
351:
352:
353:
354:
355:
356:
357:
358:
359:
360:
361:
362:
363:
364:
365:
366:
367:
368:
369:
370:
371:
372:
373:
374:
375:
376:
377:
378:
379:
380:
381:
382:
383:
384:
385:
386:
387:
388:
389:
390:
391:
392:
393:
394:
395:
396:
397:
398:
399:
400:
401:
402:
403:
404:
405:
406:
407:
408:
409:
410:
411:
412:
413:
414:
415:
416:
417:
418:
419:
420:
421:
422:
423:
424:
425:
426:
427:
428:
429:
430:
431:
432:
433:
434:
435:
436:
437:
438:
439:
440:
441:
442:
443:
444:
445:
446:
447:
448:
449:
450:
451:
452:
453:
454:
455:
456:
457:
458:
459:
460:
461:
462:
463:
464:
465:
466:
467:
468:
469:
470:
471:
472:
473:
474:
475:
476:
477:
478:
479:
480:
481:
482:
483:
484:
485:
486:
487:
488:
489:
490:
491:
492:
493:
494:
495:
496:
497:
498:
499:
500:
501:
502:
503:
504:
505:
506:
507:
508:
509:
510:
511:
512:
513:
514:
515:
516:
517:
518:
519:
520:
521:
522:
523:
524:
525:
526:
527:
528:
529:
530:
531:
532:
533:
534:
535:
536:
537:
538:
539:
540:
541:
542:
543:
544:
545:
546:
547:
548:
549:
550:
551:
552:
553:
554:
555:
556:
557:
558:
559:
560:
561:
562:
563:
564:
565:
566:
567:
568:
569:
570:
571:
572:
573:
574:
575:
576:
577:
578:
579:
580:
581:
582:
583:
584:
585:
586:
587:
588:
589:
590:
591:
592:
593:
594:
595:
596:
597:
598:
599:
600:
601:
602:
603:
604:
605:
606:
607:
608:
609:
610:
611:
612:
613:
614:
615:
616:
617:
618:
619:
620:
621:
622:
623:
624:
625:
626:
627:
628:
629:
630:
631:
632:
633:
634:
635:
636:
637:
638:
639:
640:
641:
642:
643:
644:
645:
646:
647:
648:
649:
650:
651:
652:
653:
654:
655:
656:
657:
658:
659:
660:
661:
662:
663:
664:
665:
666:
667:
668:
669:
670:
671:
672:
673:
674:
675:
676:
677:
678:
679:
680:
681:
682:
683:
684:
685:
686:
687:
688:
689:
690:
691:
692:
693:
694:
695:
696:
697:
698:
699:
700:
701:
702:
703:
704:
705:
706:
707:
708:
709:
710:
711:
712:
713:
714:
715:
716:
717:
718:
719:
720:
721:
722:
723:
724:
725:
726:
727:
728:
729:
730:
731:
732:
733:
734:
735:
736:
737:
738:
739:
740:
741:
742:
743:
744:
745:
746:
747:
748:
749:
750:
751:
752:
753:
754:
755:
756:
757:
758:
759:
760:
761:
762:
763:
764:
765:
766:
767:
768:
769:
770:
771:
772:
773:
774:
775:
776:
777:
778:
779:
780:
781:
782:
783:
784:
785:
786:
787:
788:
789:
790:
791:
792:
793:
794:
795:
796:
797:
798:
799:
800:
801:
802:
803:
804:
805:
806:
807:
808:
809:
810:
811:
812:
813:
814:
815:
816:
817:
818:
819:
820:
821:
822:
823:
824:
825:
826:
827:
828:
829:
830:
831:
832:
833:
834:
835:
836:
837:
838:
839:
840:
841:
842:
843:
844:
845:
846:
847:
848:
849:
850:
851:
852:
853:
854:
855:
856:
857:
858:
859:
860:
861:
862:
863:
864:
865:
866:
867:
868:
869:
870:
871:
872:
873:
874:
875:
876:
877:
878:
879:
880:
881:
882:
883:
884:
885:
886:
887:
888:
889:
890:
891:
892:
893:
894:
895:
896:
897:
898:
899:
900:
901:
902:
903:
904:
905:
906:
907:
908:
909:
910:
911:
912:
913:
914:
915:
916:
917:
918:
919:
920:
921:
922:
923:
924:
925:
926:
927:
928:
929:
930:
931:
932:
933:
934:
935:
936:
937:
938:
939:
940:
941:
942:
943:
944:
945:
946:
947:
948:
949:
950:
951:
952:
953:
954:
955:
956:
957:
958:
959:
960:
961:
962:
963:
964:
965:
966:
967:
968:
969:
970:
971:
972:
973:
974:
975:
976:
977:
978:
979:
980:
981:
982:
983:
984:
985:
986:
987:
988:
989:
990:
991:
992:
993:
994:
995:
996:
997:
998:
999:
1000:
1001:
1002:
1003:
1004:
1005:
1006:
1007:
1008:
1009:
1010:
1011:
1012:
1013:
1014:
1015:
1016:
1017:
1018:
1019:
1020:
1021:
1022:
1023:
1024:
1025:
1026:
1027:
1028:
1029:
1030:
1031:
1032:
1033:
1034:
1035:
1036:
1037:
1038:
1039:
1040:
1041:
1042:
1043:
1044:
1045:
1046:
1047:
1048:
1049:
1050:
1051:
1052:
1053:
1054:
1055:
1056:
1057:
1058:
1059:
1060:
1061:
1062:
1063:
1064:
1065:
1066:
1067:
1068:
1069:
1070:
1071:
1072:
1073:
1074:
1075:
1076:
1077:
1078:
1079:
1080:
1081:
1082:
1083:
1084:
1085:
1086:
1087:
1088:
1089:
1090:
1091:
1092:
1093:
1094:
1095:
1096:
1097:
1098:
1099:
1100:
1101:
1102:
1103:
1104:
1105:
1106:
1107:
1108:
1109:
1110:
1111:
1112:
1113:
1114:
1115:
1116:
1117:
1118:
1119:
1120:
1121:
1122:
1123:
1124:
1125:
1126:
1127:
1128:
1129:
1130:
1131:
1132:
1133:
1134:
1135:
1136:
1137:
1138:
1139:
1140:
1141:
1142:
1143:
1144:
1145:
1146:
1147:
1148:
1149:
1150:
1151:
1152:
1153:
1154:
1155:
1156:
1157:
1158:
1159:
1160:
1161:
1162:
1163:
1164:
1165:
1166:
1167:
1168:
1169:
1170:
1171:
1172:
1173:
1174:
1175:
1176:
1177:
1178:
1179:
1180:
1181:
1182:
1183:
1184:
1185:
1186:
1187:
1188:
1189:
1190:
1191:
1192:
1193:
1194:
1195:
1196:
1197:
1198:
1199:
1200:
1201:
1202:
1203:
1204:
1205:
1206:
1207:
1208:
1209:
1210:
1211:
1212:
1213:
1214:
1215:
1216:
1217:
1218:
1219:
1220:
1221:
1222:
1223:
1224:
1225:
1226:
1227:
1228:
1229:
1230:
1231:
1232:
1233:
1234:
1235:
1236:
1237:
1238:
1239:
1240:
1241:
1242:
1243:
1244:
1245:
1246:
1247: