	[x] Posted via EE Mobile
	Search, ask, and monitor your questions on the go with EE Mobile. Visit Experts Exchange from your mobile device and never be out of touch again.
Question
	[x] Attachment Details
[x]
The Solution Rating System
With so many solutions, how can you tell which solutions are most likely to help you and which ones are not? To provide you with a tool to use, we rate our solutions based on various elements that most accurately determine if a solution is a quality solution. To explain what factors affect the solution rating, here are the elements we take into consideration when formulating our solution rating.
The Grade of the Solution
The Zone Rank of the Expert Providing the Solution
The Number of Author and Expert Comments
The Number of Experts Contributing
The Feedback of the Community
Your Input Matters
Because of the way the system is set up, the most important variable in this equation is you. As a member of Experts Exchange, you are able to cast your vote on the quality of the solutions in regard to how complete, accurate, helpful and easy to understand each solution is. When you provide your feedback, each rating is adjusted accordingly. So, if you see a solution that has a poor rating that you think is a good solution, let us know by rating it. As you do, the rating will be adjusted and will become more accurate for other members of our site.
If you have any suggestions that you would like to make for our rating system, please ask a question in the Suggestions Zone of Community Support.
Thank you!
3.6
Heap problem

Asked by Gabriela1 in C++ Programming Language, Microsoft Visual C++.Net

Hi Guys,

I am trying to make a program that can detect edges and then select anedge as a landmark . But it seems that I have some problems with the heap. It is a long code, and I hope you can find the problem.

I tried to delete all the dynamic memory that I allocated but I still get this :

Windows has triggered a breakpoint in I90Controller.exe.

This may be due to a corruption of the heap, which indicates a bug in I90Controller.exe or any of the DLLs it has loaded.

This may also be due to the user pressing F12 while I90Controller.exe has focus.

The output window may have more diagnostic information.

The line where it gets the breakpoint is :

#if _MSC_VER >= 1210
void* __cdecl operator new[](size_t nSize, LPCSTR lpszFileName, int nLine)
{
return ::operator new[](nSize, _NORMAL_BLOCK, lpszFileName, nLine);
}
#endif

The program is running real time on a robot, and all the data is updated every 500ms. So it is pretty fast.

Thank you!
View the Solution FREE for 30 Days
         
           
             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:

           
           
             // I90ControllerDlg.cpp : implementation file
//
#include "atlimage.h"
 
#include "pixmap.h"
#include "imglib.h"
#include "ekfreal.cpp"
//#include "ukfreal.cpp"
//#include "ekfrealass.cpp"
#include "stdafx.h"
 
#include "direct.h"
#include "I90Controller.h"
#include "I90ControllerDlg.h"
#include <iostream>
#include <fstream>
 
using namespace std;
// 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;
}
 
 
//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 8.5 //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.31
#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;
 
	m_Width=IMAGE_WIDTH;  //720  768
	m_Height=IMAGE_HEIGHT; //576  576
	m_Channels=3;
	ismoving=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;
	
	
	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];
	
//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,m_sTimerCnt);
 
	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];
 
	m_iTimerCnt = 0;
	m_MOTSDK.connectRobot ("motcom");
	m_TISDK.connectRobot ("ticom");
	
	GetDlgItem(IDC_CameraUp)->EnableWindow(false);
 
	ReadMat();
	m_bCameraConnected = false;
	musicInPlay = false;
	SetTimer(1, 450, 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;
	
	
 
		
	
	
 
UpdateData(false);
}
 
void CI90ControllerDlg::OnForward() 
{
	// TODO: Add your control notification handler code here
	short desiredWheelSpeed1=-10*CIRCLE_CNT/(2*3.14*wheelR);//left has to be clockwise
	short desiredWheelSpeed2=10*CIRCLE_CNT/(2*3.14*wheelR);//right has to be counterclockwise so it should go forward
 
	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);
 
	
	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); 
}
 
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;
 
	
	
}
 
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);
		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 = ".\\tempSnapShot1.bmp";
				//strName2=".\\tempSnapShot.bmp";
				//image1 = new CImage();
				
				m_vitCtrl.SaveSnapshot(2,strName);
				if (landmark)
				{
					if ((m_Sonar1 <255)&&(distravelled+50 < m_Sonar1))
					{
						OnStop();	
					}
				}
			//	image1->Load(".\\tempSnapShot1.bmp");
			//	image1=&image3;
				//CImage* returnImage = new	CImage();
				
				//Sobel(image1,returnImage);
				
				ifstream infile_Pin1,infile_Pin2;                /* input file */
    char inFileName_Pin1[128]="tempSnapShot1.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)) // 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, "Trei.pgm"); 
 
	   delete []Pin1.img;
	   Pin1.img =NULL;
	   	
 
	
	}
					
			   if (captureresult==0)
			   {
					pData=(unsigned char *)pvData.parray->pvData;
					double dx1,dy1,phi;
				
					//ReadMat();
					
					ImageSegmentation(pData,Back_Dist,blue_Dist,green_Dist,pink_Dist,lila_Dist,orange_Dist);
			   }	
			
	
		//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 = ".\\tempSnapShot.jpg";
	m_vitCtrl.SaveSnapshot(1, 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;
}
void CI90ControllerDlg::ReadMat()// read data from matlab
{
 // TODO: Add your command handler code here
 
	MATFile *pmatFile,*pmatFile1,*pmatFile2,*pmatFile3,*pmatFile4,*pmatFile5;
	mxArray *pMxArray,*pMxArray1,*pMxArray2,*pMxArray3,*pMxArray4,*pMxArray5;
 
	const char *filename="C:\\Documents and Settings\\ogvalent\\Desktop\\I90Controller_V2\\I90Controller\\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\\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\\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\\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\\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\\pinkPattern1D.mat";
	
	pmatFile5 = matOpen(filename5,"r");
	pMxArray5 = matGetVariable(pmatFile5,"pink_dist_1D");
	pink_Dist = (double *)mxGetData(pMxArray5);
 	
 
	matClose(pmatFile5);
	mxFree(pMxArray5);
	mxFree(pmatFile5);
}
detect CI90ControllerDlg::ImageSegmentation(const unsigned char* pBGR,double* Back_Dist,double* blue_Dist,double* green_Dist,double* pink_Dist,double* lila_Dist,double* orange_Dist)
{
		//Define some parameters for segmentation
		char colordetect1;
 
		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 [5];
 
		for(int i=0;i<5;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];
		 }
 
		 // 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++;
				
			 }
 
		 }
		
		 colorlist[0]=numberofbluepixel;
		 colorlist[1]=numberofgreenpixel;
		 colorlist[2]=numberoforangepixel;
		  colorlist[3]=numberoflilapixel;
		  colorlist[4]=numberofpinkpixel;
		  int indexofcolors[6];
		
		int* maxcolorandindex;
		 maxcolorandindex=MaxofVector(colorlist,5);
		 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';
			 }
		 }
		 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;
		
		
		
		
 
		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)
 void CI90ControllerDlg::Sobel(CImage* image,CImage *returnImage)
 {
	 
 
   returnImage->Create(image->GetWidth(),image->GetHeight(), image->GetBPP(),0);
   
   double GX[3][3],GY[3][3];
   int		sumX = 0;
   int		sumY = 0;
   int		SUM = 0;
 
   // Masks //////////////////////////////////////
   //X//
   GX[0][0] = -1; GX[0][1] = 0; GX[0][2] = 1;
   GX[1][0] = -2; GX[1][1] = 0; GX[1][2] = 2;
   GX[2][0] = -1; GX[2][1] = 0; GX[2][2] = 1;
   //Y//
   GY[0][0] =  1; GY[0][1] = 2; GY[0][2] = 1;
   GY[1][0] =  0; GY[1][1] = 0; GY[1][2] = 0;
   GY[2][0] = -1; GY[2][1] = -2; GY[2][2] = -1;
   //int r=image->GetHeight();
	Ypix=0;
   for(int Ypix; Ypix < image->GetHeight(); Ypix++)  
   {
	for(int X=0; X < image->GetWidth(); X++)  
	{
	     sumX = 0;
	     sumY = 0;
 
	     if(Ypix==0 || Ypix == image->GetHeight()-1)
		SUM = 0;
	     else if(X==0 || X == image->GetWidth()-1)
		SUM = 0;
	     else
	     {
	         for(int I=-1; I<=1; I++)  
			 {
				for(int J=-1; J<=1; J++) 
					{
 
					int piX = J + X;
					int piY = I + Ypix;
 
					COLORREF pixVal = image->GetPixel(piX,piY);
 
					int R = GetRValue(pixVal);
					int G = GetGValue(pixVal);
					int B = GetBValue(pixVal);
 
					int NC = (R+G+B)/3;
 
					sumX = sumX + (NC) * GX[J+1][I+1];
					sumY = sumY + (NC) * GY[J+1][I+1];  
 
					}
	         }
                SUM = abs(sumX) + abs(sumY);
          }
         if(SUM>255) SUM=255;
         if(SUM<0) SUM=0;
         int newPixel = (255 - (unsigned char)(SUM));
 
         COLORREF newPixCol =  RGB(newPixel,newPixel,newPixel);
         returnImage->SetPixel(X,Ypix,newPixCol) ;
         }
   }
  // return returnImage;
 
 }
 
//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;
 
    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);
		
		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=10;
		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 j=1; j <= Pin_t.Width;j++)
		{
			for(int k =1; k <= Pin_t.Height;k++)
			{
				IMG_temp_matrix_2[k][j]=IMG_temp_matrix[k-1][j-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;
							}
							/*
							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 //
 
		
        
        //output the calculated 2D matrix to original 1D
		numberpixel = 0;
        pixelCnt = 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][j-1]==255)&&(IMG__gradient_matrix[k][j+1]==255)) // && (IMG__gradient_matrix[k+1][j]==255) && (IMG__gradient_matrix[k-1][j]==255) && (IMG__gradient_matrix[k+1][j+1]==255) && (IMG__gradient_matrix[k-1][j+1]==255))
					{
						numberpixel ++;
						if ((numberpixel > 0)&& (numberpixel < 800))
							{
								IMG__gradient_matrix[k][j]=0;	
							}
						landmark = true;	
					}
					
                    Pin_t.img[pixelCnt] = (unsigned char) (IMG__gradient_matrix[k][j]);
                    pixelCnt++;
              }
         }
 
        WritePGM(outfile_Pout, Pin_t);
		FreeDynamicArray(IMG__gaussian_matrix);
		FreeDynamicArray(IMG_temp_matrix);
		FreeDynamicArray(IMG_temp_matrix_2);
		FreeDynamicArray(IMG_final_matrix);
		FreeDynamicArray(IMG__sobel_matrix);
		FreeDynamicArray(IMG__gradient_matrix);
		FreeDynamicArray(IMG__direction_matrix);
	
 
 }

           
         
20091021-EE-VQP-81 - Hierarchy / EE_QW_3_20080625
Hall of Fame
1. jkr621,554
2. Infinity08428,105
3. evilrix386,063
4. itsmeandn...345,939
5. mrjoltcola136,370
6. pgnatyuk80,095
7. Zoppo69,160
8. alb6660,430
9. AndyAins...48,452
10. trinitrotolu...48,102
11. DanRollins45,961
12. Let_Me_Be43,756
13. ozo40,128
14. Gideon737,720
15. jaime_oliv...36,480
16. pepr30,816
17. ikework23,327
18. sunnycoder20,168
19. masheik18,698
20. LordOfPo...18,490
21. JohnGaby18,296
22. johnnash...17,769
23. josgood17,000
24. abel16,556
25. Gurudenis16,260
1. jkr7,897,645
2. itsmeandn...2,356,347
3. Infinity081,955,516
4. Axter1,467,234
5. evilrix1,041,001
6. nietod859,677
7. AlexFM732,762
8. DanRollins729,138
9. jaime_oliv...725,383
10. rstaveley524,876
11. grg99392,524
12. efn329,300
13. chensu313,112
14. AlexViroc...263,393
15. Zoppo255,353
16. Salte247,544
17. ikework219,121
18. wayside215,557
19. jhance207,462
20. rajeev_d...207,388
21. ambience183,231
22. AndyAins...183,072
23. sunnycoder180,898
24. bcladd171,128
25. SteH170,387
Heap problem

Asked by Gabriela1 in C++ Programming Language, Microsoft Visual C++.Net

About this solution
