#ifdef _CH_
#pragma package <opencv>
#endif

#include <..\SOCKET\SOURCES\pthreads\pthread.h>
//#include "ipp.h" //intelio primityvai
//#include "ippcv.h" //intelio primityvai
#include <opencv2/video/background_segm.hpp>
#include "opencv2/imgproc/imgproc_c.h"
#include <opencv2/objdetect/objdetect.hpp>
    #include <opencv2/features2d/features2d.hpp>
    #include <opencv2/core/core.hpp>
    #include <opencv2/highgui/highgui.hpp>
    #include <opencv2/legacy/legacy.hpp>
    #include <opencv2/legacy/compat.hpp>
    #include <opencv2/flann/flann.hpp>
    #include <opencv2/calib3d/calib3d.hpp>
    #include <opencv2/nonfree/features2d.hpp>
    #include <opencv2/nonfree/nonfree.hpp>
#include "BlobResult.h"
#include "BlobExtraction.h"
#include "blob.h"
#include "BlobContour.h"
#include "BlobLibraryConfiguration.h"
#include <iterator>
#include "../SOCKET/client1.h"
#include "h.h"   //LPT
#include "rs232.h"
#include "eProDas.h"
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <signal.h>
#include <windows.h> //
#include <stdio.h>
#include <conio.h>		// reikia _kbhit()
#include <direct.h>		// reikia mkdir()
#include <vector>
#include <string>
#include "opencv\cv.h"
#include "opencv\cvaux.h"
#include "opencv\highgui.h"
#include <cstdlib>
#include <sstream>
#include <fstream>
#include <iostream> //gal greiciau, paskui gal ismesti
#include "opencv2/video/tracking.hpp"
#include <math.h>
#include <time.h>
#include <dos.h>
#include "f_graph.h" //basic graphic and some additional functions
#define s2 "                    " //20 pix
#define short  u8
#pragma comment(lib, "inpout32.lib")
#ifdef _CH_
#pragma package <opencv>
#endif
//**  Pavadinimu priskirimas ***************************************
#define CV_NO_BACKWARD_COMPATIBILITY
#define stopvar     0
#define pirmyn      1
#define atgal       2
#define kairen      3
#define desinen     4
#define vkairen     5
#define vdesinen    6
#define akairen     7
#define adesinen    8

typedef __stdcall BYTE (*TInp32Func)(WORD PortAddr);
typedef __stdcall BYTE (*TOut32Func)(WORD PortAddr, BYTE Data);
typedef __int64 LONGLONG;

using namespace cv;  using namespace std;// using namespace cvb;

//############################ Global variables  #############################
//############ Haar Cascade kintamieji. ############################
const char *veidasCascadeFilename = "haarcascade_frontalface_alt.xml";
const char *veidasPrCascadeFilename = "haarcascade_profileface.xml";// Haar Cascade veidas profilis.
const char *kunasCascadeFilename = "haarcascade_fullbody.xml";// Haar Cascade kunas.
const char *kunasUpCascadeFilename = "haarcascade_upperbody.xml";// Haar Cascade virsutine kuno dalis.
const char *pestiejiCascadeFilename = "hogcascade_pedestrians.xml";// Haar Cascade pestieji

int SAVE_EIGENFACE_IMAGES = 1;		// Set to 0 if you dont want images of the Eigenvectors saved to files (for debugging).
//#define USE_MAHALANOBIS_DISTANCE	// You might get better recognition accuracy if you enable this.

//################ Socket jungimosi kintamieji ############################
int prisijungimas; //prisijungimo Nr prie 'JULIUS'
static char *naujasZodis; static char *senasZodis;
//int zodzio_ilgis;
//char buftmpzodis[MAXLINELEN];
//############# Valdymo kintamieji ###########################
int robotorezimas;  //algoritmo nr
int darborezimas=0;       //
int ekranorezimas=2; // darbiniai ekrano parametrai 1=>atvaizduoti ; 0 =>drausti
pthread_t pthread_Mechanika; //mechanikos aptarnavimo paprograme lygiagreciame procese

// laikino saugojimo buferiai #############################
    char tmptxt[32];    char  buferis[32];   char *tmptxto;    char buftmp[32];
// Kameru parametrai #############################
int kamera1=1, kamera2=2;  // kameru nr sistemoj
int plotis=320, aukstis=240; // 320, 320 kameros rezolke su maza rezolke greiciau

//eProdas SSC32 #############################
 //eProdas optiniu ir mechaniniu davikloiu visuma ir ssc32 4 inputai
uint8_t daviklisSSC = 0;
uint8_t daviklisePD = 0;
// Roboto akiu Servu kintamieji #############################
int servo_greitisX =400, servo_greitisY =300;
int servo_xmin =700, servo_ymin =1300, servo_xmax =2100, servo_ymax =2400;
int servo_xk =1400,  servo_xd =1400,  servo_xksen =1400,  servo_xdsen =1400,  servo_x =1400;//x=1850 vidurys
int servo_yk =1850,  servo_yd =1850,  servo_yksen =1850,  servo_ydsen =1850,  servo_y =1850;//y=1400 vidurys
// Vaziuokles kintamieji #########################
uint8_t vazDR;

//eProDas kintamieji #############################
int StatuseProDas; //
const unsigned int DeviceIdx = 0;
int Status;
unsigned char NewValue;
uint8_t kauke; //kaip glogalus paskui gal pakeisti kaip lokalu kintamaji
uint8_t  eProInputE;
uint8_t  eProInputD;
unsigned int Pin;
unsigned int InitialState;
bool RA0=0,RA1=0,RA2=0,RA3=0,RA4=0,RBA=0;
bool RB0=0,RB1=0,RB2=0,RB3=0,RB4=0,RB5=0,RB6=0,RB7=0;
bool RC0=0,RC1=0,RC2=0,RC6=0,RC7=0;
bool RE0=0,RE1=0,RE2=0,RE3=0;
//### LPT kintamieji #############################
int lptportas = 888; //tai =0x378 lpt portas
//kazkas gal bus pakabinta ant LPT
//### RS232 kintamieji #############################
    int i, n,
      cport_nr=1,        /* /0=>(dev/ttyS0 unix) ir (COM1  windows), o cia pas mane com2*/
      bdrate=115200;       /* 115200 greitis */
unsigned char buf[256]; //rs232 buferis buvo originale pasiulytas= 4096 ????
//## Daiktu paieska###########################

// Tasko trekingas **************************
IplImage *image = 0, *grey = 0, *prev_grey = 0, *pyramid = 0, *prev_pyramid =0, *swap_temp;

CvPoint origin;
CvRect selection;
int win_size = 10;
const int MAX_COUNT = 500;
CvPoint2D32f* points[2] = {0,0}, *swap_points;
char* status = 0;
int skaitliukas = 0;
int need_to_init = 0;
int night_mode = 0;
int flags = 0;
int add_remove_pt = 0;
CvPoint pt;
//### Judesio analyze ########################
// various tracking parameters (in seconds)
const double MHI_DURATION = 1;
const double MAX_TIME_DELTA = 0.5;
const double MIN_TIME_DELTA = 0.05;
// number of cyclic frame buffer used for motion detection
// (should, probably, depend on FPS)
const int N = 4;

// ring image buffer
IplImage **bufff = 0;
int last = 0;

// temporary images
IplImage *mhi = 0; // MHI
IplImage *orient = 0; // orientation
IplImage *mask = 0; // valid orientation mask
IplImage *segmask = 0; // motion segmentation map
CvMemStorage* storage = 0; // temporary storage

//### Veido atpazinimas  #############################
char senasVeidas[20]; //
char naujasVeidas[20]; //
int keyPressed = 0;
int noofegineface;
int nooftraingimage;
float detectiontime,confi;
int accuracy;
int detectfacetime;
//int apsvietimoLygis;

IplImage ** faceImgArr        = 0; // array of face images
CvMat    *  personNumTruthMat = 0; // array of person numbers
//#define	MAX_NAME_LENGTH 256		// Give each name a fixed size for easier code.
//char **personNames = 0;			// array of person names (indexed by the person number).
vector<string> personNames;			// array of person names (indexed by the person number).
int faceWidth = 120;	// Default dimensions for faces in the face recognition database.
int faceHeight = 90;	//	"		"		"		"		"		"		"		"
int nPersons                  = 0; // the number of people in the training set.
int nTrainFaces               = 0; // the number of training images
int nEigens                   = 0; // the number of eigenvalues
IplImage * pAvgTrainImg       = 0; // the average image

IplImage ** eigenVectArr      = 0; // eigenvectors
CvMat * eigenValMat           = 0; // eigenvalues
CvMat * projectedTrainFaceMat = 0; // projected training faces

   HWND hWnd;
    IplImage *camImg;     //
    IplImage *camImg1;
     IplImage *gylis;
    IplImage *shownImg;
	IplImage *shownImg1;

CvCapture* camera = 0;CvCapture*  camera1 = 0 ;	        //  kameru irenginiai

// Strukturos #################################################
//employee
struct employee
	{
      char  id[256];
   	};
struct employee emp;
int recsize=sizeof(emp);
//new user
struct newuserdata
	{
      char  id[256];
      char  name[256];
      char  dept[256];
      char  passw[256];
	};
struct newuserdata nu;
int recsize1=sizeof(nu);
/*
//login
struct login
 {
     char loginname[256];
     char logindate[256];
     char logintime[256];
};

struct login logi;
int recsize2=sizeof(logi);
/logout
struct logout
 {
     char logoutname[256];
     char logoutdate[256];
     char logouttime[256];
 };

struct logout logo;
int recsize3=sizeof(logo);
*/

//####### Funcijos-paprogrames  #################################################
//************** Elgesio *********************************************
int Pradzia ();
int Klaidziojimas();
int DialogasRobRezimas();
int PazistamiVeidai();
int PazistamiDaiktai();

//*********************************************************************
int RAM_minimize_refresh();
void kamkalibr (void);
void eproaktyv ();
char *zodzio_reiksme(); //JULIUS zodis
void *KalboaAtpazinimas_demo ();
void ssc32inDemo();

double compareSURFDescriptors( const float* d1, const float* d2, double best, int length );
int naiveNearestNeighbor( const float* vec, int laplacian,const CvSeq* model_keypoints,
                        const CvSeq* model_descriptors );

void findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
                const CvSeq* imageKeypoints , const CvSeq* imageDescriptors, vector<int>&ptpairs ) ;

int locatePlanarObject( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
            const CvSeq* imageKeypoints, const CvSeq* imageDescriptors,
            const CvPoint src_corners[4], CvPoint dst_corners[4] );

IplImage* daikto_isskyrimas(void);
int daiktas(int kaip_ieskoti, char** argv);
int trekingas( int argc, char** argv );
void on_mouse( int event, int x, int y, int flags, void* param );
void on_mouserect( int event, int x, int y, int, void* );
int judesys1(int judesiu); // fiksina judesi (judesiu kiekis po kurio pap baigia darba)
void  update_mhi( IplImage* img, IplImage* dst, int diff_threshold );

int  objsekimas( void);
int  FindBall(IplImage* Img);
void Counter(IplImage* img);
CvPoint AntiCOG(IplImage* img);
CvPoint COG(IplImage* img);
void sum_rgb( IplImage* src, IplImage* dst );
int vaziuokle(uint8_t vazrezimas,clock_t vazONlaikas);
void paieska_servo();
void rndservo();
int  judesys(int judesiu,time_t laikasveikimo);
void sysinitas(); //sistemos pradinis pasikrovimas
uint8_t ssc32in ();//SSC32 davikliu apklausa
uint32_t vazdavikliai();//vaziuokles davikliu apklausa
void *Mechanika(void *);//

void lptout(uint8_t lptportas,uint8_t lptreiksme );
int  stereoBM();
int  stereoGC();

int VeidoAtpazinimas();
void akys_servo();
int  VeidoProfilis();

int  meniurobrez();
int  meniu();
void meniuivedimas();
void meniurez();
void menumenustatistika();
void meniuapie();
//void meniustereo();
void meniupabaiga();
void menustatistika();
void meniupagr(void);
void meniurez();
void meniuStereorez();
int meniuStereo();
int meniusistema();
void meniusisrez();
int meniudialogas();
void meniuDialogasrez();
void learn(char *szFileTrain);
void doPCA();
void storeTrainingData();
int  loadTrainingData(CvMat ** pTrainPersonNumMat);
int  findNearestNeighbor(float * projectedTestFace);
int  findNearestNeighbor(float * projectedTestFace, float *pPanasumas);
int  loadFaceImgArray(char * filename);
void recognizeFileList(char *szFileTest);
void ivedimasiscam(void);
IplImage* getCameraFrame(void);
IplImage* getCameraFrame1(int kamnr);
IplImage* convertImageToGreyscale(const IplImage *imageSrc);
IplImage* convertImageToGreyscale1(const IplImage *imageSrc);
IplImage* cropImage(const IplImage *img, const CvRect region);

IplImage* resizeImage(const IplImage *origImg, int newWidth, int newHeight);
IplImage* convertFloatImageToUcharImage(const IplImage *srcImg);
void saveFloatImage(const char *filename, const IplImage *srcImg);
CvRect detectFaceInImage(const IplImage *inputImg, const CvHaarClassifierCascade* cascade );
CvMat* retrainOnline(void);

//#define _IplImage IplImage
//__declspec(dllimport);
//###############################################################################################################################
//********** Pagrindine programa ***********************************************************************
//#############################################################################################################################
int main()
{   SetPriorityClass(GetCurrentProcess(),REALTIME_PRIORITY_CLASS);//
    SetThreadPriority(GetCurrentThread(),THREAD_PRIORITY_TIME_CRITICAL);
      cv::initModule_nonfree();//importuoja SURF-a ,  kuris mokamas
     gf.setTitle("Roboto veido atpazinimo sistema + SSC32");
     gf.gotoxy(1,10);gf.setColor(72);
     printf((char *) "  VISUOSE KAMEROS REZIMUOSE                        \n");
     printf((char *) "  ISEJIMAS IPAGRINDINI VALDYMO MENIU :             \n");
     printf((char *) "  Aktynuoti 'pele' kameros langa ir spausti [ m ]  \n");
     gf.setColor(128);
     hWnd=FindWindow( NULL,"Roboto veido atpazinimo sistema + SSC32");
    //SetThreadPriority(hWnd,THREAD_PRIORITY_TIME_CRITICAL);
     SetWindowPos(hWnd,HWND_TOP,650,1,630,800, SWP_FRAMECHANGED);

  sysinitas(); //sistemos inicializacija
  Pradzia (); // roboto paleidimas su kalbos atpazinimu

  	cvNamedWindow("Desine" ,CV_WINDOW_AUTOSIZE);cvMoveWindow("Desine", 10, 10); //paslinkim antra langa kad nebutu viens anr kito
    hWnd=FindWindow( NULL,"Desine");SetForegroundWindow(hWnd);
    SetThreadPriority(hWnd,THREAD_PRIORITY_TIME_CRITICAL);//GetCurrentThread()THREAD_PRIORITY_NORMAL//

    cvNamedWindow("Kaire", CV_WINDOW_AUTOSIZE);cvMoveWindow("Kaire", (plotis+5), 10); //paslinkim antra langa kad nebutu viens anr kito
    hWnd=FindWindow( NULL,"Kaire"); SetForegroundWindow(hWnd);
    SetThreadPriority(hWnd,THREAD_PRIORITY_TIME_CRITICAL);//THREAD_PRIORITY_NORMAL//


while (1){//Pagrindinis programos ciklas

robotorezimas=DialogasRobRezimas();

if (robotorezimas<1){keyPressed='m';}                         //meniu
else if (robotorezimas==1){PazistamiVeidai();}
else if (robotorezimas==2){PazistamiDaiktai();}
else if (robotorezimas==3){Klaidziojimas();}
else if (robotorezimas==4){Klaidziojimas();}
else if (robotorezimas==5){PazistamiDaiktai();}
else if (robotorezimas==6){PazistamiDaiktai();}
else if (robotorezimas==7){PazistamiDaiktai();}
else if (robotorezimas==8){PazistamiDaiktai();}
else if (robotorezimas==9){PazistamiDaiktai();}
else if (robotorezimas==10){PazistamiDaiktai();}

if (keyPressed=='m') {ekranorezimas=2;RAM_minimize_refresh();meniupagr();  }//jei "m" iseiti i meniu

}//Pagrindines main programos While ciklo pabaiga
    return EXIT_SUCCESS;
}; // main pabaiga
//######################################################################################################

//######################################################################################################
//*********** meniu *************************************************************************
int meniu(void) { gf.makeSkeleton();
    gf.setColor(128);
    gf.clrscr();
     gf.headerLine((char *) "  Roboto veido atpazinimo sistema + SSC32  COM 2 Greitis: 115200");
     gf.showLeftSide((char *)"  MENIU",1);
     gf.showLeftSide((char *)" [1] Naujas");
     gf.showLeftSide((char *)"");
     gf.showLeftSide((char *)" [2] Atpazinimas");
     gf.showLeftSide((char *)"");
     gf.showLeftSide((char *)" [3] STEREO");
     gf.showLeftSide((char *)"");
     gf.showLeftSide((char *)" [4] Laisva");
     gf.showLeftSide((char *)"");
     gf.showLeftSide((char *)" [5] Dialogas");
     gf.showLeftSide((char *)"");
     gf.showLeftSide((char *)" [6] Sistema");
     gf.showLeftSide((char *)"");
     gf.showLeftSide((char *)" [7] Statistika");
     gf.showLeftSide((char *)"");
     gf.showLeftSide((char *)" [8] Apie ...");
     gf.showLeftSide((char *)"");
     gf.showLeftSide((char *)" [9] Iseiti     ");
     gf.footerLine((char *)" Isrinkite : [1-9] ir spausti - Enter ");

     keyPressed=999;
     int intUserChoice = 0;
     cin>>intUserChoice;
     while(intUserChoice<1 || intUserChoice>9)
       {intUserChoice = 0;  printf("\a\a\a");
                           gf.warning((char *)"KLAIDA, jus turite pasirinkti [1-9!");
                            gf.wait(1000);
                           printf("\a\a\a");
                           gf.footerLine((char *)" Isrinkite : [1-9] ir spausti - Enter ");
                           cin>>intUserChoice;
                             };

     return intUserChoice;}
//###################################################################################################### stereoBM gylis
 //=========================meniurobrez ======================//
int meniurobrez()
  {          gf.setColor(128);
        gf.clrscr();
     gf.headerLine((char *) "  Roboto veido atpazinimo sistema + SSC32  COM 2 Greitis: 115200");
     gf.showRightSide((char *)" [0]    Grizti i pagrindini Meniu",1);
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [1]    Robotas pagr. programa");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [2]    Robotas 1 programa");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [3]    Apvalaus objekto sekimas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [4]    Judesio detektavimas 1");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [5]    Daikto isskirimas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [6]    Daikto paieska");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [7]    Tasko sekimas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [8]    Judesio detektavimas 2 ");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [9]    Iseiti is programos");
     gf.footerLine((char *)"Prasau isrinkti darbo subrezima ir spausti ENTER [1-9]: ");
     int UserChoice = 11;
     scanf("%d",&UserChoice);
     keyPressed=999;
     if (UserChoice>9 ){  UserChoice = 9;
                           gf.warning((char *)"KLAIDA: isrinkti darbo rezima ribose[1-9]!");
                           gf.wait(1000);
                           gf.footerLine((char *)"Prasau isrinkti darbo rezima ir spausti ENTER [1-9]: ");

                           }


     return UserChoice;}
 //=========================meniurobrez ======================//
//######################################################################################################
//######################################################################################################
 //=========================STEREO ======================//
int meniuStereo()
  {          gf.setColor(128);
        gf.clrscr();
      gf.headerLine((char *) "  Roboto veido atpazinimo sistema + SSC32  COM 2 Greitis: 115200");
     gf.showRightSide((char *)" [0]    Grizti i pagrindini Meniu",1);
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [1]    StereoBM gylis");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [2]    StereoGC gylis");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [3]     XXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [4]     XXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [5]     XXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [6]     XXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [7]     XXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [8]     XXXXXXXXXXXXXXXXXXXXXXXX ");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [9]    Iseiti is programos");
     gf.footerLine((char *)"Prasau isrinkti darbo subrezima ir spausti ENTER [1-9]: ");
     int UserChoice = 10;
     keyPressed=999;
     scanf("%d",&UserChoice);
     if (UserChoice>9 ){  UserChoice = 9;
                           gf.warning((char *)"KLAIDA: isrinkti darbo rezima ribose[1-9]!");
                           gf.wait(1000);
                           gf.footerLine((char *)"Prasau isrinkti darbo rezima ir spausti ENTER [1-9]: ");
                           }
     return UserChoice;}
 //========================= STEREO ======================//
//######################################################################################################

int meniusistema()
  {    gf.setColor(128);
        gf.clrscr();
       gf.headerLine((char *) "  Roboto veido atpazinimo sistema + SSC32  COM 2 Greitis: 115200");
     gf.showRightSide((char *)" [0]    Grizti i pagrindini Meniu",1);
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [1]    Stereo, kameru kalibravimas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [2]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [3]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [4]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [5]    SSC32 ivedimas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [6]    'JULIUS' kalbos atpazinimas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [7]    'eProDas' valdiklio isvadai");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [8]    Isoriniu interfeisu initas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [9]    Iseiti is programos");
     gf.footerLine((char *)"Prasau isrinkti darbo subrezima ir spausti ENTER [1-9]: ");
     int UserChoice = 10;
     keyPressed=999;
     scanf("%d",&UserChoice);
     if (UserChoice>9 ){  UserChoice = 9;
                           gf.warning((char *)"KLAIDA: isrinkti darbo rezima ribose[1-9]!");
                           gf.wait(1000);
                           gf.footerLine((char *)"Prasau isrinkti darbo rezima ir spausti ENTER [1-9]: ");
                           }
     return UserChoice;}

void meniuivedimas(){ ivedimasiscam();}
//**************************************************************************************
 //========================= Dialogas ======================//
//######################################################################################################

int meniudialogas()
  {    gf.setColor(128);
        gf.clrscr();
       gf.headerLine((char *) "  Roboto veido atpazinimo sistema + SSC32  COM 2 Greitis: 115200");
     gf.showRightSide((char *)" [0]    Grizti i pagrindini Meniu",1);
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [1]    Dialogas roboto darbo rezimas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [2]    Dialogas daikto paieska    ");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [3]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [4]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [5]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [6]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [7]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [8]    XXXXXXXXXXXXXXXXXXXXXXXXXXX");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)" [9]    Iseiti is programos");
     gf.footerLine((char *)"Prasau isrinkti darbo subrezima ir spausti ENTER [1-9]: ");
     int UserChoice = 10;
     keyPressed=999;
     scanf("%d",&UserChoice);
     if (UserChoice>9 ){  UserChoice = 9;
                           gf.warning((char *)"KLAIDA: isrinkti darbo rezima ribose[1-9]!");
                           gf.wait(1000);
                           gf.footerLine((char *)"Prasau isrinkti darbo rezima ir spausti ENTER [1-9]: ");
                           }
     return UserChoice;}


//#################################################################################################
void meniupagr(void)
{
//mainwin :

        while(1){
            switch(meniu())
                     {case 1: {meniuivedimas(); break;}
                      case 2: {meniurez(); break;}
                      case 3: {meniuStereorez(); break;}
                      case 4: {gf.clrscr(); break;}
                      case 5: {meniuDialogasrez(); break;}
                      case 6: {meniusisrez(); break;} //
                      case 7: {menustatistika(); break;} //
                      case 8: {meniuapie(); break;} //
                      case 9: {meniupabaiga(); break;}

                      }

      }
    system("PAUSE");
}// meniupagr pabaiga

//meniurobrez Menu ##################################################################################################
void meniurez()
{ int testi=1;
     while(testi==1){
        switch(meniurobrez()){
                      case 0: {gf.clrscr();testi=0; break;} //
                      case 1:{ VeidoAtpazinimas();RAM_minimize_refresh(); break;}
                      case 2: { VeidoProfilis();RAM_minimize_refresh(); break;}
                      case 3: {objsekimas(); RAM_minimize_refresh();break;}
                      case 4: {judesys(1000,0); RAM_minimize_refresh();break;} //
                      case 5: {daikto_isskyrimas();RAM_minimize_refresh();break;}
                      case 6: {PazistamiDaiktai();RAM_minimize_refresh(); break;} //
                      case 7: {trekingas(1,0);RAM_minimize_refresh(); break;} //
                      case 8: {judesys1(1000);RAM_minimize_refresh(); break;} //
                      case 9:{meniupabaiga(); }
                      break;}
    }

}

// Menu Stereo ##################################################################################################
void meniuStereorez()
{ int testi=1;
     while(testi==1){
        switch(meniuStereo()){
                      case 0: {gf.clrscr();testi=0; break;} //
                      case 1: {stereoBM(); RAM_minimize_refresh();break;}
                      case 2: {stereoGC(); RAM_minimize_refresh();break;}
                      case 3: gf.clrscr(); break;
                      case 4: gf.clrscr(); break;
                      case 5: gf.clrscr(); break;
                      case 6: gf.clrscr(); break;
                      case 7: gf.clrscr(); break;
                      case 8: gf.clrscr(); break;
                      case 9:{meniupabaiga(); }
                      break;}
    }

}

// Menu Dialogo rezimas ##################################################################################################
void meniuDialogasrez()
{ int testi=1;
     while(testi==1){
        switch(meniudialogas()){
                      case 0: {gf.clrscr();testi=0; break;} //
                      case 1: {testi=DialogasRobRezimas(); RAM_minimize_refresh();break;}
                      case 2: {gf.clrscr();break;}
                      case 3: gf.clrscr(); break;
                      case 4: gf.clrscr(); break;
                      case 5: gf.clrscr(); break;
                      case 6: gf.clrscr(); break;
                      case 7: gf.clrscr(); break;
                      case 8: gf.clrscr(); break;
                      case 9:{meniupabaiga(); }
                      break;}
    }

}

//Systemos rez Menu ##################################################################################################
void meniusisrez()
{ int testi=1;
    keyPressed=999;
     while(testi==1){
        switch(meniusistema()){
                      case 0: {gf.clrscr();testi=0; break;}//
                      case 1: kamkalibr(); break;
                      case 2: gf.clrscr(); break;
                      case 3: gf.clrscr(); break;
                      case 4: {gf.clrscr(); break;} //
                      case 5: {gf.clrscr(); ssc32inDemo(); break;}
                      case 6: {gf.clrscr(); KalboaAtpazinimas_demo (); break; } //
                      case 7: {eproaktyv (); break;} //
                      case 8: {CloseComport(1);
                            gf.showRightSide((char *)" PERKRAUNAMI SISTEMOS INTERFEISAI !!!",1);
                            gf.showRightSide((char *)"________________________________________");
                            gf.showRightSide((char *)" ");
                            gf.showRightSide((char *)"  Rezoliucija : ");printf(" %dx%d.",plotis,aukstis);
                            gf.showRightSide((char *)" ");
                            printf ("  OpenCV  %s (%d.%d.%d)\n", CV_VERSION,CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
                            gf.showRightSide((char *)"________________________________________");
                            gf.showRightSide((char *)"");
                            gf.showRightSide((char *)"     ROBOTO AUTORIUS :");
                            gf.showRightSide((char *)"");
                            gf.showRightSide((char *)"     Arnoldas Cerniakovas");
                            gf.showRightSide((char *)"");
                            gf.showRightSide((char *)"           KAUNAS 2012");
                            gf.showRightSide((char *)"");
                            gf.showRightSide((char *)"             LIETUVA");
                            gf.showRightSide((char *)"");
                            gf.showRightSide((char *)"       arnoldasc@yahoo.com \n");
                            eProDas_CloseHandle();
                            gf.wait(100);
                            //komanda_pause_recog(prisijungimas);
                            //komanda_die(prisijungimas);
                            sysinitas();
                            gf.wait(10000); break;} //
                      case 9:{meniupabaiga();}
                      break;}
    }

}

//#################################################################################################
void menustatistika(void)
{
     gf.showRightSide((char *)"  Parametrai :",1);
     printf (" OpenCV  %s (%d.%d.%d)\n", CV_VERSION,CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
     gf.showRightSide((char *)"=========================================");
     gf.showRightSide((char *)"Pazistamu skaicius =  ");printf(" %d",noofegineface);
	 gf.showRightSide((char *)"Nepazistamu skaicius =  ");printf("%d",nooftraingimage);
	 gf.showRightSide((char *)"Panasumas =");printf(" %f",confi);
     gf.showRightSide((char *)"Tikslumas:");printf(" %d%%",accuracy);
     gf.showRightSide((char *)"Bendras laikas:");printf(" %.1fms average.",detectiontime );
     gf.showRightSide((char *)"Rezoliucija");printf(" %dx%d.",plotis,aukstis);
     gf.showRightSide((char *)"Veidu sk.:");printf(" %d ms",detectfacetime);
     gf.footerLine((char *)"Pauze 10 sekundziu perziurai");
     gf.wait(10000);

};


//######################################################################################################
void meniuapie(void)
{   gf.setColor(128);
     gf.showRightSide((char *)"        Apie roboto, programa... ",1);
     gf.showRightSide((char *)"=========================================");
     gf.showRightSide((char *)"Dvi WEB kameros: valdymas SSC32 valdikliu");
     gf.showRightSide((char *)"Vaziuokles valdymas: eProDas arba LPT");
     gf.showRightSide((char *)"Optiniai dav.: Atmega8 i SSC32 4 inputus");
     gf.showRightSide((char *)"Sist.darb.: parametru modulis: Atmega32");
     gf.showRightSide((char *)"Veidu atpazinimo algoritmai : OpenCV");
     gf.showRightSide((char *)"        Versija 0.5");
     gf.showRightSide((char *)"        Kaunas 2012");
     gf.showRightSide((char *)"=========================================");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)"  Programavo:");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)"       Arnoldas Cerniakovas");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)"         arnoldasc@yahoo.com"   );
     gf.footerLine((char *)"Pause 10 sekundziu perziurai");
      gf.wait(10000);}
//Exit menu######################################################################################################
void meniupabaiga(void)
{    cvDestroyAllWindows();
     cvReleaseCapture( &camera );  cvReleaseCapture( &camera1);
     int servo_greitisX =400, servo_greitisY =300;servo_y=1850; servo_x =1400;akys_servo();Sleep(2000);// vidurys
    cprintft(cport_nr,"#0 P0 S0 |");   SendByte(cport_nr, 0x0D);cprintft(cport_nr,"#7 P0 S0 |"); SendByte(cport_nr, 0x0D);

       WinExec("eSpeak.exe -v lv+m4 viyszo....gearroo",1);
     gf.showRightSide((char *)"VISO GEROOOOOOO ....",1);
     gf.warning((char *)"Programa uzdaroma, Aciu !!!");
    // komanda_resume_recog(prisijungimas);
    // komanda_terminate_recog(prisijungimas);
    do_disconnect(prisijungimas);
    eProDas_CloseHandle();
     CloseComport(1);
     exit(1);}

// ############ greiciau atlaisvina RAM-a   ###############################################
int RAM_minimize_refresh()
{

                    hWnd=FindWindow( NULL,"Roboto veido atpazinimo sistema + SSC32");
                    ShowWindow(hWnd, SW_MINIMIZE);

                   // fastFree( ptr);
                    ShowWindow(hWnd, SW_MAXIMIZE);
                    SetWindowPos(hWnd,HWND_TOPMOST,650,1,630,800, SWP_DRAWFRAME);
}


// Save all the eigenvectors as images, so that they can be checked.
void storeEigenfaceImages()
{   // Store the average image to a file
	gf.footerLine((char *)"Saving the image of the average face as 'out_averageImage.bmp'.");
	cvSaveImage("out_averageImage.bmp", pAvgTrainImg);
	// Create a large image made of many eigenface images.
	// Must also convert each eigenface image to a normal 8-bit UCHAR image instead of a 32-bit float image.
	gf.footerLine((char *)"Saving the ");printf("%d",nEigens);printf(" eigenvector images as 'out_eigenfaces.bmp'");
	noofegineface=nEigens;
    if (nEigens > 0) {
		// Put all the eigenfaces next to each other.
		int COLUMNS = 8;	// Put upto 8 images on a row.
		int nCols = min(nEigens, COLUMNS);
		int nRows = 1 + (nEigens / COLUMNS);	// Put the rest on new rows.
		int w = eigenVectArr[0]->width;
		int h = eigenVectArr[0]->height;
		CvSize size;
		size = cvSize(nCols * w, nRows * h);
		IplImage *bigImg = cvCreateImage(size, IPL_DEPTH_8U, 1);	// 8-bit Greyscale UCHAR image
		for (int i=0; i<nEigens; i++) {
	// Get the eigenface image.
			IplImage *byteImg = convertFloatImageToUcharImage(eigenVectArr[i]);
	// Paste it into the correct position.
			int x = w * (i % COLUMNS);
			int y = h * (i / COLUMNS);
			CvRect ROI = cvRect(x, y, w, h);
			cvSetImageROI(bigImg, ROI);
			cvCopyImage(byteImg, bigImg);
			cvResetImageROI(bigImg);
			cvReleaseImage(&byteImg);
		}
		cvSaveImage("out_eigenfaces.bmp", bigImg);
		cvReleaseImage(&bigImg);
	}
}
//######################################################################################################
// Train from the data in the given text file, and store the trained data into the file 'facedata.xml'.
void learn(char *szFileTrain)
{   int i, offset;
	// load training data
	gf.footerLine((char *)"Treniravimo paveiksl. ivedimas.");printf("'%s'",szFileTrain);
	nTrainFaces = loadFaceImgArray(szFileTrain);
	gf.footerLine((char *)"Got ");printf("%d",nTrainFaces);printf(" Treniruojami paveiksl..");
	nooftraingimage=nTrainFaces;
    if( nTrainFaces < 2 ){fprintf(stderr,"Reikalingi 2 ar daugiau veidu DB\n"
                                         "Ivedamas failas sudaro %d\n", nTrainFaces);  return;}
	// do PCA on the training faces
	doPCA();
	// project the training images onto the PCA subspace
	projectedTrainFaceMat = cvCreateMat( nTrainFaces, nEigens, CV_32FC1 );
	offset = projectedTrainFaceMat->step / sizeof(float);
	for(i=0; i<nTrainFaces; i++){//int offset = i * nEigens;
                                cvEigenDecomposite(
                                faceImgArr[i],
                                nEigens,
                                eigenVectArr,
                                0, 0,
                                pAvgTrainImg,
			//projectedTrainFaceMat->data.fl + i*nEigens);
                                projectedTrainFaceMat->data.fl + i*offset);}
	// store the recognition data as an xml file
	storeTrainingData();
	// Save all the eigenvectors as images, so that they can be checked.
	if (SAVE_EIGENFACE_IMAGES) {storeEigenfaceImages();}
}
//######################################################################################################
// Open the training data from the file 'facedata.xml'.
int loadTrainingData(CvMat ** pTrainPersonNumMat)
{	CvFileStorage * fileStorage;
	int i;
	// create a file-storage interface
	fileStorage = cvOpenFileStorage( "facedata.xml", 0, CV_STORAGE_READ );
	if( !fileStorage ) {gf.footerLine((char *)"Negaliu atidaryti DB failo 'facedata.xml'."); return 0;}

	// Load the person names. Added by vaibhav.
	personNames.clear();	// Make sure it starts as empty.
	nPersons = cvReadIntByName( fileStorage, 0, "nPersons", 0 );
	if (nPersons == 0) {gf.footerLine((char *)"Nera uzregistruotu DB 'facedata.xml'."); return 0;}

	// Load each person's name.
	for (i=0; i<nPersons; i++) {
		string sPersonName;
		char varname[200];
		sprintf( varname, "personName_%d", (i+1) );
		sPersonName = cvReadStringByName(fileStorage, 0, varname );
		personNames.push_back( sPersonName );}
	// Load the data
	nEigens = cvReadIntByName(fileStorage, 0, "nEigens", 0);
	nTrainFaces = cvReadIntByName(fileStorage, 0, "nTrainFaces", 0);
	*pTrainPersonNumMat = (CvMat *)cvReadByName(fileStorage, 0, "trainPersonNumMat", 0);
	eigenValMat  = (CvMat *)cvReadByName(fileStorage, 0, "eigenValMat", 0);
	projectedTrainFaceMat = (CvMat *)cvReadByName(fileStorage, 0, "projectedTrainFaceMat", 0);
	pAvgTrainImg = (IplImage *)cvReadByName(fileStorage, 0, "avgTrainImg", 0);
	eigenVectArr = (IplImage **)cvAlloc(nTrainFaces*sizeof(IplImage *));

	for(i=0; i<nEigens; i++){char varname[200]; sprintf( varname, "eigenVect_%d", i );
        eigenVectArr[i] = (IplImage *)cvReadByName(fileStorage, 0, varname, 0);	}

	// release the file-storage interface
	cvReleaseFileStorage( &fileStorage );
	//printf("Training data loaded (%d training images of %d people):\n", nTrainFaces, nPersons);
	//printf("People: ");
	if (nPersons > 0)
	//printf("<%s>", personNames[0].c_str());
        for (i=1; i<nPersons; i++) {//printf(", <%s>", personNames[i].c_str());
        }
	return 1;}

//*######################################################################################################
// Save the training data to the file 'facedata.xml'.
void storeTrainingData()
{   int i;
	CvFileStorage * fileStorage;
	// create a file-storage interface
	fileStorage = cvOpenFileStorage( "facedata.xml", 0, CV_STORAGE_WRITE );
	// Store the person names. Added by vaibhav.
	cvWriteInt( fileStorage, "nPersons", nPersons );
	for (i=0; i<nPersons; i++) {char varname[200];
		sprintf( varname, "personName_%d", (i+1) );
		cvWriteString(fileStorage, varname, personNames[i].c_str(), 0);}
	// store all the data
	cvWriteInt( fileStorage, "nEigens", nEigens );
	cvWriteInt( fileStorage, "nTrainFaces", nTrainFaces );
	cvWrite(fileStorage, "trainPersonNumMat", personNumTruthMat, cvAttrList(0,0));
	cvWrite(fileStorage, "eigenValMat", eigenValMat, cvAttrList(0,0));
	cvWrite(fileStorage, "projectedTrainFaceMat", projectedTrainFaceMat, cvAttrList(0,0));
	cvWrite(fileStorage, "avgTrainImg", pAvgTrainImg, cvAttrList(0,0));
	for(i=0; i<nEigens; i++){char varname[200];
		sprintf( varname, "eigenVect_%d", i );
		cvWrite(fileStorage, varname, eigenVectArr[i], cvAttrList(0,0));}
	// release the file-storage interface
	cvReleaseFileStorage( &fileStorage );
}

//######################################################################################################
// Find the most likely person based on a detection. Returns the index, and stores the Panasumas value into pPanasumas.
int findNearestNeighbor(float * projectedTestFace, float *pPanasumas)
{	//double leastDistSq = 1e12;
	double leastDistSq = DBL_MAX;
	int i, iTrain, iNearest = 0;
	for(iTrain=0; iTrain<nTrainFaces; iTrain++)
	{double distSq=0;
		for(i=0; i<nEigens; i++){
        float d_i = projectedTestFace[i] - projectedTrainFaceMat->data.fl[iTrain*nEigens + i];
#ifdef USE_MAHALANOBIS_DISTANCE
        distSq += d_i*d_i / eigenValMat->data.fl[i];  // Mahalanobis distance (might give better results than Eucalidean distance)
#else
        distSq += d_i*d_i; // Euclidean distance.
#endif
}
		if(distSq < leastDistSq) { leastDistSq = distSq;  iNearest = iTrain;}
	}
	// Return the Panasumas level based on the Euclidean distance,
	// so that similar images should give a Panasumas between 0.5 to 1.0,
	// and very different images should give a Panasumas between 0.0 to 0.5.
	*pPanasumas = 1.0f - sqrt( leastDistSq / (float)(nTrainFaces * nEigens) ) / 255.0f;
	// Return the found index.
	return iNearest;
}
//######################################################################################################
// Do the Principal Component Analysis, finding the average image
// and the eigenfaces that represent any image in the given dataset.
void doPCA()
{	int i;
	CvTermCriteria calcLimit;
	CvSize faceImgSize;
	// set the number of eigenvalues to use
	nEigens = nTrainFaces-1;
	// allocate the eigenvector images
	faceImgSize.width  = faceImgArr[0]->width;
	faceImgSize.height = faceImgArr[0]->height;
	eigenVectArr = (IplImage**)cvAlloc(sizeof(IplImage*) * nEigens);
	for(i=0; i<nEigens; i++)
		eigenVectArr[i] = cvCreateImage(faceImgSize, IPL_DEPTH_32F, 1);

	// allocate the eigenvalue array
	eigenValMat = cvCreateMat( 1, nEigens, CV_32FC1 );

	// allocate the averaged image
	pAvgTrainImg = cvCreateImage(faceImgSize, IPL_DEPTH_32F, 1);

	// set the PCA termination criterion
	calcLimit = cvTermCriteria( CV_TERMCRIT_ITER, nEigens, 1);

	// compute average image, eigenvalues, and eigenvectors
        cvCalcEigenObjects(nTrainFaces,
                        (void*)faceImgArr,
                        (void*)eigenVectArr,
                        CV_EIGOBJ_NO_CALLBACK,
                        0,
                        0,
                        &calcLimit,
                        pAvgTrainImg,
                        eigenValMat->data.fl);

	cvNormalize(eigenValMat, eigenValMat, 1, 0, CV_L1, 0);
}
//######################################################################################################
// Read the names & image filenames of people from a text file, and load all those images listed.
int loadFaceImgArray(char * filename)
{   int i;
	FILE * imgListFile = 0;
	char imgFilename[512];
	int iFace, nFaces=0;
	// open the input file
	if( !(imgListFile = fopen(filename, "r")) )
	{fprintf(stderr, "Negaliu atidaryti failo %s\n", filename);
		return 0;}
	// count the number of faces
	while( fgets(imgFilename, 512, imgListFile) ) ++nFaces;
	rewind(imgListFile);
	// allocate the face-image array and person number matrix
	faceImgArr        = (IplImage **)cvAlloc( nFaces*sizeof(IplImage *) );
	personNumTruthMat = cvCreateMat( 1, nFaces, CV_32SC1 );
	personNames.clear();	// Make sure it starts as empty.
	nPersons = 0;
	// store the face images in an array
	for(iFace=0; iFace<nFaces; iFace++)
	{   char personName[256];
		string sPersonName;
		int personNumber;
		// read person number (beginning with 1), their name and the image filename.
		fscanf(imgListFile, "%d %s %s", &personNumber, personName, imgFilename);
		sPersonName = personName;
		//printf("Got %d: %d, <%s>, <%s>.\n", iFace, personNumber, personName, imgFilename);
		// Check if a new person is being loaded.
		if (personNumber > nPersons) {// Allocate memory for the extra person (or possibly multiple), using this new person's name.
			for (i=nPersons; i < personNumber; i++) {personNames.push_back( sPersonName );}
			nPersons = personNumber;}
		// Keep the data
		personNumTruthMat->data.i[iFace] = personNumber;
		// load the face image
		faceImgArr[iFace] = cvLoadImage(imgFilename, CV_LOAD_IMAGE_GRAYSCALE);
      if( !faceImgArr[iFace] ) {fprintf(stderr, "Negaliu atidaryti paveikslelio %s\n", imgFilename); return 0;}
	}
	fclose(imgListFile);

	if (nPersons > 0)
		//printf("<%s>", personNames[0].c_str());
	for (i=1; i<nPersons; i++) {}//printf(", <%s>", personNames[i].c_str());
	return nFaces;
}
//######################################################################################################
// Recognize the face in each of the test images given, and compare the results with the truth.
void recognizeFileList(char *szFileTest)
{   float Panasumas;
	int i, nTestFaces  = 0;         // the number of test images
	CvMat * trainPersonNumMat = 0;  // the person numbers during training
	float * projectedTestFace = 0;
	char *answer;
	int nCorrect = 0;
	int nWrong = 0;
	double timeFaceRecognizeStart;
	double tallyFaceRecognizeTime;
	  // load test images and ground truth for person number
	  nTestFaces = loadFaceImgArray(szFileTest);
	  gf.footerLine((char *)"");printf("%d",nTestFaces);printf(" Ivestu veidu testas");
	  // load the saved training data
	if( !loadTrainingData( &trainPersonNumMat ) ) return;
	// project the test images onto the PCA subspace
	  projectedTestFace = (float *)cvAlloc( nEigens*sizeof(float) );
	  timeFaceRecognizeStart = (double)cvGetTickCount();	// Record the timing.
	for(i=0; i<nTestFaces; i++){int iNearest, nearest, truth;
		// project the test image onto the PCA subspace
		cvEigenDecomposite(
			faceImgArr[i],
			nEigens,
			eigenVectArr,
			0, 0,
			pAvgTrainImg,
			projectedTestFace);

		iNearest = findNearestNeighbor(projectedTestFace, &Panasumas);
		truth    = personNumTruthMat->data.i[i];
		nearest  = trainPersonNumMat->data.i[iNearest];

            if (nearest == truth) {answer = (char *)"Correct";  nCorrect++;}
            else {answer = (char *)"WRONG!";  nWrong++;}
		printf("Apytiksliai = %d, Truth = %d (%s). Panasumas = %f\n", nearest, truth, answer, Panasumas);
    confi=Panasumas;}
	tallyFaceRecognizeTime = (double)cvGetTickCount() - timeFaceRecognizeStart;
	if (nCorrect+nWrong > 0) {
		gf.footerLine((char *)"TOTAL ACCURACY:");printf(" %d%%",nCorrect * 100/(nCorrect+nWrong));printf(" out of");printf(" %d",(nCorrect+nWrong));printf(" tests.");
		gf.footerLine((char *)"TOTAL TIME:");printf(" %.1fms average.",tallyFaceRecognizeTime/((double)cvGetTickFrequency() * 1000.0 * (nCorrect+nWrong) ) );
        detectiontime=(tallyFaceRecognizeTime/((double)cvGetTickFrequency() * 1000.0 * (nCorrect+nWrong) ) );
        accuracy=(nCorrect * 100/(nCorrect+nWrong));}
}
//######################################################################################################
IplImage* getCameraFrame(void)
{
IplImage *frame;

	if (!camera){camera = cvCaptureFromCAM( kamera1 );//kuri kamera cvCreateCameraCapture
        if (!camera) {gf.gotoxy(7,40); gf.warning((char *)"Nera desined kameros");Sleep(2000);}// exit(1);
		cvSetCaptureProperty( camera, CV_CAP_PROP_FRAME_WIDTH, plotis );
		cvSetCaptureProperty( camera, CV_CAP_PROP_FRAME_HEIGHT, aukstis );
		}//frame = cvQueryFrame( camera );
        cvGrabFrame( camera);
      frame =cvRetrieveFrame(camera);
    	if (!frame) {fprintf(stderr, "Nera prisijungimo prie kameros ar video srauto.\n");frame = NULL; Sleep(2000);}  // exit(1); //frame =  cvRetrieveFrame(camera);

	return frame;
}

//stereo antra kamera, kopija ######################################################################################################
IplImage* getCameraFrame1(int kamnr)
{   IplImage *frame;

	if (!camera1){camera1 = cvCaptureFromCAM( kamnr);//
        if (!camera1) {gf.gotoxy(7,41); gf.warning((char *)"Nera kaires kameros");  Sleep(2000); }// exit(1);
		cvSetCaptureProperty( camera1, CV_CAP_PROP_FRAME_WIDTH, plotis );
		cvSetCaptureProperty( camera1, CV_CAP_PROP_FRAME_HEIGHT, aukstis );
		 } //frame = cvQueryFrame( camera1 );
    cvGrabFrame( camera1);
      frame = cvRetrieveFrame(camera1);
	if (!frame) {fprintf(stderr, "Nera prisijungimo prie kameros ar video srauto.\n");frame = NULL; 	Sleep(2000);}	 //	exit(1);

	return frame;
}

//######################################################################################################
IplImage* convertImageToGreyscale(const IplImage *imageSrc)
{	IplImage *imageGrey;
	if (imageSrc->nChannels == 3) {
		imageGrey = cvCreateImage( cvGetSize(imageSrc), IPL_DEPTH_8U, 1 );
		cvCvtColor( imageSrc, imageGrey, CV_BGR2GRAY );}
	else {imageGrey = cvCloneImage(imageSrc);}
	return imageGrey;
}
//######################################################################################################

IplImage* resizeImage(const IplImage *origImg, int newWidth, int newHeight)
{	int origHeight;
	IplImage *outImg = 0;
	int origWidth;
	if (origImg) {origWidth = origImg->width;
                origHeight = origImg->height;}
	if (newWidth <= 0 || newHeight <= 0 || origImg == 0 || origWidth <= 0 || origHeight <= 0) {
		gf.showRightSide((char *)"KLAIDA: resizeImage: Blogas paveikslelio dydis");printf("%dx%d", newWidth, newHeight);
		exit(1);}
	// Scale the image to the new dimensions, even if the aspect ratio will be changed.
	outImg = cvCreateImage(cvSize(newWidth, newHeight), origImg->depth, origImg->nChannels);
	if (newWidth > origImg->width && newHeight > origImg->height) {
		// Make the image larger
		cvResetImageROI((IplImage*)origImg);
		cvResize(origImg, outImg, CV_INTER_LINEAR);}	// CV_INTER_CUBIC or CV_INTER_LINEAR is good for enlarging
	else {// Make the image smaller
		cvResetImageROI((IplImage*)origImg);
		cvResize(origImg, outImg, CV_INTER_AREA);}	// CV_INTER_AREA is good for shrinking / decimation, but bad at enlarging.
	return outImg;
}
//######################################################################################################
// Returns a new image that is a cropped version of the original image.
IplImage* cropImage(const IplImage *img, const CvRect region)
{	IplImage *imageTmp;
	IplImage *imageRGB;
	CvSize size;
	size.height = img->height;
	size.width = img->width;
  if (img->depth != IPL_DEPTH_8U) {
		gf.footerLine((char *)"KLAIDA cropImage: Nezinomas vaizdo gylis");printf(" %d",img->depth);printf(" given in cropImage() instead of 8 bits per pixel.");
		exit(1);}
	// First create a new (color or greyscale) IPL Image and copy contents of img into it.
	imageTmp = cvCreateImage(size, IPL_DEPTH_8U, img->nChannels);
	cvCopy(img, imageTmp, NULL);
	// Create a new image of the detected region
	// Set region of interest to that surrounding the face
	cvSetImageROI(imageTmp, region);
	// Copy region of interest (i.e. face) into a new iplImage (imageRGB) and return it
	size.width = region.width;
	size.height = region.height;
	imageRGB = cvCreateImage(size, IPL_DEPTH_8U, img->nChannels);
	cvCopy(imageTmp, imageRGB, NULL);	// Copy just the region.
    cvReleaseImage( &imageTmp );
	return imageRGB;
}
//######################################################################################################
// Get an 8-bit equivalent of the 32-bit Float image.
// Returns a new image, so remember to call 'cvReleaseImage()' on the result.
IplImage* convertFloatImageToUcharImage(const IplImage *srcImg)
{	IplImage *dstImg = 0;
	if ((srcImg) && (srcImg->width > 0 && srcImg->height > 0)) {
		// Spread the 32bit floating point pixels to fit within 8bit pixel range.
		double minVal, maxVal;
		cvMinMaxLoc(srcImg, &minVal, &maxVal);
		//cout << "FloatImage:(minV=" << minVal << ", maxV=" << maxVal << ")." << endl;

		// Deal with NaN and extreme values, since the DFT seems to give some NaN results.
		if (cvIsNaN(minVal) || minVal < -1e30)
			minVal = -1e30;
		if (cvIsNaN(maxVal) || maxVal > 1e30)
			maxVal = 1e30;
		if (maxVal-minVal == 0.0f)
			maxVal = minVal + 0.001;	// remove potential divide by zero errors.
		// Convert the format
		dstImg = cvCreateImage(cvSize(srcImg->width, srcImg->height), 8, 1);
		cvConvertScale(srcImg, dstImg, 255.0 / (maxVal - minVal), - minVal * 255.0 / (maxVal-minVal));
		}
	return dstImg;
}
//######################################################################################################
// Store a greyscale floating-point CvMat image into a BMP/JPG/GIF/PNG image,
// since cvSaveImage() can only handle 8bit images (not 32bit float images).
void saveFloatImage(const char *filename, const IplImage *srcImg)
{	//cout << "Saving Float Image '" << filename << "' (" << srcImg->width << "," << srcImg->height << "). " << endl;
	IplImage *byteImg = convertFloatImageToUcharImage(srcImg);
	cvSaveImage(filename, byteImg);
	cvReleaseImage(&byteImg);
}
//######################################################################################################
// Perform face detection on the input image, using the given Haar cascade classifier.
// Returns a rectangle for the detected region in the given image.
CvRect detectFaceInImage(const IplImage *inputImg, const CvHaarClassifierCascade* cascade )
{	int i;
	const CvSize minFeatureSize = cvSize(20, 20);
	const int flags = CV_HAAR_FIND_BIGGEST_OBJECT | CV_HAAR_DO_ROUGH_SEARCH;	// Only search for 1 face.
	const float search_scale_factor = 1.1f;
	IplImage *detectImg;
	IplImage *greyImg = 0;
	CvMemStorage* storage;
	CvRect rc;
	double t;
	CvSeq* rects;

	storage = cvCreateMemStorage(0);
	cvClearMemStorage( storage );

	// If the image is color, use a greyscale copy of the image.
	detectImg = (IplImage*)inputImg;	// Assume the input image is to be used.
	if (inputImg->nChannels > 1){
		greyImg = cvCreateImage(cvSize(inputImg->width, inputImg->height), IPL_DEPTH_8U, 1 );
		cvCvtColor( inputImg, greyImg, CV_BGR2GRAY );
		detectImg = greyImg;}	// Use the greyscale version as the input.
	// Detect all the faces.
	t = (double)cvGetTickCount();
	rects = cvHaarDetectObjects( detectImg, (CvHaarClassifierCascade*)cascade, storage,
				search_scale_factor, 3,CV_HAAR_DO_CANNY_PRUNING | CV_HAAR_FIND_BIGGEST_OBJECT , minFeatureSize );//flags buvo dabar yra CV_HAAR_DO_CANNY_PRUNING | CV_HAAR_FIND_BIGGEST_OBJECT
	t = (double)cvGetTickCount() - t;
	//gf.footerLine("[Face Detection took");printf(" %d ms",cvRound( t/((double)cvGetTickFrequency()*1000.0) ));printf(" and found");printf(" %d objects]",rects->total );
     detectfacetime=cvRound( t/((double)cvGetTickFrequency()*1000.0) );
	// Get the first detected face (the biggest).
	if (rects->total > 0) {rc = *(CvRect*)cvGetSeqElem( rects, 0 );}
	else
		rc = cvRect(-1,-1,-1,-1);	// Couldn't find the face.

	//cvReleaseHaarClassifierCascade( &cascade );
	//cvReleaseImage( &detectImg );
	if (greyImg)
		cvReleaseImage( &greyImg );
	cvReleaseMemStorage( &storage );

	return rc;	// Return the biggest face found, or (-1,-1,-1,-1).
}
//**************************************************************************************
// Re-train the new face rec database without shutting down.
// Depending on the number of images in the training set and number of people, it might take 30 seconds or so.
CvMat* retrainOnline(void)
{   int i;
	CvMat *trainPersonNumMat;
	// Free & Re-initialize the global variables.
	if (faceImgArr) {
		for (i=0; i<nTrainFaces; i++) {
			if (faceImgArr[i])
				cvReleaseImage( &faceImgArr[i] );}
	}
	cvFree( &faceImgArr ); // array of face images
	cvFree( &personNumTruthMat ); // array of person numbers
	personNames.clear();			// array of person names (indexed by the person number). Added by vaibhav.
	nPersons = 0; // the number of people in the training set. Added by vaibhav.
	nTrainFaces = 0; // the number of training images
	nEigens = 0; // the number of eigenvalues
	cvReleaseImage( &pAvgTrainImg ); // the average image
	for (i=0; i<nTrainFaces; i++) {
		if (eigenVectArr[i])
			cvReleaseImage( &eigenVectArr[i] );
	}
	cvFree( &eigenVectArr ); // eigenvectors
	cvFree( &eigenValMat ); // eigenvalues
	cvFree( &projectedTrainFaceMat ); // projected training faces
	// Retrain from the data in the files
	gf.showRightSide((char *)"Retraining with the new person ......");
	learn((char *)"train.txt");
	gf.showRightSide((char *)"Done retraining.....");
	// Load the previously saved training data
	if( !loadTrainingData( &trainPersonNumMat ) ) {gf.footerLine((char *)"KLAIDA ivedimasiscam(): Negaliu ivesti treniravimo duomenu!");exit(1);}

	return trainPersonNumMat;
}
//######################################################################################################
// Continuously recognize the person in the camera.
// Continuously recognize the person in the camera.
void ivedimasiscam(void)
{	int i; plotis=320; aukstis=240;
	CvMat * trainPersonNumMat;  // the person numbers during training
	float * projectedTestFace;
	double timeFaceRecognizeStart;
	double tallyFaceRecognizeTime;
	CvHaarClassifierCascade* koksCascade;
	char cstr[256];
	BOOL saveNextFaces = FALSE;
	char newPersonName[256];
	int newPersonFaces;
    int yn;
	int inc;
	int inc1;
	char checkSave;
	char empid[256];
	char department[256];
	char password[30];
	char confirmPassword[30];
	char firstname[256];
	char lastname[256];
	trainPersonNumMat = 0;  // the person numbers during training
	projectedTestFace = 0;
	saveNextFaces = FALSE;
	newPersonFaces = 0;
    int ch,w;
    FILE *data;
    char temp[256];
    char emppass[256],empcon[256],empname[256];
    gf.showRightSide((char *)"    IVESTI NAUJA FIZIONOMIJA:",1);
    gf.showRightSide((char *)"");
	gf.showRightSide((char *)" - patvirtinti (Taip=[1] arba Ne=[0])");
	gf.showRightSide((char *)" - paskui spausti - [Enter]");
	gf.showRightSide((char *)" - palaukti pakol uszikraus kamera");
	gf.showRightSide((char *)" - pele aktyvuoti pagrindini meniu");
	gf.showRightSide((char *)"");
    gf.showRightSide((char *)"    PRADETI IVEDIMA: spausti -[N] ");
    gf.showRightSide((char *)"");
    gf.showRightSide((char *)" Prades ivedineti etaloninius pav.");
    gf.showRightSide((char *)" Ivedus ~50 pav. stabdyti ivedima -[T]");
    gf.showRightSide((char *)"");
	gf.showRightSide((char *)"     NEPAMIRSKITE !!!");
	gf.showRightSide((char *)"");
	gf.showRightSide((char *)" pele aktyvuoti reikiamus langus o");
	gf.showRightSide((char *)" paskui tik isrinkti reikiama lango rezima");
	gf.footerLine((char *)" Perskaitete ?, patvirtinkite (Taip=[1]/Ne=[0])");scanf("%d",&yn);
	gf.footerLine((char *)"     Palaukti !!!!");
	gf.footerLine((char *)"Nauja fizionomija spausti -[N]");
    if(yn==1)
	{
    gf.showRightSide((char *)"Laukiame kameros .....",3);

	// Load the previously saved training data
	if( loadTrainingData( &trainPersonNumMat ) ) {
		faceWidth = pAvgTrainImg->width;
		faceHeight = pAvgTrainImg->height;}

	else {	}	//printf("KLAIDA ivedimasiscam(): Negaliu ivesti treniravimo duomenu!\n");//exit(1);

	// Project the test images onto the PCA subspace
	projectedTestFace = (float *)cvAlloc( nEigens*sizeof(float) );
	// Create a GUI window for the user to see the camera image.
	cvNamedWindow("Ivedimas", CV_WINDOW_AUTOSIZE);
	// Make sure there is a "data" folder, for storing the new person.
	mkdir("data");
    mkdir("FaceRec");
	mkdir("NewUserFolder");
    char allfilename[80]="FaceRec/allinone.TXT";
    char filename1[80]="NewUserFolder/";
    char fileexten[80]=".TXT";
	// Load the HaarCascade classifier for face detection.
	koksCascade = (CvHaarClassifierCascade*)cvLoad(veidasCascadeFilename, 0, 0, 0 );
	if( !koksCascade ) {gf.showRightSide((char *)"KLAIDA : Negaliu ivesti veido Haar cascade duom. klasifikatoriaus:");printf(" '%s'", veidasCascadeFilename);Sleep(2000);exit(1);}

	timeFaceRecognizeStart = (double)cvGetTickCount();	// Record the timing.

	while (1)
	{    int c;
		int iNearest, nearest, truth;
		IplImage *camImg;
		IplImage *greyImg;
		IplImage *faceImg;
		IplImage *sizedImg;
		IplImage *equalizedImg;
		IplImage *processedFaceImg;
		CvRect faceRect;
		IplImage *shownImg;
		int keyPressed = 0;
		FILE *trainFile;
		float confidence;
        FILE *newPersonData;
		// Handle keyboard input in the console.
		if (_kbhit())
			keyPressed = getch();
		if (keyPressed == VK_ESCAPE) {break;}

		switch (keyPressed) {
			case 'n':	// Add a new person to the training set.
				// Train from the following images.

				gf.showRightSide((char *)"              Naujas",1);
                 gf.showRightSide((char *)"=========================================");
retry:
                 gf.footerLine((char *)"ID numeris : ");scanf("%s",&empid);
                 data=fopen(allfilename,"r");
                 if ( data== NULL ) {data = fopen ( allfilename,"wb+") ;
                          if ( data == NULL ) {puts("Negaliu atidaryti failo");Sleep(2000);exit(0);}
                  }
				 rewind (data) ;
				 while ( fread ( &emp, recsize, 1, data )!=EOF )
				 {  if ( strcmp (emp.id,empid) != 0) {gf.showRightSide((char *)"Veido ID:");printf("%s",empid);break;}
                    else {gf.warning((char *)"Veido Id jau yra");
                       gf.showRightSide((char *)"Veido Id jau registruota",1);
                       gf.warning((char *)"Pakartoti  !!!!!");
                       goto retry;}
                 }
	 	         fclose(data);
	 	         strcpy(temp,empid);
                 gf.showRightSide((char *)"");
				 gf.footerLine((char *)"Veido vardas : ");scanf("%s",&empname);
				 strcpy(newPersonName,temp);
                 strcat(newPersonName,"_");
				 strcat(newPersonName,empname);
				 gf.showRightSide((char *)"Veido vardas :");printf("%s",empname);
                 gf.showRightSide((char *)"");
			     gf.footerLine((char *)"Veido kodas : ");scanf("%s",&department);
                 gf.showRightSide((char *)"Veido kodas :");printf("%s",department);
				gf.showRightSide((char *)"");
     w=0;
     do
     { if(w==3)  {gf.warning((char *)"Sitos informacijos galiojimas!!!!"); break;}

     gf.footerLine((char *)"Slaptazodis [ 8 skaiciai] : ");
     int inc=0;
                while (inc<8) {emppass[inc]=getch(); printf("*"); inc=inc+1; }

                       emppass[8]='\0';
                      while(getch()!=13);

     gf.footerLine((char *)"Slaptazodi pakartoti [ 8 skaiciai] ");
     inc=0;
                while (inc<8) {empcon[inc]=getch(); printf("*"); inc=inc+1; }

                       empcon[8]='\0';
                      while(getch()!=13);
     char pass1[256];
     strcpy(pass1,emppass);loadTrainingData( &trainPersonNumMat );
     char con1[256];
     strcpy(con1,empcon);

     int comp=strcmp(pass1,con1);

     if (comp!=0) {gf.warning((char *)"Slaptazodis blogas?????");
                gf.wait(1000);
                gf.warning((char *)" Pakartoti slaptazodzio ivedima");
                w=w+1;}

     else  {gf.showRightSide((char *)"Slaptazodis:");inc=0;while(inc<10){printf("*");inc++;}
     gf.showRightSide((char *)"");
     gf.showRightSide((char *)"Patvirtinti slaptazodi:");inc=0;while(inc<10){printf("*");inc++;}
     gf.showRightSide((char *)"");  break;}
      }  //do pabaiga
      while(w<=3);
				   strcat(filename1,empid);
				   strcat(filename1,fileexten);
                   newPersonData=fopen(filename1,"a");
        if(newPersonData==NULL) {printf("Negaliu atidaryti failo");}

			gf.footerLine((char *)"Issaugoti informacija? (y/n):");checkSave=getche();

        if (checkSave=='Y' || checkSave=='y'){strcpy(nu.id,empid);
                                                strcpy(nu.name,empname);
                                                strcpy(nu.dept,department);
                                                strcpy(nu.passw,emppass);
                                                fwrite(&nu,recsize1,1,newPersonData);}

                    strcpy(emp.id,empid);
                    data=fopen(allfilename,"a");
                    fseek ( data, 0 , SEEK_END ) ;
                    fwrite(&emp,recsize,1,data);
                    fclose(newPersonData);
					fclose(data);

				gf.showRightSide((char *)"Spausti [T]  ......");
				gf.showRightSide((char *)"Spausti [T]  ... ...");
				newPersonFaces = 0;	// restart training a new person
				saveNextFaces = TRUE;
				break;

			case 't':	// Start training
				saveNextFaces = FALSE;	// stop saving next faces.
				// Store the saved data into the training file.
				gf.footerLine((char *)"Issaugoma nauja informacija");printf(" '%s'", newPersonName);
				// Append the new person to the end of the training data.
				trainFile = fopen("train.txt", "a");
				for (i=0; i<newPersonFaces; i++) {
					sprintf(cstr, "data/%d_%s%d.pgm", nPersons+1, newPersonName, i+1);
					fprintf(trainFile, "%d %s %s\n", nPersons+1, newPersonName, cstr);}
				fclose(trainFile);
				// Now there is one more person in the database, ready for retraining.
				//nPersons++;

				//break;
			//case 'r':

				// Re-initialize the local data.
				projectedTestFace = 0;
				saveNextFaces = FALSE;
				newPersonFaces = 0;

				// Retrain from the new database without shutting down.
				// Depending on the number of images in the training set and number of people, it might take 30 seconds or so.
				cvFree( &trainPersonNumMat );	// Free the previous data before getting new data
				trainPersonNumMat = retrainOnline();
				// Project the test images onto the PCA subspace
				cvFree(&projectedTestFace);	// Free the previous data before getting new data
				projectedTestFace = (float *)cvAlloc( nEigens*sizeof(float) );

		    	gf.showRightSide((char *)" Jusu veidas patalpintas i DB !");
				gf.showRightSide((char *)"");
				gf.footerLine((char *)"      Jusu veidas patalpintas i DB ! ! ");
				gf.showRightSide((char *)"");
                gf.showRightSide((char *)"Iseiti - 'Esc'");
                gf.warning((char *)"Iseiti - 'Esc'");

				continue;	// Begin with the next frame.
				break;
		}
		// Get the camera frame
	camImg = getCameraFrame();
	//if (!camImg) {gf.showRightSide((char *)"KLAIDA ivedimasiscam(): Blogas  paveikslelis!");exit(1);}
		// Make sure the image is greyscale, since the Eigenfaces is only done on greyscale image.
		greyImg = convertImageToGreyscale(camImg);
		// Perform face detection on the input image, using the given Haar cascade classifier.
		faceRect = detectFaceInImage(greyImg, koksCascade );
		// Make sure a valid face was detected.
		if (faceRect.width > 0) {
			faceImg = cropImage(greyImg, faceRect);	// Get the detected face image.
			// Make sure the image is the same dimensions as the training images.
			sizedImg = resizeImage(faceImg, faceWidth, faceHeight);
			// Give the image a standard brightness and contrast, in case it was too dark or low contrast.
			equalizedImg = cvCreateImage(cvGetSize(sizedImg), 8, 1);	// Create an empty greyscale image
			cvEqualizeHist(sizedImg, equalizedImg);
			processedFaceImg = equalizedImg;
			if (!processedFaceImg) {gf.showRightSide((char *)"KLAIDA ivedimasiscam(): Neturiu pavyzdzio paveikslelio!");exit(1);}

			// If the face rec database has been loaded, then try to recognize the person currently detected.
			if (nEigens > 0) {
				// project the test image onto the PCA subspace
				cvEigenDecomposite(
					processedFaceImg,
					nEigens,
					eigenVectArr,
					0, 0,
					pAvgTrainImg,
					projectedTestFace);

				// Check which person it is most likely to be.
				iNearest = findNearestNeighbor(projectedTestFace, &confidence);
				nearest  = trainPersonNumMat->data.i[iNearest];

				gf.footerLine((char *)"Veidas :");printf(" '%s'  panasumas=%f", personNames[nearest-1].c_str(), confidence);

			}//endif nEigens
		// Possibly save the processed face to the training set.
			if (saveNextFaces) {// MAYBE GET IT TO ONLY TRAIN SOME IMAGES ?
				// Use a different filename each time.
				sprintf(cstr, "data/%d_%s%d.pgm", nPersons+1, newPersonName, newPersonFaces+1);
				gf.footerLine((char *)"Issaugoju pav.");printf(" '%s' i DB '%s'", newPersonName, cstr);
				cvSaveImage(cstr, processedFaceImg, NULL);
				newPersonFaces++;}

			// Free the resources used for this frame.
			cvReleaseImage( &greyImg );
			cvReleaseImage( &faceImg );
			cvReleaseImage( &sizedImg );
			cvReleaseImage( &equalizedImg );}

		// Show the data on the screen.
		shownImg = cvCloneImage(camImg);
		if (faceRect.width > 0) {	// Check if a face was detected.
			// Show the detected face region.
			cvRectangle(shownImg, cvPoint(faceRect.x, faceRect.y), cvPoint(faceRect.x + faceRect.width-1, faceRect.y + faceRect.height-1), CV_RGB(0,255,0), 1, 8, 0);
			if (nEigens > 0) {	// Check if the face recognition database is loaded and a person was recognized.
				// Show the name of the recognized person, overlayed on the image below their face.
				CvFont font;
				cvInitFont(&font,CV_FONT_HERSHEY_COMPLEX_SMALL, 1.0, 1.0, 0,1,CV_AA);
				CvScalar textColor = CV_RGB(0,255,0);	// light blue text
				char text[256];
				//sprintf_s(text, sizeof(text)-1, "Name: '%s'", personNames[nearest-1].c_str());
				//cvPutText(shownImg, personNames[nearest-1].c_str(), cvPoint(faceRect.x, faceRect.y + faceRect.height + 15), &font, textColor);
				//sprintf_s(text, sizeof(text)-1, "Confidence: %f", confidence);
				//cvPutText(shownImg, text, cvPoint(faceRect.x, faceRect.y + faceRect.height + 30), &font, textColor);
			}
		}

		cvShowImage("Ivedimas", shownImg);
		// Give some time for OpenCV to draw the GUI and check if the user has pressed something in the GUI window.
		keyPressed = cvWaitKey(10);
		if (keyPressed == VK_ESCAPE) {	// Check if the user hit the 'Escape' key in the GUI window.
			cvDestroyAllWindows();
            cvReleaseImage( &shownImg);

			break;	// Stop processing input.
		} //while pab

		cvReleaseImage( &shownImg );
	}
	tallyFaceRecognizeTime = (double)cvGetTickCount() - timeFaceRecognizeStart;

	// Free the camera and memory resources used.
	cvReleaseCapture( &camera );
	cvReleaseHaarClassifierCascade( &koksCascade );
	}//end of if yn
else {gf.showRightSide((char *)"Perskaitykite atidziai instrukcija");}
}//ivedimasiscam pabaiga

//######################################################################################################
//######################################################################################################
// ôóíêöèÿ-îáðàáîò÷èê ïîëçóíêà -
void myBMTrackbarkoeficientas(int  koeficientasTMP) {      koeficientasTMP;}
void myBMTrackbarkoeficientas1(int  koeficientas1TMP) {      koeficientas1TMP;}
void myBMTrackbarkoeficientas3(int  koeficientas3TMP) {      koeficientas3TMP;}
void myBMTrackbarkoeficientas4(int  koeficientas4TMP) {      koeficientas4TMP;}

//######################################################################################################
int  stereoBM ()//
{   plotis=320;aukstis=240;
int koeficientasTMP=500,koeficientas1TMP=70,koeficientas3TMP=0,koeficientas4TMP=128;
  cvNamedWindow("stereoBM gylis", CV_WINDOW_AUTOSIZE);    cvMoveWindow("stereoBM gylis", 6,(aukstis+ 52));
   hWnd=FindWindow( NULL,"stereoBM gylis");   SetForegroundWindow(hWnd);
   cvCreateTrackbar("K","stereoBM gylis",&koeficientasTMP,4096,myBMTrackbarkoeficientas);
    cvCreateTrackbar("K1","stereoBM gylis",&koeficientas1TMP,1024,myBMTrackbarkoeficientas1);
  cvNamedWindow("stereoBM gylis 3D", CV_WINDOW_AUTOSIZE);    cvMoveWindow("stereoBM gylis 3D",  (plotis+35),(aukstis+ 52));
   hWnd=FindWindow( NULL,"stereoBM gylis 3D");   SetForegroundWindow(hWnd);
      cvCreateTrackbar("K3","stereoBM gylis 3D",&koeficientas3TMP,1024,myBMTrackbarkoeficientas3);
    cvCreateTrackbar("K4","stereoBM gylis 3D",&koeficientas4TMP,1024,myBMTrackbarkoeficientas4);

IplImage * tmpimage= cvCreateImage( cvSize(plotis,aukstis), 8, 1);;
//IplImage * tmpimage1=NULL;
//IplImage * tmpimage00=NULL;
//IplImage * tmpimage11=NULL;
double min_val,max_val;

    //çàäàíèå íà÷àëüíûõ ïàðàìåòðîâ ñòåðåîçðåíèÿ
    CvStereoBMState* state = cvCreateStereoBMState( 16, 0);
       state->preFilterSize=41;
    state->preFilterCap=31;
    state->SADWindowSize=41;
    state->minDisparity=-64;//-64
    state->numberOfDisparities=128;
    state->textureThreshold=10;
    state->uniquenessRatio=15;


IplImage *greyImg = cvCreateImage( cvSize(plotis,aukstis), 8, 1);
IplImage *greyImg1 = cvCreateImage( cvSize(plotis,aukstis), 8, 1);
//ñîçäàíèå ìàòðèö
CvMat* disparity_left = cvCreateMat( aukstis, plotis,   CV_16S );//CV_8U   CV_32F CV_32FC1
CvMat* disparity_right = cvCreateMat( aukstis, plotis,  CV_16S );//CV_8U  CV_32F CV_32FC1
//âûâîä ìàòðèöû
CvMat* disparity_left_visual = cvCreateMat( aukstis, plotis, CV_8U );//CV_32F  CV_16S  32, 3  CV_32FC1
IplImage *_3dImage=cvCreateImage(  cvSize(plotis,aukstis), 32,3);//32, 3 CV_32FC3
 CvMat *Q = (CvMat *)cvLoad("Q.xml",NULL,NULL,NULL);
 IplConvKernel* kern ;//= cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_RECT,values);
CvPoint gravcentras,agravcentras;
CBlobResult blobs,blobs1;
 CBlob blob;
CBlob blobWithBiggestPerimeter, blobWithLessArea;
IplImage *outputImage;
 outputImage = cvCreateImage(  cvSize(plotis,aukstis), IPL_DEPTH_8U, 3 );
 CvFont  font;
 cvInitFont(&font,CV_FONT_HERSHEY_COMPLEX_SMALL, 1.0, 1.0, 0,1,CV_AA);
  CvPoint p1, p2;
  CvPoint blobumaximumasP10,blobumaximumasP11;
CvPoint blobumaximumasP20,blobumaximumasP21;
int bloboplotisX,bloboplotisY;
  char text[64];
 servo_x=1400;  servo_y=1800;   akys_servo();
 int iapnr=0;
double maxmas[5],atstumasiki;
maxmas[0]=0;maxmas[1]=0;maxmas[2]=0;maxmas[3]=0;maxmas[4]=0;maxmas[5]=0;
while(1)
{ //gf.gotoxy(4,40) ;    printf ("DIRBA ");j

		shownImg   =  getCameraFrame();//getCameraFrame1(kamera1);
		//if (!shownImg) {printf("KLAIDA = ivedimasiscam(kaire): Blogi paveiksleliai!\n");exit(1);}
		shownImg1 =  getCameraFrame1(kamera2);
		//if (!shownImg1) {printf("Kameros vaizdas dubliuotas, nes kita kamera nenustatyta\n");shownImg1 = shownImg;}//exit(1);

cvNormalize(shownImg1, shownImg1, 0, 255, CV_MINMAX , CV_8U  ); //1, 0, CV_L1, 0);  CV_32FC1
cvNormalize(shownImg, shownImg  , 0, 255, CV_MINMAX , CV_8U  ); //1, 0, CV_L1, 0); CV_32FC1


	cvShowImage("Desine", shownImg);
    cvShowImage("Kaire", shownImg1);


cvSmooth(shownImg, shownImg, CV_GAUSSIAN, 5, 25);//??
cvSmooth(shownImg1, shownImg1, CV_GAUSSIAN, 5,25);//??
cvSmooth(shownImg, shownImg, CV_GAUSSIAN, 5, 25);//??
cvSmooth(shownImg1, shownImg1, CV_GAUSSIAN, 5,25);//??

 // cvCanny( shownImg,shownImg,30,70,3 );
      cvDilate( shownImg,shownImg,0,5 );//??
      cvErode( shownImg,shownImg,0,5 );//??

  //  cvCanny( shownImg1,shownImg1,30,70,3 );
      cvDilate( shownImg1,shownImg1,0,5 );//??
     cvErode( shownImg1,shownImg1,0,5 );//??

cvCvtColor( shownImg1, greyImg1, CV_BGR2GRAY);//CV_BGR2GRAY
cvCvtColor( shownImg , greyImg,  CV_BGR2GRAY);//CV_BGR2GRAY

//âûïîëíåíèå îñíîâíîé ôóíêöèè ñòåðåîçðåíèÿ, âîçâðàùàþùåé ìàòðèöó 3d
//cvFindStereoCorrespondenceGC( greyImg, greyImg1, disparity_left, disparity_right, state, 0 );//labai letas
cvFindStereoCorrespondenceBM(  greyImg1,greyImg, disparity_left, state );

cvMinMaxLoc(disparity_left, &min_val, &max_val);
    // Display it as a CV_8UC1 image
   // cvConvertScale( disparity_left,disparity_left, CV_8UC1, 255/(max_val - min_val));



maxmas[iapnr]=max_val; iapnr=iapnr+1;  if (iapnr>5){iapnr=0;}
atstumasiki=(maxmas[0]+maxmas[1]+maxmas[2]+maxmas[3]+maxmas[4]+maxmas[5])/5;
if (atstumasiki!=0)
{atstumasiki=1000/atstumasiki;}
if(min_val!=max_val){
cvConvertScale(disparity_left, disparity_left_visual,(0.01+(koeficientasTMP/(max_val-min_val))),koeficientas1TMP*(-min_val/(max_val-min_val)));

}

//cvReprojectImageTo3D(disparity_left_visual,_3dImage,disparity_left);
gf.gotoxy(4,42) ; printf("Min disp: %f Atstumas: %f \n", min_val, atstumasiki);//

cvReprojectImageTo3D(disparity_left,_3dImage,Q);
//disparity_left_visual=cv.sterstereoRectify(shownImg1,disparity_left,shownImg,disparity_right, state);

cvConvertScale(disparity_left_visual, greyImg1,1 , 0);//1.0/256.0



/*
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//cvCopy(greyImg1,outputImage);//
cvCvtColor(greyImg1,outputImage,  CV_GRAY2RGB);
blobs=CBlobResult( greyImg1,NULL,50,true );//CBlobResult( inputImage, NULL, 100, true );
blobs.Filter( blobs, B_INCLUDE, CBlobGetArea(), B_INSIDE, 500, 50000); //50,50000

for (i = 0; i < blobs.GetNumBlobs (); i++) {
// (4)Estimate the most smallest contour of each blob

blob = blobs.GetBlob (i);

p1.x = (int) blob.MinX ();
p1.y = (int) blob.MinY ();
p2.x = (int) blob.MaxX ();
p2.y = (int) blob.MaxY ();
if (bloboplotisX<(p2.x-p1.x)){bloboplotisX=(p2.x-p1.x);blobumaximumasP10.x=p1.x;blobumaximumasP20.x=p2.x;blobumaximumasP10.y=p1.y;blobumaximumasP20.y=p2.y; }
if (bloboplotisY<(p2.x-p1.x)){bloboplotisY=(p2.y-p1.y);blobumaximumasP11.x=p1.x;blobumaximumasP21.x=p2.x;blobumaximumasP11.y=p1.y;blobumaximumasP21.y=p2.y; }

// (5)Render contour,index,area,perimeter
//cvRectangle (_3dImage, p1, p2, CV_RGB (0, 2, 0), 1, 8, 0);//outputImage

}
cvRectangle (_3dImage, blobumaximumasP10, blobumaximumasP20, CV_RGB (0, 1, 0), 1, 8, 0);//
cvRectangle (_3dImage, blobumaximumasP11, blobumaximumasP21, CV_RGB (0, 0, 1), 1, 8, 0);//
//cvMerge( greyImg, greyImg, greyImg, NULL, outputImage );
blobWithBiggestPerimeter.FillBlob(  outputImage, CV_RGB( 127, 127, 127));//uztusuojam
blobWithLessArea.FillBlob(  outputImage, CV_RGB( 40, 40, 40 ));//uztusuojam juodai

blobs.GetNthBlob (CBlobGetArea (), blobs.GetNumBlobs () - 1, blobWithLessArea);
blobWithLessArea.FillBlob (outputImage, CV_RGB (30, 30, 30));
cvCvtColor( outputImage , greyImg1,  CV_BGR2GRAY);//CV_BGR2GRAY
bloboplotisX=0,bloboplotisY=0;

//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
*/
cvCopy(greyImg1,tmpimage);//
int xROI=plotis/4;//ROI koordinate x
int pabROI=plotis/6;
int plotisROI=plotis-xROI-pabROI;
int yROI=aukstis/24;//ROI koordinate y
int aukROI=aukstis/24;
int aukstisROI=aukstis-yROI-aukROI;

CvSeq* contours;

cvSetImageROI(tmpimage, cvRect(xROI,yROI,plotisROI,aukstisROI));//plotis-
//cvZero(tmpimage);
cvThreshold(tmpimage, tmpimage, 100,255, /*CV_THRESH_BINARY*/ CV_THRESH_OTSU);//
//int NumberCont = cvFindContours( tmpimage, storage,&contours,sizeof(CvContour),CV_RETR_TREE, CV_CHAIN_APPROX_NONE,cvPoint(0,0));
cvDilate(tmpimage, tmpimage, 0, 1);



cvDilate(tmpimage,tmpimage,0,20 );

gravcentras = COG(tmpimage);//gravitacijos centras

    cvDilate(tmpimage,tmpimage,0,20 );//??
    cvErode( tmpimage,tmpimage,0,20 );//??
    cvDilate(tmpimage,tmpimage,0,20 );//??

    cvNot(tmpimage,tmpimage);//
    cvErode( tmpimage,tmpimage,0,20 );//??

agravcentras = COG(tmpimage);//antigravitacijos centras
cvResetImageROI(tmpimage);

gravcentras.x=gravcentras.x+xROI;//
agravcentras.x=agravcentras.x+xROI;//
gravcentras.y=gravcentras.x+yROI;//
agravcentras.y=agravcentras.y+xROI;//

	cvCircle( _3dImage, gravcentras, 3, CV_RGB(0,255,0), -1, 8,0);
    cvCircle( greyImg1, gravcentras, 8, CV_RGB(255,255,255), -1, 40,0);
    cvCircle( _3dImage, agravcentras, 3, CV_RGB(0,0,255), -1, 8,0);
    cvCircle( greyImg1, agravcentras, 8, CV_RGB(127,127,127), -1, 40,0);
		cvShowImage("stereoBM gylis 3D",_3dImage );
        cvShowImage("stereoBM gylis",greyImg1);//tmpimage
/*
   if ((prisijungimas<2)){//roboto rezime navigacijai y nenaudoti
      if ((abs(aukstis/2 -agravcentras.y)  )>2) {
         if ((aukstis/2 -agravcentras.y)<0) {servo_y = servo_y   +2 ;}
         else {servo_y = servo_y -2 ;}
      }
        }
       */
      if ((abs((plotisROI)/2 - (agravcentras.x-xROI))  )>3) {
         if (((plotisROI)/2 -(agravcentras.x-xROI))>0)
          {servo_x = servo_x  +3 ;}
         else{servo_x = servo_x  -3 ;}
      }

        servo_greitisX= 500 + abs(plotis/2 - gravcentras.x )/10;
        servo_greitisY= 700 + abs(aukstis/2 - gravcentras.y )/10;
        servo_xksen = servo_x ;
        pthread_create(&pthread_Mechanika, NULL,Mechanika, NULL);
        pthread_join(pthread_Mechanika, NULL);//NULL



    // Check if the user hit the 'Escape' key in the GUI window.
    	keyPressed = cvWaitKey(1);
		if (keyPressed == 'm')
		{maxmas[0]=0;maxmas[1]=0;maxmas[2]=0;maxmas[3]=0;maxmas[4]=0;maxmas[5]=0;//be sito luzdavo
         //gf.gotoxy(4,40) ;    printf ("Iseina");
        cvReleaseMat( &disparity_left_visual);
        delete maxmas;
           cvReleaseMat( &disparity_left);
          cvReleaseMat( &disparity_right);
            cvReleaseStereoBMState( &state );
            cvReleaseImage( &greyImg  );
           cvReleaseImage( &greyImg1  );
           cvReleaseImage( &_3dImage  );
            cvReleaseImage( &outputImage  );
           cvReleaseImage( &tmpimage  );

            break;}	//


   } //while pabaiga

           // cvReleaseCapture( &camera );
           // cvReleaseCapture( &camera1);
        cvDestroyWindow("stereoBM gylis");
        cvDestroyWindow("stereoBM gylis 3D");


}// stereoBM pabaiga
//## STEREO GC  ###################################################################################################

//######################################################################################################
// ôóíêöèÿ-îáðàáîò÷èê ïîëçóíêà -
void myGCTrackbarkoeficientas(int   koeficientasTMP) {       koeficientasTMP;}
void myGCTrackbarkoeficientas1(int  koeficientas1TMP) {      koeficientas1TMP;}
void myGCTrackbarkoeficientas3(int  koeficientas3TMP) {      koeficientas3TMP;}
void myGCTrackbarkoeficientas4(int  koeficientas4TMP) {      koeficientas4TMP;}

int  stereoGC ()//
{   plotis=320;aukstis=240;
int koeficientasTMP=200,koeficientas1TMP=1,koeficientas3TMP=0,koeficientas4TMP=128;

	//cvNamedWindow("Kaire", CV_WINDOW_AUTOSIZE);	cvMoveWindow("Kaire", (plotis+35), 5);
   // hWnd=FindWindow( NULL,"Kaire"); SetForegroundWindow(hWnd);

  //  cvNamedWindow("Desine", CV_WINDOW_AUTOSIZE);    cvMoveWindow("Desine", 6, 5);
  // hWnd=FindWindow( NULL,"Desine"); SetForegroundWindow(hWnd);

    cvNamedWindow("stereoGC gylis", CV_WINDOW_AUTOSIZE);    cvMoveWindow("stereoGC gylis", 6,(aukstis+ 52));
   hWnd=FindWindow( NULL,"stereoGC gylis");   SetForegroundWindow(hWnd);

   cvCreateTrackbar("K","stereoGC gylis",&koeficientasTMP,4096,myGCTrackbarkoeficientas);
    cvCreateTrackbar("K1","stereoGC gylis",&koeficientas1TMP,1024,myGCTrackbarkoeficientas1);


    cvNamedWindow("stereoGC gylis 3D", CV_WINDOW_AUTOSIZE);    cvMoveWindow("stereoGC gylis 3D",  (plotis+35),(aukstis+ 52));
   hWnd=FindWindow( NULL,"stereoGC gylis 3D");   SetForegroundWindow(hWnd);

    cvCreateTrackbar("K3","stereoGC gylis 3D",&koeficientas3TMP,1024,myGCTrackbarkoeficientas3);
    cvCreateTrackbar("K4","stereoGC gylis 3D",&koeficientas4TMP,1024,myGCTrackbarkoeficientas4);
IplImage * tmpimage= cvCreateImage( cvSize(plotis,aukstis), 8, 1);;
double min_val,max_val;
//Íà îäíîì êèòàéñêîì ñàéòå, íàøåë ïðèìåð âûâîäà â 3d, disparity image :)Äëÿ ëó÷øåãî ýôôåêòà ïðåäâàðèòåëüíî íîðìàëèçóåì èçîáðàæåíèå
//imageDepthNormalized = cvCreateMat( imageSize.height,imageSize.width, CV_8U );
//cvScale(imageDepth, imageDepthNormalized, 1, 256);
    //çàäàíèå íà÷àëüíûõ ïàðàìåòðîâ ñòåðåîçðåíèÿ
   CvStereoGCState* state = cvCreateStereoGCState( 16, 2);//labai letas
IplImage *greyImg = cvCreateImage( cvSize(plotis,aukstis), 8, 1);
IplImage *greyImg1 = cvCreateImage( cvSize(plotis,aukstis), 8, 1);
//ñîçäàíèå ìàòðèö
CvMat* disparity_left = cvCreateMat( aukstis, plotis,   CV_16S );//CV_8U   CV_32F CV_32FC1
CvMat* disparity_right = cvCreateMat( aukstis, plotis,  CV_16S );//CV_8U  CV_32F CV_32FC1
//âûâîä ìàòðèöû
CvMat* disparity_left_visual = cvCreateMat( aukstis, plotis, CV_8U );//CV_32F  CV_16S  32, 3  CV_32FC1
IplImage *_3dImage=cvCreateImage(  cvSize(plotis,aukstis), 32,3);//32, 3 CV_32FC3
 CvMat *Q = (CvMat *)cvLoad("Q.xml",NULL,NULL,NULL);
 IplConvKernel* kern ;//= cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_RECT,values);
   time_t tt;
CvPoint gravcentras,agravcentras;

while(1)
{
    // Çàñåêàåì âðåìÿ
    //double tt = (double)cvGetTickCount();
    tt=clock();
		shownImg   =  getCameraFrame();//getCameraFrame1(kamera1);
		//if (!shownImg) {printf("KLAIDA = ivedimasiscam(kaire): Blogi paveiksleliai!\n");exit(1);}
		shownImg1 =  getCameraFrame1(kamera2);
		//if (!shownImg1) {printf("Kameros vaizdas dubliuotas, nes kita kamera nenustatyta\n");shownImg1 = shownImg;}//exit(1);

cvNormalize(shownImg1, shownImg1, 0, 255, CV_MINMAX , CV_8U  ); //1, 0, CV_L1, 0);  CV_32FC1
cvNormalize(shownImg, shownImg  , 0, 255, CV_MINMAX , CV_8U  ); //1, 0, CV_L1, 0); CV_32FC1


	cvShowImage("Desine", shownImg);
    cvShowImage("Kaire", shownImg1);

cvSmooth(shownImg, shownImg, CV_GAUSSIAN, 5, 25);
cvSmooth(shownImg1, shownImg1, CV_GAUSSIAN, 5,25);

 // cvCanny( shownImg,shownImg,30,70,3 );
      cvDilate( shownImg,shownImg,0,1 );
      cvErode( shownImg,shownImg,0,1 );

  //  cvCanny( shownImg1,shownImg1,30,70,3 );
      cvDilate( shownImg1,shownImg1,0,1 );
      cvErode( shownImg1,shownImg1,0,1 );

cvCvtColor( shownImg1, greyImg1, CV_BGR2GRAY);//CV_BGR2GRAY
cvCvtColor( shownImg , greyImg,  CV_BGR2GRAY);//CV_BGR2GRAY

//âûïîëíåíèå îñíîâíîé ôóíêöèè ñòåðåîçðåíèÿ, âîçâðàùàþùåé ìàòðèöó 3d
cvFindStereoCorrespondenceGC( greyImg1, greyImg, disparity_left, disparity_right, state, 0 );//labai letas

cvMinMaxLoc(disparity_left, &min_val, &max_val);

if(min_val!=max_val){
cvConvertScale(disparity_left, disparity_left_visual,(-16+(koeficientasTMP/(32*(max_val-min_val)))),koeficientas1TMP*(-min_val/(max_val-min_val)));
}

//cvReprojectImageTo3D(disparity_left_visual,_3dImage,disparity_left);
gf.gotoxy(4,40) ; printf("Min disp: %f Max value: %f \n", min_val, max_val);

cvReprojectImageTo3D(disparity_left,_3dImage,Q,3);//,3
//disparity_left_visual=cv.sterstereoRectify(shownImg1,disparity_left,shownImg,disparity_right, state);

cvConvertScale(_3dImage , shownImg,1 , 0);//1.0/256.0
cvCvtColor( shownImg , greyImg,  CV_BGR2GRAY);//CV_BGR2GRAY
cvConvertScale(disparity_left_visual, greyImg1,1 , 0);//1.0/256.0



cvCopy(greyImg,tmpimage);


gravcentras = COG(tmpimage);//gravitacijos centras
agravcentras = AntiCOG(tmpimage);//antigravitacijos centras
cvResetImageROI(tmpimage);


	cvCircle( _3dImage, gravcentras, 3, CV_RGB(0,255,0), -1, 8,0);
    cvCircle( greyImg1, gravcentras, 8, CV_RGB(200,200,200), -1, 40,0);
    cvCircle( _3dImage, agravcentras, 3, CV_RGB(0,0,255), -1, 8,0);
    cvCircle( greyImg1, agravcentras, 8, CV_RGB(200,200,200), -1, 40,0);
		cvShowImage("stereoGC gylis 3D",_3dImage );
        cvShowImage("stereoGC gylis",greyImg1 );//

//tmpimage1=cvCloneImage( shownImg1);
//tmpimage=cvCloneImage( shownImg);
gf.gotoxy(4,42) ;printf( "Steroe skaiciavimo laikas = %ld  ms\n", (clock()-tt));//printf( "Laikas = %f s\n", tt/(cvGetTickFrequency()*1000000000));

    // Check if the user hit the 'Escape' key in the GUI window.
    	keyPressed = cvWaitKey(5000);
		if (keyPressed == 'm')
		{
        cvReleaseMat( &disparity_left_visual);
           cvReleaseMat( &disparity_left);
          cvReleaseMat( &disparity_right);
            cvReleaseStereoGCState( &state );
            cvReleaseImage( &greyImg  );
           cvReleaseImage( &greyImg1  );
           cvReleaseImage( &tmpimage );
              // cvDestroyAllWindows();
               break;}	//

   } //while pabaiga
           cvReleaseCapture( &camera );
         cvReleaseCapture( &camera1);
        cvDestroyWindow("stereoGC gylis");
        cvDestroyWindow("stereoGC gylis 3D");
}// stereoGC pabaiga

//***** Veido Atpazinimas roboto programa #######################################################################################
int  VeidoAtpazinimas()
{    int i;plotis=320;aukstis=240;
	CvMat * trainPersonNumMat;  // zmogaus  numeris DB
	float * projectedTestFace;
	CvHaarClassifierCascade* koksCascade;
	trainPersonNumMat = 0;  // the person numbers during training
	projectedTestFace = 0;
	//newPersonFaces = 0;
    time_t  davikliuTaimeris = clock();
    time_t niekonerandu = clock();
    time_t veidoTaimeris;
    time_t kastuTaimeris;
    int paieskos_skaitliukas=0;
        gf.clrscr();gf.setColor(128);
        gf.showRightSide((char *)" Rezoliucija : ",1);printf(" %dx%d.",plotis,aukstis);
        gf.showRightSide((char *)" [m]   - uzdaryti 'Roboto' programa");
        gf.showRightSide((char *)"   ir iskviesti pagrindini meniu");
		gf.showRightSide((char *)" [s]   - (x , y) servus i centra");
		gf.showRightSide((char *)" [e]   - kameru langu vaizdas");
		gf.showRightSide((char *)" [f]   - rodyti kameru vaizda");
        gf.showRightSide((char *)" [n]   - isjungti kameru vaizda");
		gf.showRightSide((char *)" []- ");
    WinExec("eSpeak.exe -v lv+m4  eiyyyy.........kaasz.......eszyy",1);
	// Load the previously saved training data
	if( loadTrainingData( &trainPersonNumMat ) ) {faceWidth = pAvgTrainImg->width;faceHeight = pAvgTrainImg->height;}
	else {gf.gotoxy(7,40);gf.warning((char *)"KLAIDA : Negaliu ivesti veidu treniravimo duomenu!\n");Sleep(1000); exit(1);}//
	// Project the test images onto the PCA subspace
	projectedTestFace = (float *)cvAlloc( nEigens*sizeof(float) );

        if (ekranorezimas==0){
        CvFont fontb;
		cvInitFont(&fontb,CV_FONT_HERSHEY_PLAIN|CV_FONT_ITALIC, 2.0, 2.0, 0,1,CV_AA);
		CvScalar textColor = CV_RGB(255,0,0);
        cvPutText(shownImg, "Vaizdas isjungtas", cvPoint(10, 60), &fontb, textColor);
        cvPutText(shownImg, "Didesnis greitis", cvPoint(10, 120), &fontb, textColor);
        cvPutText(shownImg, "Maziau resursu", cvPoint(10, 180), &fontb, textColor);
        cvShowImage("Desine", shownImg); cvShowImage("Kaire", shownImg);
         }
         cvResizeWindow("Kaire",plotis,aukstis);cvResizeWindow("Desine",plotis,aukstis);

	koksCascade = (CvHaarClassifierCascade*)cvLoad(veidasCascadeFilename, 0, 0, 0 );
  if( !koksCascade ) {gf.gotoxy(7,40);gf.warning((char *) "KLAIDA  : Negaliu isvesti Haar cascade veido detektavimo klasifikatoriu " ); gf.wait(1000);	exit(1);}
    //  timeFaceRecognizeStart = (double)cvGetTickCount();	 Record the timing.
            veidoTaimeris=clock()-600;

while (1){char text[256];
		int iNearest, nearest, truth,stabdrec;
		IplImage *greyImg, *greyImg1;
		IplImage *faceImg;
		IplImage *sizedImg;
		IplImage *equalizedImg;
		IplImage *processedFaceImg;
		CvRect faceRect;
		CvRect faceRect1;
	//	FILE *trainFile;
		float Panasumas;


    if (keyPressed){
		if (keyPressed == 'm') {	break;}
		else if (keyPressed == 's') {  //servo i pradzia
            servo_x=1400; servo_y=1850;servo_xk=1400; servo_yk=1850;servo_xd=1400; servo_yd=1850;
            servo_xdsen=servo_x; servo_ydsen=servo_y;servo_xksen=servo_x; servo_yksen=servo_y;
            servo_greitisX= 700; servo_greitisY= 500; akys_servo();}
         else if (keyPressed == 'e') {  ekranorezimas=1;  }//keyPressed  'e'

         else if (keyPressed == 'f') {ekranorezimas=2;}
         else if (keyPressed == 'n') {ekranorezimas=0;
            CvFont fontb; cvInitFont(&fontb,CV_FONT_HERSHEY_PLAIN|CV_FONT_ITALIC, 2.0, 2.0, 0,1,CV_AA);
            CvScalar textColor = CV_RGB(255,0,0);
            cvPutText(shownImg, "Vaizdas isjungtas", cvPoint(10, 60), &fontb, textColor);
            cvPutText(shownImg, "Didesnis greitis", cvPoint(10, 120), &fontb, textColor);
            cvPutText(shownImg, "Maziau resursu", cvPoint(10, 180), &fontb, textColor);
            cvShowImage("Desine", shownImg); cvShowImage("Kaire", shownImg); }
         } //keipressed

//************************************************************
//*********** SERVO DUOMENU PERDAVIMAS COM PORTUI   ***********
//************************************************************
    if (clock()-niekonerandu < 15000 ){
        servo_x =(servo_xd+servo_xk)/2;//
        servo_y =(servo_yd+servo_yk)/2;
        } //
   else { servo_greitisX=300; servo_greitisY=200;// 15sek jei veido neranda - iesko
          strcpy(senasVeidas,"    " );          WinExec("eSpeak.exe -v lv+m4 eeyyyy......kuuorr...jiuuuuoszzs",1);
          if ((paieskos_skaitliukas>1)&&(paieskos_skaitliukas<3)) {paieska_servo();akys_servo();Sleep(2000); }//reiks perrasyti
          else if ((paieskos_skaitliukas>=3)&&(paieskos_skaitliukas<5)){rndservo();akys_servo();Sleep(2000);}
          else if ((paieskos_skaitliukas>=5)){
             servo_x=1400; servo_y=1850;servo_xk=1400; servo_yk=1850;servo_xd=1400; servo_yd=1850;
            servo_xdsen=servo_x; servo_ydsen=servo_y;servo_xksen=servo_x; servo_yksen=servo_y;
            servo_greitisX= 700; servo_greitisY= 500; akys_servo();
              break;}
          judesys(25,10000);
          paieskos_skaitliukas=paieskos_skaitliukas+1;
        niekonerandu = clock()-2000;//jei nerado veido, tai iesko jau po 9 sekundziu
    }
    gf.gotoxy(4,56);
printf("paieskos_skaitliukas= %i",paieskos_skaitliukas);
    pthread_create(&pthread_Mechanika, NULL,Mechanika, NULL);

//******************************************************
//******************************************************
    shownImg = getCameraFrame();shownImg1 =  getCameraFrame1(kamera2);
		//if (!shownImg) {printf("KLAIDA ,nera jokiu kameru \n");Sleep(2000); exit(1);}
		//if (!shownImg1) {gf.gotoxy(7,40); printf((char *)"Kameros vaizdas bus dubliuotas, nes nera vaizdo is kitos kameros nera\n"); shownImg1 =shownImg ;gf.wait(1000);}//exit(1);
     cvNormalize(shownImg1, shownImg1, 0, 255, CV_MINMAX  ); cvNormalize(shownImg, shownImg  , 0, 255, CV_MINMAX  ); //1, 0, CV_L1, 0); CV_32FC1, CV_8U
		greyImg = convertImageToGreyscale(shownImg);greyImg1 = convertImageToGreyscale(shownImg1);
//veido atpazinimas su haar
		faceRect = detectFaceInImage(greyImg, koksCascade );faceRect1 = detectFaceInImage(greyImg1, koksCascade );
//Ar veidas yra.
	if ((clock()- veidoTaimeris > 500)|| (ekranorezimas==2))//ant realaus roboto isjungtas ekranas defaultu
	{//tik kas 500*1000ms bandom atpazinti, kad greitis butu didesnis
		if ((faceRect.width > 10)) {//
			faceImg = cropImage(greyImg, faceRect);	//Ieskom veido.
			// konvert. dydi, lango vaizdas pavyzdiniu pav dydzio.
			sizedImg = resizeImage(faceImg, faceWidth, faceHeight);
			//vaizdo ryskumai privedami prie DB ryskumo
			equalizedImg = cvCreateImage(cvGetSize(sizedImg), 8, 1);
			cvEqualizeHist(sizedImg, equalizedImg);
			processedFaceImg = equalizedImg;// sukurti nauja
			if (!processedFaceImg) {gf.warning((char *)"KLAIDA  : Neturiu pavyzdiniu paveiksleliu!\n");exit(1);}
// Jei veidas uzkrautas is DB, bandom atpazinti.
		if (nEigens > 0) {
		// ñïðîåêòèðóéòå èñïûòàòåëüíîå èçîáðàæåíèå íà ïîäïðîñòðàíñòâî PCA
		cvEigenDecomposite(processedFaceImg,nEigens,eigenVectArr,0, 0,pAvgTrainImg,projectedTestFace);
		// Tikrinam kas tai galetu buti
		iNearest = findNearestNeighbor(projectedTestFace, &Panasumas);nearest  = trainPersonNumMat->data.i[iNearest];
			}//yra veidas
		}//veido plotis
    //else {kastuTaimeris=clock(); }
	veidoTaimeris=clock();stabdrec=0;
	}	//laiko
    else{stabdrec=1;}
// Kaire *************************************************************
  if ((faceRect1.width > 10)) {	//Jei atpazinimo regino dydis didesnis 10
    cvRectangle(shownImg1, cvPoint(faceRect1.x, faceRect1.y), cvPoint(faceRect1.x + faceRect1.width-1, faceRect1.y + faceRect1.height-1), CV_RGB(0,255,0), 1, 8, 0);
        if (abs((aukstis/2 -faceRect1.height/2) - faceRect1.y )>20) {
         if (((aukstis/2 -faceRect1.height/2) - faceRect1.y )<0) {servo_yk = servo_yk  + faceRect1.y/10 +5 ;}
         else{servo_yk = servo_yk - faceRect1.y/10 -5 ;}
        }
    if  (abs((plotis-faceRect1.x-faceRect1.width)-(faceRect.x))>30)//jei nuo krastu nevienodai ant 30 tipo ne per viduri
    { //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if ((plotis-faceRect1.x -faceRect1.width)>(faceRect.x)){servo_xk = (servo_xk +(plotis-faceRect1.x)/12+3);} //x servo vid raskas =1400 xservo koordinates
        else{servo_xk = (servo_xk -(plotis-faceRect1.x)/8-5);} //x servo vid raskas =1400 xservo koordinates
     }
        servo_greitisX= 1000 + abs(plotis - faceRect1.x );
        servo_greitisY= 800 +(faceRect1.y);
        servo_xksen = servo_xk ;
        niekonerandu = clock();paieskos_skaitliukas=0;
    } //faceRect1.width > 10
        else{

            servo_yk = servo_yd; servo_xk =  (servo_xksen+servo_xd)/2 ;//

            }//faceRect1.width < 10-plotis
// Desine *******************************************************************
  if ((faceRect.width > 10))  {	//Jei atpazinimo regino dydis didesnis 10
        if (abs((aukstis/2-faceRect.height/2) - faceRect.y )>20) {
        //servo_yd = ((servo_ymax-servo_ymin)/2-((plotis-faceRect.y )*(servo_ymax-servo_ymin)/plotis)); //servo koordinates buferis *tmptxt
            if (((aukstis/2-faceRect1.height/2) - faceRect.y )<0 ) {servo_yd = servo_yd  + faceRect.y/10 +5 ;}
            else{servo_yd = servo_yd  - faceRect.y/10 -5  ;  }
        }
    if  (abs((plotis-faceRect1.x -faceRect1.width)-(faceRect.x))>30)//jei nuo krastu nevienodai ant 30 tipo ne per viduri
    {   //--------------------------------------------------------------------
        if ((faceRect.x+faceRect.width )>(plotis-faceRect1.x )){servo_xd = (servo_xd -(faceRect.x)/12-3);} //x servo vid taskas =1400 xservo koordinates
        else{servo_xd = (servo_xd +(faceRect.x/8)+5); }//x servo vid taskas =1400 x servo koordinates
    }
		// Detekcijos regionas
        cvRectangle(shownImg, cvPoint(faceRect.x, faceRect.y), cvPoint(faceRect.x + faceRect.width-1, faceRect.y + faceRect.height-1), CV_RGB(0,255,0), 1, 8, 0);
	if ((nEigens > 0)&&(stabdrec==0) ){	// Patikrinti ar uzkrauta veidu DB, ir zmogus atpazintas.
				//Vardas zemiau veido detekcijos staciakampio
				CvFont font;cvInitFont(&font,CV_FONT_HERSHEY_PLAIN|CV_FONT_ITALIC, 1.0, 1.0, 0,1,CV_AA);
				CvScalar textColor = CV_RGB(0,255,0);
                gf.gotoxy(7,44);printf("Panasumas(%) : ");printf("%f%",Panasumas*100);
                strcpy(naujasVeidas ,personNames[nearest-1].c_str());
     //*************************************************************************
     if(Panasumas >0.93){CvScalar textColor = CV_RGB(255,255,0);	// p>0.93 optimalus mano Logitech ckamerom c>0.9 Xpointui darbo
        cvPutText(shownImg, naujasVeidas, cvPoint(faceRect.x, faceRect.y + faceRect.height + 15), &font, textColor);
            if ((strcmp(naujasVeidas,"                    " ))!=0)  {
              if (((strcmp(senasVeidas,naujasVeidas)) != 0) ){
                strcpy(tmptxt,"eSpeak.exe -v lv+m4  ");
                char tmptextbuf[20];
                strcpy(tmptextbuf,"                    " );strcpy(tmptextbuf , (naujasVeidas));
              if ( isdigit(tmptextbuf[0]) ) {tmptextbuf[0]=' ';}
              if ( isdigit(tmptextbuf[2]) ) {tmptextbuf[0]=' ';}
                strcat(tmptxt,tmptextbuf );
          WinExec((tmptxt ),1);  kastuTaimeris=clock()+2000;//
             strcpy(senasVeidas,naujasVeidas );
            }

            }

    }//pabaiga c>0.93 *****************************************************
              else {
                if ((clock()-kastuTaimeris) >3000 ){strcpy(naujasVeidas,"Nezinomas");}//bele koks
                   CvScalar textColor = CV_RGB(255,0,0);
				cvPutText(shownImg, "Nepazistu", cvPoint(faceRect.x, faceRect.y + faceRect.height + 15), &font, textColor);
                if ((((strcmp(senasVeidas,naujasVeidas)) != 0) and ((strcmp(senasVeidas,"nepazistu")) != 0))||((clock()-kastuTaimeris) >7000) ){
                WinExec("eSpeak.exe -v lv+m4 kaasz....tuuu ",1);
                 strcpy(senasVeidas,"nepazistu");kastuTaimeris=clock();}
                //if (clock()-kastuTaimeris >3000 ){strcpy(naujasVeidas,"Nezinau");kastuTaimeris=clock();}
                }
             gf.gotoxy(7,47); printf("                     "); //pratrinimas
            gf.gotoxy(7,47); printf(naujasVeidas); //patikrinimas
    } //********* (nEigens > 0 pabaiga)*******************
    else if ((nEigens<=0)){  ////(nEigens<=0)    ||(ekranorezimas==2)
    //Eksperimentas, kas ryja RAMa *************************************************
         cvReleaseImage( &processedFaceImg);   cvReleaseImage( eigenVectArr);
         cvReleaseImage( &pAvgTrainImg); cvReleaseMat(&trainPersonNumMat);	cvReleaseImage( &equalizedImg );
    }////nEigens<=0
    //if ( (ekranorezimas==2))
    {cvReleaseImage( &faceImg );cvReleaseImage( &sizedImg );cvReleaseImage( &equalizedImg );}
            //cvReleaseMat(&trainPersonNumMat);
            //cvReleaseImage( eigenVectArr);
        servo_greitisX= 600 +abs(faceRect.x - plotis)/2; servo_greitisY= 500 +(faceRect.y)/2;
       servo_xdsen = servo_xd ;
       niekonerandu = clock();paieskos_skaitliukas=0;
   //Skaiciuojam apytikslu atstuma iki objekto, cia tik bandymas, paskui patikslinti
        if (abs(faceRect.width-faceRect1.width) < 5){
        float atstumas = 30*(abs(faceRect.x-(faceRect.x - plotis)))/(faceRect.width+1)  ;
        gf.gotoxy(7,48);printf("Atstumas(cm) : ");printf("%f",atstumas); }
	} //pabaiga(faceRect.width > 10)
    else {//faceRect.width > 10
          servo_yd = servo_yk; servo_xd =(servo_xdsen+servo_xk)/2;//+servo_xmax-
         }//jei(faceRect.width < 10)
     //Eksperimentas, kas ryja RAMa *************************************************
  cvReleaseImage( &greyImg1 );   cvReleaseImage( &greyImg );
    if (ekranorezimas>0){cvShowImage("Desine", shownImg);  cvShowImage("Kaire", shownImg1); }
	keyPressed = cvWaitKey(1);
	 pthread_join(pthread_Mechanika, NULL);//NULL
 } //while pabaiga ^^^^^^^^^^^^^^^^^^^^^^^^^^^
       // Atlaisvinam atminti.
           //cvReleaseFaceTracker();
            cvReleaseMat(&trainPersonNumMat);
            cvReleaseHaarClassifierCascade( &koksCascade );
         //  cvReleaseHist(&cvEqualizeHist);
           //cvRelease
       cvReleaseCapture( &camera1 );
  cvReleaseCapture( &camera );
};  //Veido Atpazinimas pabaiga
//##### Veido Profilis ################################################################################################
int  VeidoProfilis()
{    int i,stabdrec;plotis=320;aukstis=240;
	CvHaarClassifierCascade* koksCascade;
    int desineVeidoDalis=0;
    time_t laikassenas = clock();
    time_t veidoTaimeris;
    time_t fraze1 = clock();
    IplImage *shownImgi = cvCreateImage( cvSize(plotis,aukstis), 8, 3);   IplImage *shownImgi1 = cvCreateImage( cvSize(plotis,aukstis), 8, 3);
        gf.clrscr();
        gf.setColor(128);
        gf.showRightSide((char *)" Rezoliucija : ",1);printf(" %dx%d.",plotis,aukstis);
        gf.showRightSide((char *)" [m]   -  i pagrindini meniu");
		gf.showRightSide((char *)" [s]   - (x,y) akys i centra");
		gf.showRightSide((char *)" [e]   - ijungti/isjungti vaizda");
        gf.showRightSide((char *)" []- ");
		gf.showRightSide((char *)" []- ");
    WinExec("eSpeak.exe -v lv+m4  proo`fiyliysz",1);
    if (ekranorezimas==0){
        shownImg =  getCameraFrame1(kamera2);
    	CvFont fontb;
		cvInitFont(&fontb,CV_FONT_HERSHEY_PLAIN|CV_FONT_ITALIC, 2.0, 2.0, 0,1,CV_AA);
		CvScalar textColor = CV_RGB(255,0,0);
        cvPutText(shownImg, "Vaizdas isjungtas", cvPoint(10, 60), &fontb, textColor);
        cvPutText(shownImg, "Didesnis greitis", cvPoint(10, 120), &fontb, textColor);
        cvPutText(shownImg, "Maziau resursu", cvPoint(10, 180), &fontb, textColor);
        cvShowImage("Desine", shownImg);
        cvShowImage("Kaire", shownImg);
        }
	// Load the HaarCascade classifier for face detection.
	koksCascade = (CvHaarClassifierCascade*)cvLoad(veidasPrCascadeFilename, 0, 0, 0 );
  if( !koksCascade ) {gf.gotoxy(7,40);gf.warning((char *) "KLAIDA  : Negaliu isvesti Haar cascade veido detektavimo klasifikatoriu " ); gf.wait(1000);	exit(1);}

while (1){   char text[256];
		IplImage *greyImg1;
		IplImage *greyImg;
		IplImage *greyImgi;
		IplImage *greyImgi1;
		CvRect faceRect;
		CvRect faceRect1;

		if (keyPressed == 'm') {
		cvReleaseImage(&greyImg1);
		cvReleaseImage(&greyImg);
        cvReleaseImage(&greyImgi);
		cvReleaseImage(&greyImgi1);
			break;}
		if (keyPressed == 's') {  //servo i pradzia
            servo_x=1400; servo_y=1850;
            servo_xk=1400; servo_yk=1850;
            servo_xd=1400; servo_yd=1850;
            servo_xdsen=servo_x; servo_ydsen=servo_y;
            servo_xksen=servo_x; servo_yksen=servo_y;
            servo_greitisX= 300;
            servo_greitisY= 200;
            akys_servo();}
          if (keyPressed == 'e') {//
            ekranorezimas=!ekranorezimas;
            if (ekranorezimas==0){
             	CvFont fontb;
            cvInitFont(&fontb,CV_FONT_HERSHEY_PLAIN|CV_FONT_ITALIC, 2.0, 2.0, 0,1,CV_AA);
            CvScalar textColor = CV_RGB(255,0,0);
            cvPutText(shownImg, "Vaizdas isjungtas", cvPoint(10, 60), &fontb, textColor);
            cvPutText(shownImg, "Didesnis greitis", cvPoint(10, 120), &fontb, textColor);
            cvPutText(shownImg, "Maziau resursu", cvPoint(10, 180), &fontb, textColor);
            cvShowImage("Desine", shownImg);
            cvShowImage("Kaire", shownImg);
            }
          }

//************************************************************
//*********** SERVO DUOMENU PERDAVIMAS COM PORTUI   ***********
//************************************************************

    if (clock()-laikassenas < 15000 ){  // 15sek jei veido neranda 1 karta- iesko
        servo_x =(servo_xd+servo_xk)/2;//-faceRect1.width
        servo_y =(servo_yd+servo_yk)/2;} //-faceRect1.height
   else { servo_greitisX=300; servo_greitisY=200;
          strcpy(senasVeidas,"    " );
          paieska_servo();
          //rndservo();
          WinExec("eSpeak.exe -v lv+m4 eeyyyy......kuuorr...jiuuuuoszzs",1);
       laikassenas = clock()-8000;//jei nerado veido, tai iesko jau po 7 sekundziu
        }

    pthread_create(&pthread_Mechanika, NULL,Mechanika, NULL);

//******************************************************
    shownImg = getCameraFrame();   shownImg1 =  getCameraFrame1(kamera2);
		//if (!shownImg) {printf("KLAIDA ,nera kameros \n"); exit(1);}
		//if (!shownImg1) {gf.gotoxy(7,40); gf.warning((char *)"Kameros vaizdas bus dubliuotas, nes nera vaizdo is kitos kameros nera\n");	shownImg1 =shownImg ;gf.wait(1000);}//exit(1);

        cvNormalize(shownImg1, shownImg1, 0, 255, CV_MINMAX  ); cvNormalize(shownImg, shownImg  , 0, 255, CV_MINMAX  ); //1, 0, CV_L1, 0); CV_32FC1, CV_8U


        cvCopyImage(shownImg, shownImgi);
        cvCopyImage(shownImg1, shownImgi1);

        cvMirror(shownImg,shownImgi ,1);
        cvMirror( shownImg1,shownImgi1,1);

		greyImg = convertImageToGreyscale(shownImg);
        greyImg1 = convertImageToGreyscale(shownImg1);
		greyImgi = convertImageToGreyscale(shownImgi);
        greyImgi1 = convertImageToGreyscale(shownImgi1);

// atpazinimas su haar
//Ar veidas yra.
        desineVeidoDalis=0;
		faceRect = detectFaceInImage(greyImg, koksCascade );
		if (faceRect.width<1) {faceRect = detectFaceInImage(greyImgi, koksCascade ); desineVeidoDalis=1;}
		if (faceRect.width<1) { desineVeidoDalis=0;}

		if (desineVeidoDalis==0) {faceRect1 = detectFaceInImage(greyImg1, koksCascade );}
		else {faceRect1 = detectFaceInImage(greyImgi1, koksCascade ); }

		//if ((clock()- veidoTaimeris > 300)|| (ekranorezimas==2))//ant realaus roboto isjungtas ekranas defaultu

// Kaire *************************************************************
  if ((faceRect1.width > 10)) {	//Jei atpazinimo regino dydis didesnis 10
        if (abs((aukstis/2 -faceRect1.height/2) - faceRect1.y )>20) {
         if (((aukstis/2 -faceRect1.height/2) - faceRect1.y )<0) {servo_yk = servo_yk  + faceRect1.y/10 +5 ;}
         else{servo_yk = servo_yk - faceRect1.y/10 -5 ;}
        }
    if  (abs((plotis-faceRect1.x-faceRect1.width)-(faceRect.x))>30)//jei nuo krastu nevienodai ant 30 tipo ne per viduri
    {   if  (desineVeidoDalis==0) {
            //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
            if ((plotis-faceRect1.x -faceRect1.width)>(faceRect.x)){servo_xk = (servo_xk +(plotis-faceRect1.x)/12+3);} //x servo vid raskas =1400 xservo koordinates
            else{servo_xk = (servo_xk -(plotis-faceRect1.x)/8-5);} //x servo vid raskas =1400 xservo koordinates
        }
        else{
            //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
            if ((plotis-faceRect1.x -faceRect1.width)<(faceRect.x)){servo_xk = (servo_xk +(plotis-faceRect1.x)/12+3);} //x servo vid raskas =1400 xservo koordinates
            else{servo_xk = (servo_xk -(plotis-faceRect1.x)/8-5);} //x servo vid raskas =1400 xservo koordinates
        }
    }
        servo_greitisX= 300 + abs(plotis/2 - faceRect1.x );
        servo_greitisY= 200 +abs(aukstis/2-faceRect1.y);
        servo_xksen = servo_xk ;laikassenas = clock();
    } //faceRect1.width > 10
        else{servo_yk = servo_yd; servo_xk = (servo_xksen+servo_xd)/2 ;}//faceRect1.width < 10-plotis
// Desine *******************************************************************
  if ((faceRect.width > 10))  {	//Jei atpazinimo regino dydis didesnis 10
stabdrec=0;
        if (abs((aukstis/2-faceRect.height/2) - faceRect.y )>20) {
        //servo_yd = ((servo_ymax-servo_ymin)/2-((plotis-faceRect.y )*(servo_ymax-servo_ymin)/plotis)); //servo koordinates buferis *tmptxt
            if (((aukstis/2-faceRect1.height/2) - faceRect.y )<0 ) {servo_yd = servo_yd  + faceRect.y/10 +5 ;}
            else{servo_yd = servo_yd  - faceRect.y/10 -5  ;  }
        }
    if  (abs((plotis-faceRect1.x -faceRect1.width)-(faceRect.x))>30)//jei nuo krastu nevienodai ant 30 tipo ne per viduri
    {  if  (desineVeidoDalis==0) {
        if ((faceRect.x+faceRect.width )>(plotis-faceRect1.x )){servo_xd = (servo_xd -(faceRect.x)/12-3);} //x servo vid taskas =1400 xservo koordinates
        else{servo_xd = (servo_xd +(faceRect.x/8)+5); }//x servo vid taskas =1400 x servo koordinates
    }
      else{
        if ((faceRect.x+faceRect.width )<(plotis-faceRect1.x )){servo_xd = (servo_xd -(faceRect.x)/12-3);} //x servo vid taskas =1400 xservo koordinates
        else{servo_xd = (servo_xd +(faceRect.x/8)+5); }//x servo vid taskas =1400 x servo koordinates
      }
    }
      cvReleaseImage( &greyImg1 ); cvReleaseImage( &greyImg ); cvReleaseImage( &greyImgi );  cvReleaseImage( &greyImgi1 );

	   //Skaiciuojam apytikslu atstuma iki objekto
        if (abs(faceRect.width-faceRect1.width) < 5){float atstumas = 30*(abs(faceRect.x-(faceRect.x - plotis)))/(faceRect.width+1)  ;
        gf.gotoxy(7,48); printf("Atstumas(cm) : %f",atstumas);  }

        gf.gotoxy(7,46);
          if (clock()-fraze1>3000){
            if (desineVeidoDalis==1){ WinExec("eSpeak.exe -v de+m4   atszyiszuook....deaschinean",1); }
            if (desineVeidoDalis==0){  WinExec("eSpeak.exe -v lv+m4   atszyiszuook....kayirean",1);}
            fraze1=clock();    }

			// Detekcijos regionas
		if (!desineVeidoDalis){printf("Kaire veido puse");
        cvRectangle(shownImg,  cvPoint(faceRect.x, faceRect.y)  , cvPoint(faceRect.x  + faceRect.width-1 , faceRect.y  + faceRect.height-1), CV_RGB(0,255,0), 1, 8, 0);
        cvRectangle(shownImg1, cvPoint(faceRect1.x, faceRect1.y), cvPoint(faceRect1.x + faceRect1.width-1, faceRect1.y + faceRect1.height-1), CV_RGB(0,255,0), 1, 8, 0);
        }
        else {printf("Desine veido puse");
       cvRectangle(shownImg, cvPoint(plotis-faceRect.x, faceRect.y), cvPoint(plotis-(faceRect.x + faceRect.width-1), faceRect.y + faceRect.height-1), CV_RGB(0,255,0), 1, 8, 0);
       cvRectangle(shownImg1, cvPoint(plotis-faceRect1.x, faceRect1.y), cvPoint(plotis-(faceRect1.x + faceRect1.width-1), faceRect1.y + faceRect1.height-1), CV_RGB(0,255,0), 1, 8, 0);
        }

        servo_greitisX= 300 +abs(plotis/2-faceRect.x );  servo_greitisY= 200 + abs(aukstis/2-faceRect.y);
        servo_xdsen = servo_xd ; laikassenas = clock();

	} //(faceRect.width > 20)
    else { servo_yd = servo_yk; servo_xd = (servo_xk+servo_xdsen)/2;
    stabdrec=1;gf.gotoxy(7,46); printf("                         ");}//(faceRect.width > 10)

    if (ekranorezimas>0){	cvShowImage("Desine", shownImg);cvShowImage("Kaire", shownImg1); }
    keyPressed = cvWaitKey(1);
   pthread_join(pthread_Mechanika, NULL);//NULL
 } //while pabaiga ^^^^^^^^^^^^^^^^^^^^^^^^^^^
    	//tallyFaceRecognizeTime = (double)cvGetTickCount() - timeFaceRecognizeStart;
       // Atlaisvinam atminti.

		cvReleaseImage(&shownImgi);
		cvReleaseImage(&shownImgi1);

        //cvReleaseCapture( &camera );
        // cvReleaseCapture( &camera1);
            cvReleaseHaarClassifierCascade( &koksCascade );
       //     cvReleaseImage(&cvEqualizeHist);

}// profilio atpazinimo pabaiga
//#####Judesio detectas #################################################################################################
int  judesys(int judesiu,time_t laikasveikimo)
{plotis=320; aukstis=240;
time_t laikniekonera = clock();//timeautas
time_t laikassenas = clock()+1500; //leisti nurimti vaizdui nuo roboto judejimo
time_t  laikas;
//CvCapture *input;
CvSize imgSize;
imgSize.width = plotis;
imgSize.height = aukstis;
IplImage* greyImg3= cvCreateImage( imgSize, IPL_DEPTH_8U, 1) ;//
IplImage* movingAverage = cvCreateImage( imgSize, IPL_DEPTH_32F, 3);
IplImage* difference;
IplImage* tempimage;
IplImage* motionHistory = cvCreateImage( imgSize, IPL_DEPTH_8U, 3);
//Rectangle to use to put around the people.
CvRect bndRect = cvRect(0,0,0,0);
//Points for the edges of the rectangle.
CvPoint pt1, pt2;
CvFont font;
//Capture the movie frame by frame.
int prevX = 0;
int judesiutmp = 0;

//Buffer to save the number of people when converting the integer
//to a string.
char wow[65];
//The midpoint X position of the rectangle surrounding the moving objects.
int avgX = 0;
//Indicates whether this is the first time in the loop of frames.
bool first = true;
//Indicates the contour which was closest to the left boundary before the object
//entered the region between the buildings.
int closestToLeft = 0;
//Same as above, but for the right.
int closestToRight = plotis;

        if (ekranorezimas==0){
        CvFont fontb;
		cvInitFont(&fontb,CV_FONT_HERSHEY_PLAIN|CV_FONT_ITALIC, 2.0, 2.0, 0,1,CV_AA);
		CvScalar textColor = CV_RGB(255,0,0);
        cvPutText(shownImg, "Vaizdas isjungtas", cvPoint(10, 60), &fontb, textColor);
        cvPutText(shownImg, "Didesnis greitis", cvPoint(10, 120), &fontb, textColor);
        cvPutText(shownImg, "Maziau resursu", cvPoint(10, 180), &fontb, textColor);
        cvShowImage("Desine", shownImg); cvShowImage("Kaire", shownImg);
         }
         else{cvResizeWindow("Kaire",plotis,aukstis);}
//Keep processing frames...
    while(1){
        if (laikasveikimo==0){laikniekonera=clock();}//isjungti timeouta
        //if (judesiu==0){judesiutmp=0;}//isjungti skaic
         if ((keyPressed == 'm'||judesiutmp>judesiu)||((clock()-laikniekonera)>laikasveikimo)) {	//
           // HWND hWnd;
           // hWnd = FindWindow(NULL, "Kaire");
           // if (hWnd) DestroyWindow(hWnd); //arba CloseWindow(hWnd)
            // Destroy the image, movies, and window.
            cvReleaseImage(&tempimage);
            cvReleaseImage(&difference);
            cvReleaseImage(&greyImg3);
            cvReleaseImage(&movingAverage);
            cvReleaseMemStorage(&storage);
            cvReleaseImage(&motionHistory);
            //cvRelease
			break;}	// Stop


		shownImg1 = getCameraFrame1(kamera2);
		//if (!shownImg1) {printf("KLAIDA ivedimasiscam(): Blogas ivestas paveikslelis!\n");	Sleep(2000); break;}
    cvNormalize(shownImg1, shownImg1, 0, 255, CV_MINMAX  );

//If this is the first time, initialize the images.
if(first){  difference = cvCloneImage(shownImg1);
            tempimage = cvCloneImage(shownImg1);
            cvConvertScale(shownImg1, movingAverage, 1.0, 1.0);//shownImg1, movingAverage, 1.0, 0.0
            first = false;
            gf.wait(300); //servo_xk=1400; pt1.x  =1400; servo_yk=1800; pt1.y =1850;
            }
//else, make a running average of the motion.
else {cvRunningAvg(shownImg1, movingAverage, 0.25, NULL);}//0.020 Ñêîëüçÿùåå ñðåäíåå

//Convert the scale of the moving average.
cvConvertScale(movingAverage,tempimage, 1.0, 1.0); //movingAverage,tempimage,1.0,0.0
//Minus the current frame from the moving average.
cvAbsDiff(shownImg1,tempimage,difference);
//Convert the image to grayscale.
cvCvtColor(difference,greyImg3,CV_RGB2GRAY);
//Convert the image to black and white.
cvThreshold(greyImg3, greyImg3, 50, 60, CV_THRESH_BINARY);//—70, 255 âûïîëíÿåò ôèêñèðîâàííîå ïîðîãîâîå ïðåîáðàçîâàíèå äëÿ ýëåìåíòîâ ìàññèâà
//Dilate and erode to get people blobs
cvDilate(greyImg3, greyImg3, 0, 18); //0, 18 Dilate — ðàñòÿãèâàíèå(îïåðàöèÿ ðàñøèðåíèÿ)
cvErode(greyImg3, greyImg3, 0, 10); // 0, 10 Erode — ðàçìûâàíèå(îïåðàöèÿ ñóæåíèÿ)
//Find the contours of the moving images in the frame.
CvMemStorage* storage = cvCreateMemStorage(0);
CvSeq* contour = 0;
CvSeq* h_next=0;
cvFindContours( greyImg3, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );//äåòåêòèðîâàòü êîíòóðû èçîáðàæåíèÿ è ìàíèïóëèðîâàòü èìè

//Process each moving contour in the current frame...
for( ; contour != 0; contour = contour->h_next )//h_next->h_next=h_next->h_next->h_next;
{
    //óäàëÿåì ìåëêèå êîíòóðû  contour =contour->h_next->h_next->h_next;
    //Ïîèñê ìàêñèìàëüíîãî êîíòóðà ****************
    for( CvSeq* c=contour; c!=NULL; c=c->h_next ){
        if (c!=contour){ //Ïðîâåðÿåì êàêîé êîíòóð áîëüøå
            if (h_next->total>=c->total) { h_next->h_next=h_next->h_next->h_next;   continue;}
        laikniekonera=clock();//timeauto atidejimas
        }
     h_next=c;}

//Get a bounding rectangle around the moving object.
bndRect = cvBoundingRect(contour, 0);

pt1.x = bndRect.x;
pt1.y = bndRect.y;
pt2.x = bndRect.x + bndRect.width;
pt2.y = bndRect.y + bndRect.height;

//Get an average X position of the moving contour.
avgX = (pt1.x + pt2.x) / 2;
//**********************************************
    if (clock() > (laikassenas+1500))//reikia, kad nurimtu vaizdas
    {  if (abs((aukstis/2) -pt1.y )>20) {
         if (((aukstis/2) - pt1.y )<0) {servo_yk = servo_yk  + pt1.y/8 +5 ;}//
         else{servo_yk =servo_yk - pt1.y/8 -5;}
       }
       if (abs((plotis/2 -avgX ) )>20) {
         if (((plotis )/2 - avgX )>0) {servo_xk = servo_xk  + avgX /4 +5 ;}
         else{servo_xk = servo_xk - avgX /4 -5 ;}
       }
        servo_greitisX= 300 + abs(plotis/2 - avgX  )/4;
        servo_greitisY= 200 +abs(pt1.y-aukstis/2)/4;
        //servo_xksen = servo_xk ;
        //laikassenas = clock();
        servo_x = servo_xk; servo_y = servo_yk; servo_xd = servo_xk; servo_yd = servo_yk;

        pthread_create(&pthread_Mechanika, NULL,Mechanika, NULL);
        pthread_join(pthread_Mechanika, NULL);//NULL

     laikassenas=clock();}
//***************************************************

//If the contour is within the edges of the building...
if(avgX > 0 && avgX < plotis){//90 , 250
//If the the previous contour was within 2 of the left boundary...
    if(closestToLeft >= 0 && closestToLeft <= 2){
        //If the current X position is greater than the previous...
        if(avgX > prevX){judesiutmp++;
        closestToLeft = 0;}//Reset the closest object to the left indicator.
    }
    //else if the previous contour was within 2 of the right boundary...
    else if(closestToRight >= plotis-2 && closestToRight <= plotis){
        //If the current X position is less than the previous...
        if(avgX < prevX){judesiutmp++;//Increase the number of people.
        closestToRight = plotis;}//Reset the closest object to the right counter.
    }
//Draw the bounding rectangle around the moving object.
cvRectangle(shownImg1, pt1, pt2, CV_RGB(255,0,0), 1);
}
//If the current object is closer to the left boundary but still not across
//it, then change the closest to the left counter to this value.
if(avgX > closestToLeft && avgX <= 2){closestToLeft = avgX;}

//If the current object is closer to the right boundary but still not across
//it, then change the closest to the right counter to this value.
if(avgX < closestToRight && avgX >= plotis-2){closestToRight = avgX;}
//Save the current X value to use as the previous in the next iteration.
prevX = avgX;
}
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.8, 0.8, 0, 2);
cvPutText(shownImg1, _itoa(judesiutmp, wow, 10), cvPoint(10, 20), &font, cvScalar(0, 0, 300));

 if (ekranorezimas>0){cvShowImage("Kaire", shownImg1);}

    keyPressed =cvWaitKey(1);

} //while pabaiga

} // Judesys pabaiga

//#################################################################################################
// apvalaus objekto sekimas ******************************************************************************************

CvPoint2D32f center;
float radius;

int  objsekimas( void)
{plotis=640; aukstis=480;
cvReleaseCapture( &camera1 );
int   k;
while(1){
    camImg1  = getCameraFrame1(kamera2);//
    //if (!camImg1) {printf("KLAIDA ,nera kameros \n");break; }//exit(1);
    FindBall(camImg1);
    cvShowImage( "Kaire", camImg1 );
    keyPressed  = cvWaitKey(1);
    if( (char)keyPressed  == 'm' ){  break;}

      }//while
cvReleaseCapture( &camera1 );
}// pabaiga  objsekimas

//*********************************************************************
int  FindBall(IplImage* Img)
{
IplImage* Image=cvCreateImage( cvGetSize(Img), 8, 3 );
cvCopy(Img,Image);

//Òåïåðü íåîáõîäèìî ïîëó÷èòü äîñòóï ê âñåì ïèêñåëÿì.

uchar* ptr1;
ptr1 = (uchar*) (Image->imageData );
int i,j;

for(i=4;i<Img->height-4;i++) //buvo i=0 ir i<Img->height

for(j=0;j<Img->width;j++){

//R > 1.5*G, R > 2*B

if (ptr1[j*3+2+i*Image->widthStep]>1.5*ptr1[j*3+1+i*Image->widthStep] && ptr1[j*3+2+i*Image->widthStep]>2*ptr1[j*3+i*Image->widthStep]){
    ptr1[j*3+i*Image->widthStep]=255;
    ptr1[j*3+1+i*Image->widthStep]=255;
    ptr1[j*3+2+i*Image->widthStep]=255;}

else{ptr1[j*3+i*Image->widthStep]=0;
    ptr1[j*3+1+i*Image->widthStep]=0;
    ptr1[j*3+2+i*Image->widthStep]=0;}
}
//Âûäåëåíèå êîíòóðîâ è ïîèñê íàèáîëüøåãî êîíòóðà
Counter(Image);

if (center.x>-1)  {CvPoint p;  p.x=center.x;  p.y=center.y;
cvCircle( Img, p, radius, CV_RGB(255,0,0), 3, 8, 0 ); }// ( Img, p, radius, CV_RGB(255,0,0), 3, 8, 0 )
cvCopy(camImg1,Image);
cvReleaseImage( &Image );

}// FindBall apvalaus objekto sekimo pabaiga

//***********************************************************************
void Counter(IplImage* img)
{
IplImage* img_gray= cvCreateImage( cvSize(img->width,img->height), 8, 1);
CvSeq* contours = 0;
CvMemStorage* storage = cvCreateMemStorage(0);
cvCvtColor( img, img_gray, CV_BGR2GRAY );
cvFindContours( img_gray, storage, &contours, sizeof(CvContour),
CV_RETR_LIST  , CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );
CvSeq* h_next=0;

//Ïîèñê ìàêñèìàëüíîãî êîíòóðà

for( CvSeq* c=contours; c!=NULL; c=c->h_next ){

    if (c!=contours){
//Ïðîâåðÿåì êàêîé êîíòóð áîëüøå
      if (h_next->total>=c->total){h_next->h_next=h_next->h_next->h_next; continue;}
    }
h_next=c;}

center.x=-1;

if (h_next->total<200) return;//íåò  ìÿ÷à – ñëèøêîì ìàëåíüêèå êîíòóðû

cvDrawContours( img, h_next, CV_RGB(255,0,0), CV_RGB(0,255,0),2, 2, CV_AA, cvPoint(0,0) );

//Ìèíèìàëüíàÿ îêðóæíîñòü
cvMinEnclosingCircle(h_next,&center,&radius);
cvReleaseMemStorage( &storage);
cvReleaseImage( &img_gray );
}// counter apvalaus objekto sekimo pabaiga

// pabaiga objekto sekimas********************************************************************************************

void akys_servo (void){
           if (servo_x>servo_xmax) {servo_x=servo_xmax;}
           else if (servo_x<servo_xmin) {servo_x=servo_xmin;}
           if (servo_y>servo_ymax) {servo_y=servo_ymax;}
           else if (servo_y<servo_ymin) {servo_y=servo_ymin;}
           if (servo_xk>servo_xmax) {servo_xk=servo_xmax;}
           else if (servo_xk<servo_xmin) {servo_xk=servo_xmin;}
           if (servo_yk>servo_ymax) {servo_yk=servo_ymax;}
           else if (servo_yk<servo_ymin) {servo_yk=servo_ymin;}
           if (servo_xd>servo_xmax) {servo_xd=servo_xmax;}
           else if (servo_xd<servo_xmin) {servo_xd=servo_xmin;}
           if (servo_yd>servo_ymax) {servo_yd=servo_ymax;}
           else if (servo_yd<servo_ymin) {servo_yd=servo_ymin;}
           if (servo_greitisX>600) {servo_greitisX=600;}
           else if (servo_greitisX<200) {servo_greitisX=200;}
           if (servo_greitisY>600) {servo_greitisY=600;}
           else if (servo_greitisY<20) {servo_greitisY=20;}
        //**** Servo X ************************************
            gf.gotoxy(7,50);printf("                ");
            gf.gotoxy(7,50);printf("Servo X = ");printf("%i",servo_x);
            strcpy(buftmp,"#7 P"); // 7 servo
           itoa(servo_x,buferis,10);
           strcat(buftmp,buferis);
           strcat(buftmp," S");//
           itoa(servo_greitisX,buferis,10);
           strcat(buftmp,buferis);  //
           strcat(buftmp," |");// S
            strcpy(buferis,buftmp);
            gf.gotoxy(7,51);  printf(buferis);
           cprintft(cport_nr,buferis);//com -> servo duom
            SendByte(cport_nr, 0x0D); //komandos pabaiga 0x0D
        //**** Servo Y **************************************
             gf.gotoxy(7,52);printf("                ");
            gf.gotoxy(7,52);printf("Servo Y = ");printf("%i",servo_y);
            strcpy(buftmp,"#0 P");
           itoa(servo_y,buferis,10);
           strcat(buftmp,buferis);
           strcat(buftmp," S");//
           itoa(servo_greitisY,buferis,10);
           strcat(buftmp,buferis);  //
           strcat(buftmp," |");// S
            strcpy(buferis,buftmp);
            gf.gotoxy(7,53);printf(buferis);
           cprintft(cport_nr,buferis);//com -> servo duom
            SendByte(cport_nr, 0x0D); //komandos pabaiga 0x0D
};
//#################################################################################################
void lptout(int lptportas,uint8_t lptreiksme ){//nedarasytas
   HINSTANCE hDLL = LoadLibrary("inpout32.dll");

   TInp32Func Inp32;
   TOut32Func Out32;
   BYTE r;

   if (hDLL!=NULL)
   {    Inp32 = (TInp32Func)GetProcAddress(hDLL, "Inp32");
        Out32 = (TOut32Func)GetProcAddress(hDLL, "Out32");

      if (Out32) Out32(lptportas, lptreiksme );
     // if (Inp32) { r = Inp32(888); printf("r=0x%02X\n", r);}
    //  if (Out32) Out32(888, 2);
     // if (Inp32) {r = Inp32(888); printf("r=0x%02X\n", r);}
    //  if (Out32) Out32(888, 4);
    //  if (Inp32) {r = Inp32(888); printf("r=0x%02X\n", r);}
    //  if (Out32) Out32(888, 0);

      FreeLibrary(hDLL);
   }

}
//#################################################################################################
uint8_t ssc32in (){//nedarasytas
int j ;
unsigned char buferiukas[8];

        //gf.gotoxy(4,47);strcat(buferis,"ver");  printf("A B C D ");
        cprintft(cport_nr,"A B C D");//buferis servo versijos duom uzklausa -> com
        SendByte(cport_nr, 0x0D); //komandos pabaiga 0x0D
Sleep(3); //reikalindas nes be jo uzgriebia siuntima
        gf.gotoxy(4,55);
  n = PollComport(cport_nr, buferiukas, 8);
    if(n > 0){buferiukas[n] = 0;   /* always put a "null" at the end of a string! */
      for(i=0; i < n; i++) {if(buferiukas[i] < 32) {buferiukas[i] = '.';} }/* replace unreadable control-codes by dots */
      gf.setColor(32);
      printf("A B C D : %s     'SSC32' atsakymas: %i baitai.\n", (char *)buferiukas, n);}
      gf.setColor(128);
#ifdef _WIN32
  //  Sleep(100);  /*  it's ugly to use a sleeptimer, in a real program, change the while-loop into a (interrupt) timerroutine */
#else
   usleep(100000);  /* sleep for 100 milliSeconds */
#endif



 if (buferiukas[3]=='0'){   daviklisSSC=0b10000000;
    if (buferiukas[2]=='0'){daviklisSSC=0b01000000|daviklisSSC;  }
    if (buferiukas[1]=='0'){daviklisSSC=0b00100000|daviklisSSC;  }
    if (buferiukas[0]=='0'){daviklisSSC=0b00010000|daviklisSSC;  }
  }
    else {
    if (buferiukas[2]=='0'){daviklisSSC=0b00000100|daviklisSSC;  }
    if (buferiukas[1]=='0'){daviklisSSC=0b00000010|daviklisSSC;  }
    if (buferiukas[0]=='0'){daviklisSSC=0b00000001|daviklisSSC;  }
    }
return (daviklisSSC);
}

//#################################################################################################

uint32_t vazdavikliai()
{
return (daviklisePD);
}


//#################################################################################################
void rndservo(void)
{       servo_x = servo_xmin + (rand() * (servo_xmax - servo_xmin) / RAND_MAX);
        servo_y = servo_ymin-(servo_ymax - servo_ymin)/2 + (rand() * (servo_ymax - servo_ymin) / RAND_MAX);
		servo_xk=servo_x; servo_yk=servo_y;
		servo_xd=servo_x; servo_yd=servo_y;
		servo_xdsen=servo_x; servo_ydsen=servo_y;
		servo_xksen=servo_x; servo_yksen=servo_y;
}
//#################################################################################################
void paieska_servo(void)
{   //nedarasytas
        servo_x = servo_xmin + (rand() * (servo_xmax - servo_xmin) / RAND_MAX);
        servo_y = servo_ymin-(servo_ymax - servo_ymin)/2 + (rand() * (servo_ymax - servo_ymin) / RAND_MAX);
		servo_xk=servo_x; servo_yk=servo_y;
		servo_xd=servo_x; servo_yd=servo_y;
		servo_xdsen=servo_x; servo_ydsen=servo_y;
		servo_xksen=servo_x; servo_yksen=servo_y;
}
//#################################################################################################
int vaziuokle(uint8_t vazrezimas,clock_t vazONlaikas)
{//nedarasytas
clock_t esamasLaikas;
        //int eProDas_SetPin(unsigned int DeviceIdx, unsigned int Port, unsigned int Pin);
        //int eProDas_ClearPin(unsigned int DeviceIdx, unsigned int Port, unsigned int Pin);
        //int eProDas_TogglePin(unsigned int DeviceIdx, unsigned int Port, unsigned int Pin);
        //int eProDas_TogglePinTwice(unsigned int DeviceIdx, unsigned int Port,  unsigned int Pin);
    switch (vazrezimas){

        case (pirmyn ):{

            if ((RB0==1)){
            esamasLaikas = clock();
            RB0=0;
            Status =  eProDas_ConfigurePinAsOutput( DeviceIdx, 1,0, RB0);}


            if ((RB2==1)){
            esamasLaikas = clock();
            RB2=0;
            Status =  eProDas_ConfigurePinAsOutput( DeviceIdx, 1,2, RB2);}


            if   ((RB0==0)){
            if   (((esamasLaikas + vazONlaikas) <= clock())){
            RB0=1;
            Status =  eProDas_ConfigurePinAsOutput( DeviceIdx, 1,0, RB0);}
            }

            if   (RB2==0){
            if   (((esamasLaikas +  vazONlaikas)<= clock())){
            RB2=1;
            Status =  eProDas_ConfigurePinAsOutput( DeviceIdx, 1,2, RB2);}
            }


       break;}
//******************************************************
        case (atgal ):{

        break;}
//******************************************************
        case (kairen  ):{

        break;}
//******************************************************
        case (desinen ):{

        break;}

//******************************************************
        case (vkairen ):{

        break;}
//******************************************************

        case (vdesinen  ):{

        break;}
//******************************************************

        case (akairen  ):{

        break;}

//******************************************************
        case (adesinen ):{

        break;}

  //continue;
}

//******************************************************
return(vazrezimas);

}//int vaziuokle() pabaiga

//#################################################################################################
void sysinitas(void)
{
 cvUseOptimized(true);

WinExec("eSpeak.exe -v en+m4  ashsh",1);
gf.wait(700);
WinExec("eSpeak.exe -v lv+m4  ee`szu....rroobootasz",1);
    prisijungimas=do_connect((char*)"localhost",10500);//jungiames per socket prie kalbos atpazinimo servo
cvUseOptimized(1); // load IPP
char *plugin_info;
    cvGetModuleInfo(0,0,(const char**) &plugin_info);
    gf.gotoxy(4,38);
    bool ipp_is_used = plugin_info != 0 && strstr(plugin_info,"ipp")!=0;//tbb
  if (ipp_is_used){gf.setColor(32);printf("* IPP  naudojamas\n"); Sleep(2000);}
  else{gf.setColor(192);printf("* IPP nenaudojamas! \n"); Sleep(2000);}


 if (prisijungimas<2){gf.setColor(192);gf.gotoxy(4,40);printf("NEPRISUJUNGIA prie 'JULIUS' serverio- (kalbos atpazinimas)\n");gf.setColor(128);

//ShellExecute(handle,"open","julian.exe ","-C server_robot.jconf  -input mic -module","../Julius/bin/",SW_SHOWDEFAULT);
 //WinExec("SERVER_robot.bat",SW_SHOW );
 //gf.wait(5000);
 //prisijungimas=do_connect("localhost",10500);//jungiames per socket prie kalbos atpazinimo servo
 }
 else
 {gf.setColor(32);gf.gotoxy(4,40); printf("PRISIJUNGE prie 'JULIUS' serverio- (kalbos atpazinimas)\n");  komanda_pause_recog(prisijungimas);gf.setColor(128);}


//gf.setColor(128);
 if(OpenComport(cport_nr, bdrate))
{    gf.gotoxy(4,42);gf.setColor(192);
    printf("Negaliu atidaryti COM porto :");
    itoa(cport_nr+1,buferis,10);
     printf(buferis);
    gf.wait(100);gf.setColor(128);
    //return(0);  iseiti jei nera com
}
else //JEI RS232 PORTAS KLAIDOS NEMETA INICIALIZUOTI SSC32
{       gf.setColor(32);gf.gotoxy(4,42);printf("RS232 portas com ");
        itoa(cport_nr+1,buferis,10);printf(buferis);
        gf.gotoxy(4,43);strcat(buferis,"ver");
        cprintft(cport_nr,buferis);// servo versijos duom uzklausa -> com
        SendByte(cport_nr, 0x0D); //komandos pabaiga 0x0D
        gf.setColor(128);
//int j;
        gf.gotoxy(4,44);
        Sleep(5);
//for(j=0; j < 13; j++) //nereikia
  {n = PollComport(cport_nr, buf, 4095);
    if(n > 0){buf[n] = 0;   /* always put a "null" at the end of a string! */
      for(i=0; i < n; i++) {if(buf[i] < 32) {buf[i] = '.';} }/* replace unreadable control-codes by dots */
      gf.setColor(32);
      printf("Servo ID: %s     'SSC32' atsakymas i 'RS232': %i baitu.\n", (char *)buf, n);}
      gf.setColor(128);
#ifdef _WIN32
    Sleep(100);  /* it's ugly to use a sleeptimer, in a real program, change the while-loop into a (interrupt) timerroutine */
#else
   usleep(100000);  /* sleep for 100 milliSeconds */
#endif
  }
if (buf[1]!='S') {gf.setColor(192);gf.gotoxy(4,44);printf("Neprijungtas 'SSC32', servo valdiklis");gf.setColor(128);}
else {akys_servo();} //servai i pozicija, akytes i pradzia
} //else pabaiga RS232 PORTAS KLAIDOS NEMETA
gf.wait(100);

StatuseProDas = eProDas_OpenHandle();
if (StatuseProDas !=0)
{   gf.setColor(192);
    gf.gotoxy(4,46);
    printf("Neprijungtas 'eProDas', USB vaziuokles valdiklis");
    gf.setColor(128);
    gf.wait(100);
    }//return(0);  iseiti jei nera eProDAS
else {gf.setColor(32);gf.gotoxy(4,46);
    printf("Prijungtas 'eProDas', USB vaziuokles valdiklis");
    gf.wait(100);gf.setColor(128);}

   StatuseProDas = eProDas_Initialize(DeviceIdx);//DeviceIdx=0
    if (StatuseProDas!= eProDas_Error_SUCCESS)
    {gf.setColor(192);gf.gotoxy(4,48);
    printf("'eProdas' valdiklio inicializacijos KLAIDA ");//
     gf.setColor(128);gf.wait(100);}//return;

else {gf.setColor(32);gf.gotoxy(4,48);
    printf("'eProDas' valdiklis inicializavosi");
    gf.setColor(128);gf.wait(100);
    //****  eProdas input/output konfiguracija  **********************
     //Pradziai kiekviena bita projektavimo paskui pakeisti i viso porto vienoda -> eProDas_ConfigurePort
    InitialState=1; //kaip output
    //PORTAS RB OUTPUT
        Pin=0; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB ,Pin, InitialState);//RB0 isvadas
        Pin=1; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB ,Pin, InitialState);//RB1 isvadas
        Pin=2; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB ,Pin, InitialState);//RB2 isvadas
        Pin=3; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB ,Pin, InitialState);//RB3 isvadas
        Pin=4; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB ,Pin, InitialState);//RB4 isvadas
        Pin=5; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB ,Pin, InitialState);//RB5 isvadas
        Pin=6; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB ,Pin, InitialState);//RB6 isvadas
        Pin=7; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB ,Pin, InitialState);//RB7 isvadas
    //kaip Input Portas E
        Pin=0; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTE ,Pin);//RE0 isvadas
        Pin=1; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTE ,Pin);//RE1 isvadas
        Pin=2; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTE ,Pin);//RE2 isvadas
        Pin=3; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTE ,Pin);//RE3 isvadas
    //kaip Input Portas D
        Pin=0; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTB ,Pin);//RD0 isvadas
        Pin=1; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTB ,Pin);//RD1 isvadas
        Pin=2; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTB ,Pin);//RD2 isvadas
        Pin=3; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTB ,Pin);//RD3 isvadas
        Pin=4; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTB ,Pin);//RD4 isvadas
        Pin=5; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTB ,Pin);//RD5 isvadas
        Pin=6; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTB ,Pin);//RD6 isvadas
        Pin=7; eProDas_ConfigurePinAsInput( DeviceIdx, eProDas_Index_PORTB ,Pin);//RD7 isvadas
    }


}//Systemos inito pabaiga***************************************


//#################################################################################################
    // Ñðàâíåíèå äâóõ îñîáåííîñòåé
double compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
{   double total_cost = 0;
    //assert( length % 4 == 0 );
    for( int i = 0; i < length; i += 4 )
    {   double t0 = d1[i] - d2[i];
        double t1 = d1[i+1] - d2[i+1];
        double t2 = d1[i+2] - d2[i+2];
        double t3 = d1[i+3] - d2[i+3];
        total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
        if( total_cost > best )
            break;
    }
    return total_cost;
}
 //#################################################################################################
    // Ñðàâíèâàåò îäíó îñîáåííîñòü îáúåêòà ñî âñåìè îñîáåííîñòÿìè ñöåíû
int naiveNearestNeighbor( const float* vec, int laplacian,
                    const CvSeq* model_keypoints, const CvSeq* model_descriptors )

{   int length = (int)(model_descriptors->elem_size/sizeof(float));
    int i, neighbor = -1;
    double d, dist1 = 1e6, dist2 = 1e6;
    CvSeqReader reader, kreader;
    // Íà÷àëüíàÿ îñîáåííîñòü ñöåíû
    cvStartReadSeq( model_keypoints, &kreader, 0 );
    cvStartReadSeq( model_descriptors, &reader, 0 );
    // Ïåðåáîð âñåõ îñîáåííîñòåé ñöåíû
    for( i = 0; i < model_descriptors->total; i++ )
    {   const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
        const float* mvec = (const float*)reader.ptr;
        CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
        CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
        // Äëÿ óñêîðåíèÿ ñíà÷àëà ñðàâíèâàåòñÿ ëàïëàñèàí îñîáåííîñòåé
        if( laplacian != kp->laplacian )
            continue;
        // Ñðàâíåíèå îñîáåííîñòåé
        d = compareSURFDescriptors( vec, mvec, dist2, length );
        if( d < dist1 )
        {   // Íàéäåíà ëó÷øåå ñîâïàäåíèå îñîáåííîñòåé
            dist2 = dist1;
            dist1 = d;
            neighbor = i;
        }
        else if ( d < dist2 )   dist2 = d;
    }
    if ( dist1 < 0.6*dist2 )
        return neighbor;
    return -1;
	}
//#################################################################################################
    // Ôóíêöèÿ èùåò ñîâïàäàþùèå ïàðû
void findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
           const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>&ptpairs )
{   int i;
    CvSeqReader reader, kreader;
    // Óñòàíîâêà íà÷àëüíîé îñîáåííîñòè îáúåêòà ðàñïîçíàâàíèÿ
    cvStartReadSeq( objectKeypoints, &kreader );
    cvStartReadSeq( objectDescriptors, &reader );
    ptpairs.clear();
    // Ïåðåáîð âñåõ îñîáåííîñòåòåé îáúåêòà
    for( i = 0; i < objectDescriptors->total; i++ )
    {   const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
        const float* descriptor = (const float*)reader.ptr;
        CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
        CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
        // Ñðàâíåíèå òåêóùåé îñîáåííîñòè ñî âñåìè îñîáåííîñòÿìè èç ñöåíû
        int nearest_neighbor = naiveNearestNeighbor( descriptor, kp->laplacian,
imageKeypoints, imageDescriptors );
        if( nearest_neighbor >= 0 )
        {   // Íàøëîñü ñîâïàäåíèå îñîáåííîñòåé
            ptpairs.push_back(i);
            ptpairs.push_back(nearest_neighbor);
        }
    }
}
//#########################################################################################
// Ãðóáîå íàõîæäåíèå ìåñòîïîëîæåíèÿ îáúåêòà

int locatePlanarObject( const CvSeq* objectKeypoints, const CvSeq*
objectDescriptors, const CvSeq* imageKeypoints, const CvSeq* imageDescriptors,
                   const CvPoint src_corners[4], CvPoint dst_corners[4] )
{   double h[9]; int i, n;
    CvMat _h = cvMat(3, 3, CV_64F, h);
    vector<int> ptpairs;
    vector<CvPoint2D32f> pt1, pt2;
    CvMat _pt1, _pt2;
    // Èùåì ïàðû îñîáåííîñòåé íà îáåèõ êàðòèíêàõ, êîòîðûå ñîîòâåòñòâóþò
    // äðóã äðóãó
    findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
    n = ptpairs.size()/2;
    // Åñëè ïàð ìàëî, çíà÷èò íàäî âûõîäèòü – îáúåêò íå íàéäåí buvo 4
    if( n < 4 )   return 0;

    // Âûäåëÿåì ïàìÿòü
    pt1.resize(n);
    pt2.resize(n);
    // Ñ÷èòûâàåì êîîðäèíàòû «îñîáûõ»òî÷åê
    for( i = 0; i < n; i++ )
    {   pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
        pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
    }
    // Ïî ïîëó÷åííûì âåêòîðàì ñîçäà¸ì ìàòðèö
    _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
    _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
    // Íàõîäèì òðàíñôîðìàöèþ ìåæäó èñõîäíûì èçîáðàæåíèåì è ñ òåì, êîòîðîå
    // èùåì
    if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))
        return 0;
    // Ïî ïîëó÷åííîìó çíà÷åíèþ òðàíñôîðìàöèè (â ìàòðèöó _h) íàõîäèì
	 // êîîðäèíàòû ÷åòûð¸õóãîëüíèêà, õàðàêòåðèçóþùåãî îáúåêò
    for( i = 0; i < 4; i++ )
    {   double x = src_corners[i].x, y = src_corners[i].y;
        double Z = 1./(h[6]*x + h[7]*y + h[8]);
        double X = (h[0]*x + h[1]*y + h[2])*Z;
        double Y = (h[3]*x + h[4]*y + h[5])*Z;
        dst_corners[i] = cvPoint(cvRound(X), cvRound(Y));
    }
    return 1;
}

//   daikto paieska #########################################################################################
int daiktas (int kaip_ieskoti, char** argv)
{ plotis=640; aukstis=480;
cvReleaseCapture( &camera1 );
	FILE * imgListFile = 0;
	const char* object_filename ;
	char imgFilename[64];
IplImage* object;
CvSize objekto_rezolke;
float  santykis, santykis_esamas;
CvPoint objekto_prad_taskas;
float   objekto_izambine, objekto_izambine1,objekto_izambine2,objekto_plotis ,objekto_aukstis;
    // Èíèöèàëèçàöèÿ ïàðàìåòðîâ
    if (kaip_ieskoti==0) {object = daikto_isskyrimas();}
    else if (kaip_ieskoti==1) {object_filename = ( char*) argv;
 imgListFile = fopen(object_filename, "r");
 object = cvLoadImage( object_filename, CV_LOAD_IMAGE_GRAYSCALE );}
if( !object ){printf("Nera objekto  paveikslelio");Sleep(2000);return(-1);}
    WinExec("eSpeak.exe -v lv+m4  kuor.........taasz.......daayjktaasz",1);
    CvMemStorage* storage = cvCreateMemStorage(0);

    cvNamedWindow("Objektas", 1);
    cvMoveWindow("Objektas", 1, 1);
    hWnd=FindWindow( NULL,"Objektas");
   SetForegroundWindow(hWnd);
    cvNamedWindow("Paieska", 1);
    cvMoveWindow("paieska", plotis, 1);
       hWnd=FindWindow( NULL,"Paieska");
   SetForegroundWindow(hWnd);
    static CvScalar colors[] =
    {
        {{0,0,255}},
        {{0,128,255}},
        {{0,255,255}},
        {{0,255,0}},
        {{255,128,0}},
        {{255,255,0}},
        {{255,0,0}},
        {{255,0,255}},
        {{255,255,255}}
    };

   //



        gf.clrscr();gf.setColor(128);
        gf.showRightSide((char *)" Rezoliucija : ",1);printf(" %dx%d.",plotis,aukstis);
        gf.showRightSide((char *)" [m]   - uzdaryti  programa");
        gf.showRightSide((char *)"ir iskviesti pagrindini meniu");
		gf.showRightSide((char *)" [s]   - (x , y) servus i centra");
		gf.showRightSide((char *)" [e]   - kameru langu vaizdas");
		gf.showRightSide((char *)" [f]   - rodyti kameru vaizda");
        gf.showRightSide((char *)" [n]   - isjungti kameru vaizda");
		gf.showRightSide((char *)" []- ");

    CvSeq *objectKeypoints = 0, *objectDescriptors = 0;
    CvSeq *imageKeypoints = 0, *imageDescriptors = 0;
    int i;
    // Ïåðåâîä â ãðàäàöèè ñåðîãî
    objekto_rezolke  =(cvGetSize(object));
    IplImage* object_color = cvCreateImage(objekto_rezolke, 8, 3);//
    cvCvtColor( object, object_color, CV_GRAY2BGR );

     objekto_plotis=(object->width);
    objekto_aukstis=(object->height);
   // objekto_izambine=sqrt(objekto_plotis^2+objekto_aukstis^2);
    santykis=objekto_plotis/objekto_aukstis;
         objekto_plotis=0;
    objekto_aukstis=1;
    // Èíèöèàëèçàöèÿ ñòðóêòóðû CvSURFParams ñ ðàçìåðîì äåñêðèïòîðîâ â 128
    // ýëåìåíòîâ
    CvSURFParams params = cvSURFParams(500, 1);//buvo (500, 1)

    // Èùåì îñîáåííîñòè îáúåêòà ðàñïîçíàâàíèÿ
    cvExtractSURF( object, 0, &objectKeypoints, &objectDescriptors, storage,params );


    while (1)
    {
    camImg1 = getCameraFrame1(kamera2); //if( !camImg1 ){printf("Nera  paveikslelio is kameros");Sleep(2000);return(-1);} // exit(-1);
    cvNormalize(camImg1, camImg1, 0, 255, CV_MINMAX , CV_8U  );//   CV_L2
    IplImage*  image = convertImageToGreyscale(camImg1);




    double tt = (double)cvGetTickCount();

    gf.gotoxy(4,38);
    printf("Objecto descriptoriu:  %d\n", objectDescriptors->total);
    // Èùåì îñîáåííîñòè ñöåíû
    cvExtractSURF( image, 0, &imageKeypoints, &imageDescriptors, storage,params );
    gf.gotoxy(4,40);
    printf("Pavyzdzio descriptoriu: %d\n", imageDescriptors->total);
    // Ñêîëüêî ïîòðåáîâàëîñü âðåìåíè (Ó ìåíÿ 167 ìèëëè ñåêóíä)
    tt = (double)cvGetTickCount() - tt;
    gf.gotoxy(4,42);
    printf( "Paieskos laikas = %gms\n", tt/(cvGetTickFrequency()*1000.));

    // Óñòàíàâëèâàåì ãðàíèöû èçîáðàæåíèé, âíóòðè êîòîðûõ áóäóò ñðàâíèâàòüñÿ

	   //îñîáåííîñòè
    CvPoint src_corners[4] = {{0,0}, {object->width,0}, {object->width, object->height}, {0, object->height}};

    CvPoint dst_corners[4];
    // Ñîçäàíèå äîïîëíèòåëüíîãî èçîáðàæåíèå (â í¸ì áóäåò ñöåíà è îáúåêò)
    // Çàïóñòèòå ïðèìåð è ïîéì¸òå î ÷¸ì ðå÷ü
    IplImage* correspond = cvCreateImage( cvSize(image->width, object->height+image->height), 8, 1 );//8, 1

    cvSetImageROI( correspond, cvRect( 0, 0, object->width, object->height ) );
    cvCopy( object, correspond );
    cvSetImageROI( correspond, cvRect( 0, object->height, correspond->width,correspond->height ) );

    cvCopy( image, correspond );
    cvResetImageROI( correspond );
    // Âûçûâàåì ôóíêöèþ, íàõîäÿùóþ îáúåêò íà ýêðàíå
    if( locatePlanarObject( objectKeypoints, objectDescriptors, imageKeypoints,
        imageDescriptors, src_corners, dst_corners ))
    {   CvPoint r1;
        CvPoint r2;
        // Îáâîäèì íóæíûé ÷åòûð¸õóãîëüíèê
       for( i = 0; i < 4; i++ )  { r1 = dst_corners[i%4];
                                   r2 = dst_corners[(i+1)%4];
                if (i==0) { objekto_plotis=abs(r1.x-r2.x);objekto_prad_taskas=r1;}
                if (i==1) {objekto_aukstis=abs(r1.y-r2.y);}
               if (i==2) { objekto_plotis=(abs(r1.x-r2.x)+objekto_plotis)/2;}
                if (i==3) {objekto_aukstis=(abs(r1.y-r2.y)+objekto_aukstis)/2;}
                                    cvLine( correspond, cvPoint(r1.x, r1.y+object->height ),
                                    cvPoint(r2.x, r2.y+object->height ), colors[8] );}

//cvRectangle(correspond, cvPoint(r1.x, r1.y+object->height), cvPoint(r2.x+object->width , r2.y+object->height), CV_RGB(0,255,0), 1, 8, 0);
           //**********************************************


santykis_esamas=(objekto_plotis/objekto_aukstis);
gf.gotoxy(1,44);
printf("  Santykis= %f% \n",santykis);
printf("EsSantykis= %f% \n",santykis_esamas);
if ((santykis_esamas>0.2)&&(santykis_esamas<5)){
     if  (abs(santykis-santykis_esamas)<0.15){
       pthread_create(&pthread_Mechanika, NULL,Mechanika, NULL);//pries tai, geriau vaizdas veluoja

            if (abs(((correspond->height-object->height)/2-objekto_aukstis/2-objekto_prad_taskas.y) )>20) {
                if ((((correspond->height-object->height)/2-objekto_aukstis/2-objekto_prad_taskas.y) )<0) {servo_yd = servo_yd  + abs(objekto_prad_taskas.y -(aukstis-objekto_aukstis/2)/2)/20 +5 ;}//
                else{servo_yd =servo_yd - abs(objekto_prad_taskas.y -(aukstis-objekto_aukstis/2)/2)/15 -5;}
             }
            if (abs(((plotis/2) - objekto_plotis/2-objekto_prad_taskas.x) )>30) {
                if (((plotis/2 -objekto_plotis/2-objekto_prad_taskas.x)) >0) {servo_xd = servo_xd  + abs((plotis-objekto_plotis/2)/2  - objekto_prad_taskas.x  )/15 +5 ;}
                else{servo_xd = servo_xd - abs((plotis-objekto_plotis/2)/2  - objekto_prad_taskas.x  )/10 -5 ;}
            }
                servo_greitisX= 100 + abs((plotis-objekto_plotis/2)/2  - objekto_prad_taskas.x  )/4;
                servo_greitisY= 100 +abs(objekto_prad_taskas.y -(aukstis-objekto_aukstis/2)/2)/6;
                servo_x = servo_xd; servo_y = servo_yd;
      }//NULL

    }
            santykis_esamas=0;
          //***************************************************
        }
    // Åñëè â ýòîì ìåñòå âûâåñòè ðåçóëüòàò íà ýêðàí, òî ïîëó÷èòüñÿ òî, ÷òî
    // ïîêàçàíî íà ðèñóíêå 23.3.

    vector<int> ptpairs;
    // Ñíîâà èùóòñÿ âñå ñîâïàäàþùèå ïàðû îñîáåííîñòåé â îáåèõ êàðòèíêàõ
    findPairs( objectKeypoints, objectDescriptors, imageKeypoints,imageDescriptors, ptpairs );

    // Ìåæäó ïàðàìè îñîáåííîñòåé íà ðèñóíêå ïðîâîäÿòñÿ ïðÿìûå
	    for( i = 0; i < (int)ptpairs.size(); i += 2 ){
        CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( objectKeypoints,ptpairs[i] );
        CvSURFPoint* r2 = (CvSURFPoint*)cvGetSeqElem( imageKeypoints,ptpairs[i+1] );
        cvLine( correspond, cvPointFrom32f(r1->pt),cvPoint(cvRound(r2->pt.x), cvRound(r2->pt.y+object->height)),colors[8] );
      }
     pthread_join(pthread_Mechanika, NULL);
    cvShowImage( "Paieska", correspond );//

if (ekranorezimas==2){
    // Âûäåëÿåì îñîáåííîñòèî îêðóæíîñòÿìè (Ðèñ. 23.5)
    for( i = 0; i < objectKeypoints->total; i++ )
    {   CvSURFPoint* r = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, i );
        CvPoint center;
        int radius;
        center.x = cvRound(r->pt.x);
        center.y = cvRound(r->pt.y);
        radius = cvRound(r->size*1.2/9.*2);
        cvCircle( object_color, center, radius, colors[0], 1, 8, 0 );

     }
     }//if ekranorezimas==2
cvShowImage( "Objektas", object_color );
 //naikinti , nes eda resursus jei kas-kart nenaikinti
        cvReleaseImage(&object_color);
        cvReleaseImage(&correspond);


         cvClearSeq(imageKeypoints );
        cvClearSeq( imageDescriptors);
        cvReleaseImage(&image);
        delete (&ptpairs);


    keyPressed  = cvWaitKey(1);
    if( (char)keyPressed  == 'm' ){


       // cvRelease(&object);
        cvDestroyWindow("Objektas");
        cvDestroyWindow("Paieska");
 cvClearSeq( objectKeypoints);
 cvClearSeq( objectDescriptors);
        //cvReleaseImage(&camImg1);
        cvReleaseImage(&object);
        break;}
    else if (keyPressed == 's') {  //servo i pradzia
       servo_x=1400; servo_y=1850;servo_xk=1400; servo_yk=1850;servo_xd=1400; servo_yd=1850;
       servo_xdsen=servo_x; servo_ydsen=servo_y;servo_xksen=servo_x; servo_yksen=servo_y;
       servo_greitisX= 700; servo_greitisY= 500; akys_servo();}
//cvClearMemStorage(storage);






}//while pabaiga
 //   servo_x=1400; servo_y=1850;servo_greitisX= 600;servo_greitisY= 400; akys_servo();
cvReleaseMemStorage(& storage);
    cvReleaseCapture( &camera1 );

return(0);
}

//########### Peles apklausa #########################################################################################
void on_mouserect( int event, int x, int y, int, void* selectObject )
{
    if( selectObject )
    {   selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);
//        selection &= cvRect(0, 0,selection ->width ,selection ->height );
     }

    switch( event )
    {
    case CV_EVENT_LBUTTONDOWN:
//        origin = Point(x,y);
//        selection = Rect(x,y,0,0);
//        selectObject = true;
        break;
    case CV_EVENT_LBUTTONUP:
        selectObject = false;
        if( selection.width > 0 && selection.height > 0 )
//            trackObject = -1;
        break;
    }
}
void on_mouse( int event, int x, int y, int flags, void* param )
{   if( !image ) return;
    if( image->origin ) y = image->height - y;
    if( event == CV_EVENT_LBUTTONDOWN ){  //Åñëè íàæàëè ëåâóþ êíîïêó ìûøè, òî äîáàâëÿåì îòñëåæèâàåìóþ òî÷êó
        pt = cvPoint(x,y);
        add_remove_pt = 1;}
}

//########### Tasko sekimas #########################################################################################
int trekingas( int argc, char** argv )
{   cvReleaseCapture( &camera1 );
    plotis=640; aukstis=480;
    servo_xk=1400; servo_yk=1850;


            gf.clrscr();
            gf.showRightSide((char *)" Rezoliucija : ",1);printf(" %dx%d.",plotis,aukstis);
            gf.showRightSide((char *)" ");
            printf ("OpenCV  %s (%d.%d.%d)\n", CV_VERSION,CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
            gf.showRightSide((char *)" [m] - uzdaryti 'Roboto' programa");
            gf.showRightSide((char *)" [r]   - auto tasku sekimas");
            gf.showRightSide((char *)" [c]   - trinti pasirinkima");
            gf.showRightSide((char *)" [n]   - naktinis rez");
            gf.showRightSide((char *)" Pasirinkti sekimo taska pele ");

    cvNamedWindow( "Sekam", 1 );
    cvMoveWindow("Sekam", 10,aukstis+5);  hWnd=FindWindow( NULL,"Sekam");   SetForegroundWindow(hWnd);


   //Óñòàíàâëèâàåì ôóíêöèþ îáðàáîò÷èê ìûøè, ñàìà ôóíêöèÿ íàõîäèòñÿ âûøå
    cvSetMouseCallback( "Sekam", on_mouse, 0 );
   //Ãëàâíûé ðàáî÷èé öèêë ïðîãðàììû - ðàáîòàåò áåñêîíå÷íî, ïîêà âû ïðèíóäèòåëüíî åãî íå çàêðîåòå, íàïðèìåð íàæàâ ESCAPE
   int i, k, c;
    IplImage* frame = 0;
    while(1)
    {   frame = getCameraFrame1(kamera2); //if( !frame  ){printf("Nera  paveikslelio is kameros");Sleep(2000);return(-1);} // exit(-1);
    cvNormalize(frame, frame, 0, 255, CV_MINMAX , CV_8U  );//   CV_L2
  //cvCanny( frame,frame,30,70,3 );
     // cvDilate( frame,frame,0,1 );
      cvErode( frame,frame,0,1 );

        if( !image ){/*Åñëè èçîáðàæåíèå image íå ñîçäàíî, òî íàäî ðàñïðåäåëèòü âñå áóôåðû.
                    Èçîáðàæåíèå íå ñîçäàíî ïåðâîíà÷àëüíî,
                    ïîýòîìó ýòî âñåãî íàâñåãî èíèöèàëèçàöèÿ.*/
            image = cvCreateImage( cvGetSize(frame), 8, 3 );
            image->origin = frame->origin;
            grey = cvCreateImage( cvGetSize(frame), 8, 1 );
            prev_grey = cvCreateImage( cvGetSize(frame), 8, 1 );
            pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
            prev_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
           //Âûäåëÿåì ìàññèâû ïîä òî÷êè, ñóäÿ ïî MAX_COUNT ìàêñèìàëüíî ó íàñ ìîæåò áûòü 500 òî÷åê
            points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
            points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
            status = (char*)cvAlloc(MAX_COUNT);
            flags = 0;}
            //êîïèðîâàíèå è ïåðåâîä â ÷¸ðíî-áåëîå èçîáðàæåíèå
        cvCopy( frame, image, 0 );
        cvCvtColor( image, grey, CV_BGR2GRAY );
            //Ýòî äëÿ ýôôåêòà, êîãäà âû íàæèìàåòå n èçîáðàæåíèå ñòàíîâèòñÿ ò¸ìíûì è âû âèäèòå òîëüêî ïåðåìåùàþùèåñÿ

            //íàçíà÷åííûå âàìè òî÷êè
        if( night_mode ) cvZero( image );

        if( need_to_init ){ //Ýòî íóæíî äëÿ àâòîìàòè÷åñêîé èíèöèàëèçàöèè íàáëþäåíèÿ - åñëè âàì ëåíü
             //çàïóñòèòü àâòîìàòè÷åñêîå ñêàíèðîâàíèå èçîáðàæåíèÿ íà ôàêò âûäåëåíèÿ òûêàòü ìûøêîé, òî ìîæíî ïîäõîäÿùèõ òî÷åê
            IplImage* eig = cvCreateImage( cvGetSize(grey), 32, 1 );
            IplImage* temp = cvCreateImage( cvGetSize(grey), 32, 1 );
            double quality = 0.01;
            double min_distance = 10;
            skaitliukas = MAX_COUNT;
           //îïðåäåëÿåò íàèáîëåå "ñèëüíûå" óãëû èçîáðàæåíèÿ
            cvGoodFeaturesToTrack( grey, eig, temp, points[1], &skaitliukas , quality, min_distance, 0, 3, 0, 0.04 );
            //óòî÷íÿåò ìåñòîïîëîæåíèå óãëîâ
            cvFindCornerSubPix( grey, points[1], skaitliukas ,
            cvSize(win_size,win_size), cvSize(-1,-1),
            cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
            //óäàëÿåò íåíóæíûå èçîáðàæåíèÿ
            cvReleaseImage( &eig );
            cvReleaseImage( &temp );
            add_remove_pt = 0;}

        else if( skaitliukas  > 0 ){//Åñëè åñòü õîòÿ áû îäíà òî÷êà íà ýêðàíå, òî çà íåé íàäî ñëåäèòü
                  //Âû÷èñëÿåì îïòè÷åñêèé ïîòîê íà ôàêò ïîèñêà îñîáåííîñòåé
                  //points[0] - ïðåäûäóùèå òî÷êè points[1] - òåêóùèå òî÷êè
            cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
            points[0], points[1], skaitliukas , cvSize(win_size,win_size),
            3, status, 0, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );

            flags |= CV_LKFLOW_PYR_A_READY;
                  //ïåðåáèðàåì âñå òî÷êè
            for( i = k = 0; i < skaitliukas ; i++ )
            {//Åñëè âû äîáàâèëè íîâóþ òî÷êó íàæàòèåì ìûøè è îíà ñëèøêîì áëèçêà ê ïðåäûäóùåé - ïðîñòî óäàëÿåì å¸

                if( add_remove_pt )
                {   double dx = pt.x - points[1][i].x;
                    double dy = pt.y - points[1][i].y;
                    if( dx*dx + dy*dy <= 25 )  { add_remove_pt = 0; continue;}
                 }

                if( !status[i] )
                    continue;

                points[1][k++] = points[1][i];
               //Îòîáðàæàåì òî÷êó íà ýêðàíå
                cvCircle( image, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0);

            //**********************************************
                if (abs((aukstis/2) -points[1][i].y )>40) {
                if (((aukstis/2) - points[1][i].y)<0) {servo_yk = servo_yk  + points[1][i].y/25 +2 ;}//
                else{servo_yk =servo_yk - points[1][i].y/25 -2;}
                }
                if (abs((plotis/2 -points[1][i].x ) )>30) {
                if (((plotis )/2 - points[1][i].x )>0) {servo_xk = servo_xk  + points[1][i].x /20 +2 ;}
                else{servo_xk = servo_xk - points[1][i].x /20 -2 ;}
                }
                servo_greitisX= 20 + abs(plotis/2 - points[1][i].x  )/70;
                servo_greitisY= 20 +abs(points[1][i].y -aukstis/2)/50;
                servo_x = servo_xk; servo_y = servo_yk;
                pthread_create(&pthread_Mechanika, NULL,Mechanika, NULL);
                pthread_join(pthread_Mechanika, NULL);//NULL
            }//***************************************************
            skaitliukas  = k;
        }

        if( add_remove_pt && skaitliukas  < MAX_COUNT )
        {  //Åñëè òî÷êà ïðîøëà ïðîöåäóðó ïðîâåðêè (ñì. âûøå), òî äîáàâëÿåì å¸ â áóôåð
            points[1][skaitliukas ++] = cvPointTo32f(pt);
           //óòî÷íÿåò ìåñòîïîëîæåíèå óãëîâ äëÿ âñåõ òî÷åê
            cvFindCornerSubPix( grey, points[1] + skaitliukas  - 1, 1,
                cvSize(win_size,win_size), cvSize(-1,-1),
                cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
            add_remove_pt = 0;
        }

        CV_SWAP( prev_grey, grey, swap_temp );
        CV_SWAP( prev_pyramid, pyramid, swap_temp );
            //Ìåíÿåì ìåñòàìè òî÷êè - íîâûå ñòàíîâÿòñÿ ñòàðûìè íà ñëåäóþùåì øàãå öèêëà
        CV_SWAP( points[0], points[1], swap_points );
        need_to_init = 0;
        cvShowImage( "Sekam", image );

        c = cvWaitKey(100);
        if( (char)c == 'm' )  break;

        switch( (char) c )
        {case 'r':  {need_to_init = 1;  break;}
         case 'c': {skaitliukas  = 0;break;}
         case 'n': {night_mode ^= 1;break;}

        default:;

        }
    } //while pabaiga
   // servo_x=1400; servo_y=1850;servo_greitisX= 600;servo_greitisY= 400; akys_servo();
    cvReleaseCapture( &camera1 );
    cvDestroyWindow("Sekam");
    return 0;
} //trekingas pabaiga
//###########################################################################################################

void  update_mhi( IplImage* img, IplImage* dst, int diff_threshold )
{   double timestamp = (double)clock()/CLOCKS_PER_SEC; // get current time in seconds
    CvSize size = cvSize(img->width,img->height); // get current frame size
    int i, idx1 = last, idx2;
    IplImage* silh;
    CvSeq* seq;
    CvRect comp_rect;
    double count;
    double angle;
    CvPoint center;
    double magnitude;
    CvScalar color;
    // allocate images at the beginning or
    // reallocate them if the frame size is changed
    if( !mhi || mhi->width != size.width || mhi->height != size.height ) {
        if( bufff == 0 ) {bufff = (IplImage**)malloc(N*sizeof(bufff[0]));
            memset( bufff, 0, N*sizeof(bufff[0]));}

        for( i = 0; i < N; i++ ) {
            cvReleaseImage( &bufff[i] );
            bufff[i] = cvCreateImage( size, IPL_DEPTH_8U, 1 );
            cvZero( bufff[i] );}

        cvReleaseImage( &mhi );
        cvReleaseImage( &orient );
        cvReleaseImage( &segmask );
        cvReleaseImage( &mask );

        mhi = cvCreateImage( size, IPL_DEPTH_32F, 1 );
        cvZero( mhi ); // clear MHI at the beginning
        orient = cvCreateImage( size, IPL_DEPTH_32F, 1 );
        segmask = cvCreateImage( size, IPL_DEPTH_32F, 1 );
        mask = cvCreateImage( size, IPL_DEPTH_8U, 1 );
    }
    cvCvtColor( img, bufff[last], CV_BGR2GRAY ); // convert frame to grayscale

    idx2 = (last + 1) % N; // index of (last - (N-1))th frame
    last = idx2;

    silh = bufff[idx2];
    cvAbsDiff( bufff[idx1], bufff[idx2], silh ); // get difference between frames

    cvThreshold( silh, silh, diff_threshold, 1, CV_THRESH_BINARY ); // and threshold it

    cvUpdateMotionHistory( silh, mhi, timestamp, MHI_DURATION ); // update MHI

    // convert MHI to blue 8u image
    cvCvtScale( mhi, mask, 255./MHI_DURATION,
                (MHI_DURATION - timestamp)*255./MHI_DURATION );
    cvZero( dst );
    cvCvtPlaneToPix( mask, 0, 0, 0, dst );

    // calculate motion gradient orientation and valid orientation mask
    cvCalcMotionGradient( mhi, mask, orient, MAX_TIME_DELTA, MIN_TIME_DELTA, 3 );

    if( !storage ) storage = cvCreateMemStorage(0);
    else  cvClearMemStorage(storage);
    // segment motion: get sequence of motion components
    // segmask is marked motion components map. It is not used further
    seq = cvSegmentMotion( mhi, segmask, storage, timestamp, MAX_TIME_DELTA );

    // iterate through the motion components,
    // One more iteration (i == -1) corresponds to the whole image (global motion)
    for( i = -1; i < seq->total; i++ ) {
        if( i < 0 ) { // case of the whole image
            comp_rect = cvRect( 0, 0, size.width, size.height );
            color = CV_RGB(255,255,255);
            magnitude = 100;
        }
        else { // i-th motion component
            comp_rect = ((CvConnectedComp*)cvGetSeqElem( seq, i ))->rect;
            if( comp_rect.width + comp_rect.height < 100 ) // reject very small components

                continue;
            color = CV_RGB(255,0,0);
            magnitude = 30;
        }
        // select component ROI
        cvSetImageROI( silh, comp_rect );
        cvSetImageROI( mhi, comp_rect );
        cvSetImageROI( orient, comp_rect );
        cvSetImageROI( mask, comp_rect );

        // calculate orientation
        angle = cvCalcGlobalOrientation( orient, mask, mhi, timestamp,MHI_DURATION);

        angle = 360.0 - angle;  // adjust for images with top-left origin
        count = cvNorm( silh, 0, CV_L1, 0 ); // calculate number of points within silhouette ROI

        cvResetImageROI( mhi ); cvResetImageROI( orient );cvResetImageROI( mask ); cvResetImageROI( silh );

        // check for the case of little motion
        if( count < comp_rect.width*comp_rect.height * 0.05 )
            continue;

        // draw a clock with arrow indicating the direction
        center = cvPoint( (comp_rect.x + comp_rect.width/2),
                          (comp_rect.y + comp_rect.height/2) );

        cvCircle( dst, center, cvRound(magnitude*1.2), color, 3, CV_AA, 0 );
        cvLine( dst, center, cvPoint( cvRound( center.x +
magnitude*cos(angle*CV_PI/180)),
                cvRound( center.y - magnitude*sin(angle*CV_PI/180))), color, 3, CV_AA,0 );

    }
}

//##############################################################################################################
int judesys1(int judesiu)
{       plotis=320; aukstis=240;
    IplImage* motion = 0;

        while(1)
        {   IplImage* image;

            image = getCameraFrame();
          if( image ){
            if( !motion )
                {   motion = cvCreateImage( cvSize(image->width,image->height), 8, 3 );
                    cvZero( motion );
                    motion->origin = image->origin;}
            }
            update_mhi( image, motion, 30 );
             cvAddWeighted(image, 0.6, motion, 0.4, 0.0,motion);
            cvShowImage( "Kaire", motion );
            if( cvWaitKey(1) >= 0 ) break;
        }
      cvReleaseImage(&motion);  //plotis=320; aukstis=240;//cvDestroyWindow( "Judesys" );

    return 0;
}

//stereo kameros kalibravimas paprogrames #################################################################################
// ôóíêöèÿ-îáðàáîò÷èê ïîëçóíêà -
// ïîðÿäîê ïðîèçâîäíîé ïî X
void myTrackbarkameruindelis(int  kameruindelisTMP) {   kameruindelisTMP;}
//#####################################################
void myTrackbarryskumas(int ryskumasTMP) { ryskumasTMP;}
//#####################################################
void myTrackbarslenkstis(int slenkstisTMP) { slenkstisTMP;}
//####################################################
void myTrackbarposlinkis(int poslinkis) { poslinkis=poslinkis;}

//stereo kameros kalibravimas #########################################################################################
void kamkalibr ()//int argc, char *argv[]
{IplImage*  stereosum=0; plotis=640 ;  aukstis=480;
CvButtonCallback slenkst;
float kameruindelis=0.5;
int kameruindelisTMP=kameruindelis*100;
float ryskumas=0.5;
int ryskumasTMP=ryskumas*100;
float slenkstis=0;
int slenkstisTMP=slenkstis*100;
int poslinkis=0;

    cvNamedWindow("Stereo suma", CV_WINDOW_AUTOSIZE);
    cvMoveWindow("Stereo suma", 1,(10)); //aukstis+1 paslinkim antra langa kad nebutu viens anr kito
       hWnd=FindWindow( NULL,"Stereo suma");
   SetForegroundWindow(hWnd);
            gf.clrscr();
            gf.showRightSide((char *)" Rezoliucija : ",1);printf(" %dx%d.",plotis,aukstis);
            gf.showRightSide((char *)" ");
            printf ("OpenCV  %s (%d.%d.%d)\n", CV_VERSION,CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
            gf.showRightSide((char *)" [m] - uzdaryti  programa");
            gf.showRightSide((char *)" [q]   - Aukstyn");
            gf.showRightSide((char *)" [a]   - Zemyn");
            gf.showRightSide((char *)" [<]   - Kairen");
            gf.showRightSide((char *)" [>]   - Desinin");
 cvCreateTrackbar("A<>B","Stereo suma",& kameruindelisTMP,100,myTrackbarkameruindelis);
 cvCreateTrackbar("Ryskumas","Stereo suma",& ryskumasTMP,100,myTrackbarryskumas);
  cvCreateTrackbar("Slenkstis","Stereo suma",& slenkstisTMP,100,myTrackbarslenkstis);
 cvCreateTrackbar("Atstumas","Stereo suma",&poslinkis,plotis,myTrackbarposlinkis);

while(1){camImg   =  getCameraFrame();
		//if (!camImg) {printf("KLAIDA = ivedimasiscam(kaire): Blogi paveiksleliai!\n");exit(1);}
		camImg1 =  getCameraFrame1(kamera2);
		//if (!camImg1) {printf("Kameros vaizdas dubliuotas, nes kita kamera nenustatyta\n");camImg1 = camImg;}//exit(1);
        // if (poslinkis){}

kameruindelis =kameruindelisTMP/5;
ryskumas =ryskumasTMP;
slenkstis =slenkstisTMP*4;

cvAddWeighted(camImg,  (20-kameruindelis)/20, camImg1,kameruindelis/20, ryskumas ,camImg1);//
if (slenkstis)cvThreshold( camImg1, camImg1,slenkstis,100, 1 );
		//cvShowImage("Kaire", camImg);
		//cvShowImage("Desine", camImg1);
	cvShowImage("Stereo suma", camImg1);
//    on_mouserect(20,20,stereosum));
    	keyPressed = cvWaitKey(1);
		if (keyPressed == ',') {	servo_x=servo_x-10;akys_servo(); }
		if (keyPressed == '.') {	servo_x=servo_x+10;akys_servo(); }
		if (keyPressed == 'q') {	servo_y=servo_y-10;akys_servo(); }
		if (keyPressed == 'a') {	servo_y=servo_y+10;akys_servo(); }
		if (keyPressed == 'm') {	// Check if the user hit the 'Escape' key in the GUI window.
         //   cvDestroyWindow("Kaire"); //CloseWindow(hWnd)
         //   cvDestroyWindow("Desine"); //CloseWindow(hWnd)
            cvDestroyWindow("Stereo suma");
            //cvDestroyAllWindows();
            //cvReleaseImage( &shownImg);
            //cvReleaseImage( &shownImg1 );
            //cvReleaseImage( &stereosum ); //disparity_left_visual
           cvReleaseCapture( &camera );
           cvReleaseCapture( &camera1);
            plotis=320 ;  aukstis=240;
			break;}	// Stop processing input.

    } //while pabaiga
}//stereo kameros kalibravimas pabaiga

// SSC32 ivadai, isvadu aktyvavimas #################################################################################
void ssc32inDemo()
{  uint32_t tmp32bit;
uint32_t sscDavikliai32bit=0;
    plotis=320; aukstis=240; //velniava su rezolkem didziausia normali i 2 kam.... plotis=469; aukstis=352;

            gf.clrscr();
            gf.showRightSide((char *)" Rezoliucija : ",1);printf(" %dx%d.",plotis,aukstis);
            gf.showRightSide((char *)" ");
            printf ("OpenCV  %s (%d.%d.%d)\n", CV_VERSION,CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
            gf.showRightSide((char *)" [m] - uzdaryti 'Roboto' programa");
            gf.showRightSide((char *)" [q]   - Aukstyn");
            gf.showRightSide((char *)" [a]   - Zemyn");
            gf.showRightSide((char *)" [<]   - Kairen");
            gf.showRightSide((char *)" [>]   - Desinin");
            gf.showRightSide((char *)" [s]   - (x , y) servo i centra");
            gf.showRightSide((char *)" [0-7] - SSC32   valdymas");
            gf.showRightSide((char *)" []- ");
            gf.showRightSide((char *)" []- ");


while(1){
		camImg   =  getCameraFrame();
		//if (!camImg) {printf("Nera jokios kameros");Sleep(2000);}//exit(1);
		camImg1 =  getCameraFrame1(kamera2);
		//if (!camImg1) {printf("Kameros vaizdas dubliuotas, nes kita kamera nenustatyta\n");}//exit(1);camImg1 = camImg;

		cvShowImage("Kaire", camImg);
		cvShowImage("Desine", camImg1);

//    on_mouserect(20,20,stereosum));
    		keyPressed = cvWaitKey(1);

		if (keyPressed == ',') {	servo_x=servo_x-10;akys_servo(); }
		if (keyPressed == '.') {	servo_x=servo_x+10;akys_servo(); }
		if (keyPressed == 'q') {	servo_y=servo_y-10;akys_servo(); }
		if (keyPressed == 'a') {	servo_y=servo_y+10;akys_servo(); }
		if (keyPressed == 'm') {	// Check if the user hit the 'Escape' key in the GUI window.

           // cvReleaseCapture( &camera );
           // cvReleaseCapture( &camera1);
			break;}	// Stop processing input.
        if (keyPressed == 's') {  //servo i pradzia
            servo_x=1400; servo_y=1850;
            servo_xk=1400; servo_yk=1850;
            servo_xd=1400; servo_yd=1850;
            servo_xdsen=servo_x; servo_ydsen=servo_y;
            servo_xksen=servo_x; servo_yksen=servo_y;

            servo_greitisX= 700;
            servo_greitisY= 500;
            akys_servo();}

           sscDavikliai32bit= ssc32in();


}//while pabaiga
}//SSC32 ivadai pabaiga
//(eProDas) isvadu aktyvavimas #################################################################################
void eproaktyv ()//int argc, char *argv[]
{   uint8_t tmp8bit;
    plotis=320; aukstis=240; //velniava su rezolkem didziausia normali i 2 kam.... plotis=469; aukstis=352;

            gf.clrscr();
            gf.showRightSide((char *)" Rezoliucija : ",1);printf(" %dx%d.",plotis,aukstis);
            gf.showRightSide((char *)" ");
            printf ("OpenCV  %s (%d.%d.%d)\n", CV_VERSION,CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
            gf.showRightSide((char *)" [m] - uzdaryti 'Roboto' programa");
            gf.showRightSide((char *)" [q]   - Aukstyn");
            gf.showRightSide((char *)" [a]   - Zemyn");
            gf.showRightSide((char *)" [<]   - Kairen");
            gf.showRightSide((char *)" [>]   - Desinin");
            gf.showRightSide((char *)" [s]   - (x , y) servo i centra");
            gf.showRightSide((char *)" [0-7] - eProDas  RB0 - RB7 valdymas");
            gf.showRightSide((char *)" []- ");
            gf.showRightSide((char *)" []- ");


while(1){
		camImg   =  getCameraFrame1(kamera1);
		//if (!camImg) {printf("Nera jokios kameros");Sleep(2000);}//exit(1);
		camImg1 =  getCameraFrame1(kamera2);
		//if (!camImg1) {printf("Kameros vaizdas dubliuotas, nes kita kamera nenustatyta\n");}//exit(1);camImg1 = camImg;

		cvShowImage("Kaire", camImg);
		cvShowImage("Desine", camImg1);

//    on_mouserect(20,20,stereosum));
    		keyPressed = cvWaitKey(1);

		if (keyPressed == ',') {	servo_x=servo_x-10;akys_servo(); }
		if (keyPressed == '.') {	servo_x=servo_x+10;akys_servo(); }
		if (keyPressed == 'q') {	servo_y=servo_y-10;akys_servo(); }
		if (keyPressed == 'a') {	servo_y=servo_y+10;akys_servo(); }
		if (keyPressed == 'm') {	// Check if the user hit the 'Escape' key in the GUI window.

           // cvReleaseCapture( &camera );
           // cvReleaseCapture( &camera1);
			break;}	// Stop processing input.
        if (keyPressed == 's') {  //servo i pradzia
            servo_x=1400; servo_y=1850;
            servo_xk=1400; servo_yk=1850;
            servo_xd=1400; servo_yd=1850;
            servo_xdsen=servo_x; servo_ydsen=servo_y;
            servo_xksen=servo_x; servo_yksen=servo_y;

            servo_greitisX= 700;
            servo_greitisY= 500;
            akys_servo();}
       //int  NewTris =255;
    //StatuseProDas =  eProDas_ConfigurePort(DeviceIdx, eProDas_Index_PORTB, NewTris, Action);    //   eProDas_TogglePin( DeviceIdx, eProDas_Index_PORTB ,Pin);
    if  ( StatuseProDas!= eProDas_Error_SUCCESS ) {gf.clrscr();gf.gotoxy(7,44);printf("Neprijungtas eProdas valdiklis  klaida : %i ",  StatuseProDas); Sleep(2000); break; }
    else {
        if (keyPressed == '0') { Pin=0;  //eProDAS RB0- aktyvuoti// Status =  eProDas_ConfigurePort(DeviceIdx, Port, NewTris, Action);
        if (RB0==0){RB0=1;}
        else{RB0=0;}
        InitialState=RB0;  eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB,Pin, InitialState);
        gf.gotoxy(2,30);   printf("RB0 = %i",RB0);  }

		if (keyPressed == '1') { Pin=1;//eProDAS RB1- aktyvuoti                      // Status =  eProDas_ConfigurePort(DeviceIdx, Port, NewTris, Action);
            if (RB1==0){RB1=1;}
            else{RB1=0;}
        InitialState=RB1; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB,Pin, InitialState);
             gf.gotoxy(2,31);  printf("RB1 = %i",RB1);  }

		if (keyPressed == '2') {  Pin=2; //eProDAS RB2- aktyvuoti                      // Status =  eProDas_ConfigurePort(DeviceIdx, Port, NewTris, Action);
            if (RB2==0){RB2=1;}
            else{RB2=0;}
        InitialState=RB2; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB,Pin, InitialState);
            gf.gotoxy(2,32);printf("RB2 = %i",RB2);	}

		if (keyPressed == '3') {   Pin=3; //eProDAS RB3- aktyvuoti                      // Status =  eProDas_ConfigurePort(DeviceIdx, Port, NewTris, Action);
            if (RB3==0){RB3=1;}
            else{RB3=0;}
        InitialState=RB3; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB,Pin, InitialState);
        gf.gotoxy(2,33);    printf("RB3 = %i",RB3);	}

		if (keyPressed == '4') {   Pin=4;//eProDAS RB4- aktyvuoti                      // Status =  eProDas_ConfigurePort(DeviceIdx, Port, NewTris, Action);
            if (RB4==0){RB4=1;}
            else{RB4=0;}
        InitialState=RB4;  eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB,Pin, InitialState);
		gf.gotoxy(2,34);    printf("RB4 = %i",RB4);}

		if (keyPressed == '5') {    Pin=5; //eProDAS RB5- aktyvuoti                      // Status =  eProDas_ConfigurePort(DeviceIdx, Port, NewTris, Action);
            if (RB5==0){RB5=1;}
            else{RB5=0;}
        InitialState=RB5;  eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB,Pin, InitialState);
            gf.gotoxy(2,35);  printf("RB5 = %i",RB5);	}

		if (keyPressed == '6') {      Pin=6;//eProDAS RB6- aktyvuoti                      // Status =  eProDas_ConfigurePort(DeviceIdx, Port, NewTris, Action);
            if (RB6==0){RB6=1;}
            else{RB6=0;}
        InitialState=RB6; eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB,Pin, InitialState);
            gf.gotoxy(2,36);  printf("RB6 = %i",RB6);	}

		if (keyPressed == '7') {  Pin=7; //eProDAS RB7- aktyvuoti                     // Status =  eProDas_ConfigurePort(DeviceIdx, Port, NewTris, Action);
            if (RB7==0){RB7=1;}
            else{RB7=0;}
        InitialState=RB7; StatuseProDas =  eProDas_ConfigurePinAsOutput( DeviceIdx, eProDas_Index_PORTB,Pin, InitialState);
        gf.gotoxy(2,37);   printf("RB7 = %i",RB7);	}


    // PORTO E input apklausa (porte E naudojama tik 4 bitai, daugiau nera proce)
        eProDas_ReadPort( DeviceIdx,   eProDas_Index_PORTE , eProInputE);
        gf.gotoxy(43,33);printf("Porto %i  reiksme: %i", eProDas_Index_PORTE  ,eProInputE);
    kauke=0b00001000;
    tmp8bit=kauke&eProInputE;
    if (tmp8bit){gf.gotoxy(43,35);printf("1");   gf.gotoxy(40,40); gf.setColor(96);printf("   ");  gf.setColor(128); }
    else{gf.gotoxy(43,35);printf("0");  gf.gotoxy(40,40); gf.setColor(64) ;printf("   ");  gf.setColor(128); }
    kauke=0b00000100;
    tmp8bit=kauke&eProInputE;
    if (tmp8bit){gf.gotoxy(47,35);printf("1");   gf.gotoxy(20,40); gf.setColor(96);printf("   ");  gf.setColor(128); }
    else{gf.gotoxy(47,35);printf("0");  gf.gotoxy(20,40); gf.setColor(64) ;printf("   ");  gf.setColor(128);}
    kauke=0b00000010;
    tmp8bit=kauke&eProInputE;
    if (tmp8bit){gf.gotoxy(51,35);printf("1");   gf.gotoxy(20,50); gf.setColor(96);printf("   ");  gf.setColor(128); }
    else{gf.gotoxy(51,35);printf("0");  gf.gotoxy(20,50); gf.setColor(64) ;printf("   ");  gf.setColor(128); }
    kauke=0b00000001;
    tmp8bit=kauke&eProInputE;
    if (tmp8bit){gf.gotoxy(55,35);printf("1");   gf.gotoxy(40,50); gf.setColor(96);printf("   ");  gf.setColor(128); }
    else{gf.gotoxy(55,35);printf("0");  gf.gotoxy(40,50); gf.setColor(64) ;printf("   ");  gf.setColor(128);}

         gf.gotoxy(29,52); gf.setColor(32) ;printf("PRIEKIS");  gf.setColor(128);

    // PORTO D input apklausa
         eProDas_ReadPort( DeviceIdx,  eProDas_Index_PORTD , eProInputD);
            gf.gotoxy(12,33);printf("Porto %i  reiksme: %i", eProDas_Index_PORTD  ,eProInputD);
    kauke=0b10000000;
    tmp8bit=kauke&eProInputD;
    if (tmp8bit){gf.gotoxy(12,35);printf("1");}
    else{gf.gotoxy(12,35);printf("0");}
    kauke=0b01000000;
    tmp8bit=kauke&eProInputD;
    if (tmp8bit){gf.gotoxy(14,35);printf("1");}
    else{gf.gotoxy(14,35);printf("0");}
    kauke=0b00100000;
        tmp8bit=kauke&eProInputD;
    if (tmp8bit){gf.gotoxy(16,35);printf("1");}
    else{gf.gotoxy(16,35);printf("0");}
    kauke=0b00010000;
        tmp8bit=kauke&eProInputD;
    if (tmp8bit){gf.gotoxy(18,35);printf("1");}
    else{gf.gotoxy(18,35);printf("0");}
    kauke=0b00001000;
        tmp8bit=kauke&eProInputD;
    if (tmp8bit){gf.gotoxy(20,35);printf("1");}
    else{gf.gotoxy(20,35);printf("0");}
    kauke=0b00001000;
        tmp8bit=kauke&eProInputD;
    if (tmp8bit){gf.gotoxy(22,35);printf("1");}
    else{gf.gotoxy(22,35);printf("0");}
    kauke=0b00000010;
        tmp8bit=kauke&eProInputD;
    if (tmp8bit){gf.gotoxy(24,35);printf("1");}
    else{gf.gotoxy(24,35);printf("0");}
    kauke=0b00000001;
        tmp8bit=kauke&eProInputD;
    if (tmp8bit){gf.gotoxy(26,35);printf("1");}
    else{gf.gotoxy(26,35);printf("0");}

     }//else pabaiga

    } //while pabaiga
}//(eProDas) isvadu aktyvavimas pabaiga

//******************************************************************************************************
char *zodzio_reiksme() {
komanda_resume_recog(prisijungimas);
command_loop(prisijungimas,1); //jei 2-as parametras 1 tai loop ciklas client1 progoje praeis 1 karta jei = 0, tai suksis...
    naujasZodis=  do_output(prisijungimas);senasZodis=naujasZodis;
    komanda_pause_recog(prisijungimas);
    gf.gotoxy(4,38);printf(" Atpazintas zodis:   %s ",(naujasZodis)); printf("\n");
return naujasZodis;
}
//###############################################################################################
//******************************************************************************************************
void *KalboaAtpazinimas_demo ()
{static char * demo_zodis;
int rrez=1;
gf.setColor(96); gf.gotoxy(1,1);
printf(" NAUDOJAMI ZODZIAI : \n\n");
gf.setColor(100);
printf(" APLINK    \n APSISUK   \n ATGAL     \n CIA       \n DESINEN   \n EIK       \n GERO      \n I         \n ISIDEMEK  \n JAUTIES   \n");
printf(" JUDEK     \n KA        \n KAS       \n KAIP      \n KAIREN    \n KO        \n MATAI     \n NAMO      \n NORI      \n PAIMK     \n PAZIUREK  \n");
printf(" PIRMYN    \n STOK      \n SURASK    \n SVEIKAS   \n TU        \n VEIKI     \n VIETA     \n VIRSU     \n ZHEMYN    \n");
gf.setColor(72);
gf.gotoxy(1,36);
printf(" NEPAMIRSTI paleisti 'JULIUS' serveri ir inicializuoti clienta !!! \n");
printf("                                                                   \n");
printf("  'DEMO' isjungti garsa su roboto atsakymu spausti 'd' klavisa     \n");
printf("                                                                   \n");
printf("     Iseiti is 'DEMO' spausti 'esc' ir + bet koks garsas           \n"); //printf(" 'JULIUS' kliento Nr.: ");
printf("                                                                   \n\n");
komanda_resume_recog(prisijungimas);
gf.setColor(196);
command_loop(prisijungimas,1); //jei 2-as parametras 1 tai loop ciklas client1 progoje praeis 1 karta jei = 0, tai suksis...
gf.setColor(128);
for(;;)
{
demo_zodis=  do_output(prisijungimas);
        gf.gotoxy(1,46); printf("                                          ");
        gf.gotoxy(1,46);printf(" Atpazintas  zodis : %s ",(demo_zodis));//

if (rrez==1){ //leisti roboto atsakyma
      if (!strcmp(demo_zodis,"APLINK")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  aap`lyiynk",0);
    strcpy(demo_zodis,"+");Sleep(1800);
    komanda_resume_recog(prisijungimas);  }
    else   if (!strcmp(demo_zodis,"APSISUK")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  aap`syi`sook",0);
    strcpy(demo_zodis,"+");
    Sleep(2000);
    komanda_resume_recog(prisijungimas);   }
    else      if (!strcmp(demo_zodis,"ATGAL")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  aat`gaaall",0);
    strcpy(demo_zodis,"+");
    Sleep(1400);
    komanda_resume_recog(prisijungimas);  }
    else     if (!strcmp(demo_zodis,"CHIA")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v en+m4  chiyaa",0);
    strcpy(demo_zodis,"+");
    Sleep(1000);
    komanda_resume_recog(prisijungimas); }
    else      if (!strcmp(demo_zodis,"DESHINEEN")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v en+m4  dea`shyy`neaen",0);
    strcpy(demo_zodis,"+");
    Sleep(2000);
    komanda_resume_recog(prisijungimas);  }
    else     if (!strcmp(demo_zodis,"EIK")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  eiyjk",0);
    strcpy(demo_zodis,"+");
    Sleep(1000);
    komanda_resume_recog(prisijungimas);  }
    else     if (!strcmp(demo_zodis,"GERO")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  gearo",0);
    strcpy(demo_zodis,"+");
    Sleep(1000);
    komanda_resume_recog(prisijungimas); }
    else     if (!strcmp(demo_zodis,"I")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  yy",0);
    strcpy(demo_zodis,"+");
    Sleep(800);
    komanda_resume_recog(prisijungimas); }
    else     if (!strcmp(demo_zodis,"ISIDEMEEK")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  yy`syyy`deea`meek",0);
    strcpy(demo_zodis,"+");
    Sleep(2000);
    komanda_resume_recog(prisijungimas);   }
    else     if (!strcmp(demo_zodis,"JAUTIES")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  yjaauu`tjieesz",0);
    strcpy(demo_zodis,"+");
    Sleep(2000);
    komanda_resume_recog(prisijungimas); }
    else    if (!strcmp(demo_zodis,"JUDEEK")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  juu`deek",0);
    strcpy(demo_zodis,"+");
    Sleep(1800);
    komanda_resume_recog(prisijungimas);  }
    else   if (!strcmp(demo_zodis,"KA")) {
   komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  kaa",0);
    strcpy(demo_zodis,"+");
    Sleep(800);
    komanda_resume_recog(prisijungimas);  }
    else   if (!strcmp(demo_zodis,"KAS")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  kaasz",0);
    strcpy(demo_zodis,"+");
    Sleep(1000);
    komanda_resume_recog(prisijungimas);}
    else   if (!strcmp(demo_zodis,"KAIP")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  kajyp",0);
    strcpy(demo_zodis,"+");
    Sleep(1200);
    komanda_resume_recog(prisijungimas);   }
    else   if (!strcmp(demo_zodis,"KAIREEN")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  kayj`rean",0);
    strcpy(demo_zodis,"+");
    Sleep(2800);
    komanda_resume_recog(prisijungimas);   }
     else   if (!strcmp(demo_zodis,"KO")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  kooo",0);
    strcpy(demo_zodis,"+");
    Sleep(1000);
    komanda_resume_recog(prisijungimas);   }
     else   if (!strcmp(demo_zodis,"MATAI")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  maa`tayi",0);
    strcpy(demo_zodis,"+");
    Sleep(1900);
    komanda_resume_recog(prisijungimas);  }
       else     if (!strcmp(demo_zodis,"NAMO")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  naa`moo",0);
    strcpy(demo_zodis,"+");
    Sleep(1500);
    komanda_resume_recog(prisijungimas);   }
    else      if (!strcmp(demo_zodis,"NORI")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  noo`riy",0);
    strcpy(demo_zodis,"+");
    Sleep(1400);
    komanda_resume_recog(prisijungimas);   }
    else      if (!strcmp(demo_zodis,"PAIMK")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  paa`ymk",0);
    strcpy(demo_zodis,"+");
    Sleep(1500);
    komanda_resume_recog(prisijungimas);   }
    else     if (!strcmp(demo_zodis,"PAZHIUREEK")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v en+m4  paa`zhioo`reeak",0);
    strcpy(demo_zodis,"+");
    Sleep(2200);
    komanda_resume_recog(prisijungimas);  }
    else      if (!strcmp(demo_zodis,"PIRMYN")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  .......pyyr`myyn",0);
    strcpy(demo_zodis,"+");
    Sleep(2000);
    komanda_resume_recog(prisijungimas);  }
    else   if ((!strcmp(demo_zodis,"STOK" ))|| (!strcmp(demo_zodis,"STOP" )) ){
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4 sztooopp",0);
    strcpy(demo_zodis,"+");
    Sleep(1500);
    komanda_resume_recog(prisijungimas);   }
    else   if (!strcmp(demo_zodis,"SVEIKAS")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  la'ba'sz",0);
    strcpy(demo_zodis,"+");
    Sleep(2000);
    komanda_resume_recog(prisijungimas);   }
   else    if (!strcmp(demo_zodis,"SURASK")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  szuu`raazsk",0);
    strcpy(demo_zodis,"+");
    Sleep(1800);
    komanda_resume_recog(prisijungimas);   }
   else    if (!strcmp(demo_zodis,"TU")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  tuu",0);
    strcpy(demo_zodis,"+");
    Sleep(1000);
    komanda_resume_recog(prisijungimas);  }
    else   if (!strcmp(demo_zodis,"VEIKI")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  veiy`kiy",0);
    strcpy(demo_zodis,"+");
    Sleep(1500);
    komanda_resume_recog(prisijungimas);  }
     else  if (!strcmp(demo_zodis,"VIETA")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  vjiee`taaa",0);
    Sleep(1500);
    strcpy(demo_zodis,"+");
    komanda_resume_recog(prisijungimas);   }
    else   if (!strcmp(demo_zodis,"VIRSHU")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v en+m4  vyirr`shoo",0);
    Sleep(1700);
    strcpy(demo_zodis,"+");
    komanda_resume_recog(prisijungimas);  }
    else   if (!strcmp(demo_zodis,"ZHEMYN")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v en+m4  zhea`meeyynn",0);
    Sleep(1700);
    strcpy(demo_zodis,"+");
    komanda_resume_recog(prisijungimas);  }
    else if(!strcmp(demo_zodis,"+")) {Sleep(500); strcpy(demo_zodis,"-");}
}//"taip demo rezimas"  pabaiga

    if (_kbhit())  {  keyPressed=getch();
      if (keyPressed==27) {Sleep(500);demo_zodis=(char*)" " ;do_output(prisijungimas) ;Sleep(500); komanda_pause_recog(prisijungimas);  break;}//jei "esc" iseiti
      if (keyPressed=='d') {     if (rrez==1){rrez=0;}    else {rrez=1;}    }//leisti roboto atsakyma
     }
}//ciklas for pabaiga
}//garsas demo pabaiga
//###########################################################################################################################

 void sum_rgb( IplImage* src, IplImage* dst )
 {
 // Allocate individual image planes.
 IplImage* r = cvCreateImage( cvGetSize(src) , IPL_DEPTH_8U, 1 );
 IplImage* g = cvCreateImage( cvGetSize(src) , IPL_DEPTH_8U, 1 );
 IplImage* b = cvCreateImage( cvGetSize(src) , IPL_DEPTH_8U, 1 );
 // Split image onto the color planes.
 cvSplit( src, r, g, b, NULL );
 // Temporary storage.
 IplImage* s = cvCreateImage( cvGetSize(src) , IPL_DEPTH_8U, 1 );
 // Add equally weighted rgb values.
 cvAddWeighted( r, 1./3., g, 1./3., 0.0, s );
 cvAddWeighted( s, 2./3., b, 1./3., 0.0, s );
 // Truncate values above 100.
 cvThreshold( s, dst, 100, 50, CV_THRESH_TRUNC) ;
 cvReleaseImage( &r ); cvReleaseImage( &g ); cvReleaseImage( &b ); cvReleaseImage( &s );
 }
//###########  Gravitacijos centras  #########################################################################################
CvPoint COG(IplImage* img){
//this function takes an image and calculates it center of gravity. It also only affects the   ROI but this is relative
//to the origin of the original image so it must be shifted
CvMoments moments;
double M00,M10,M01;
CvPoint center;
cvNormalize(img, img, 0, 255, CV_MINMAX , CV_8U  );//   CV_L2
cvMoments(img,&moments,1);
M00 = cvGetSpatialMoment(&moments,0,0);
M10 = cvGetSpatialMoment(&moments,1,0);
M01 = cvGetSpatialMoment(&moments,0,1);
center.x = (int)(M10/M00);center.y = (int)(M01/M00);
return center;
}

//###########  Gravitacijos centras  #########################################################################################
CvPoint AntiCOG(IplImage* img){
//this function takes an image and calculates it center of gravity. It also only affects the   ROI but this is relative
//to the origin of the original image so it must be shifted
CvMoments moments;
double M00,M10,M01;
CvPoint center;
cvNot(img,img);
cvNormalize(img, img, 0, 255, CV_MINMAX , CV_8U  );//   CV_L2
cvMoments(img,&moments,1);
M00 = cvGetSpatialMoment(&moments,0,0);
M10 = cvGetSpatialMoment(&moments,1,0);
M01 = cvGetSpatialMoment(&moments,0,1);
center.x = (int)(M10/M00);center.y = (int)(-M01/M00);
//cvNot(img,img);
return center;
}
//###### Pradzia ############################################################################################################
int  Pradzia ()
{ if (prisijungimas==1)   {return -1;}
static char * demo_zodis; int rrez=1;
gf.setColor(96); gf.gotoxy(1,52);printf(" Pasisveikink su robotu: \n");
printf(" Pasakyk zodi: 'SVEIKAS' tada pasikurs !!!");
gf.setColor(72);gf.gotoxy(1,54);
printf(" Paleistas kalbos atpazibimo serveris !!! \n");
printf("         Pakalbek su draugeliu        !!! \n");
printf(" Iseiti spausti 'esc' ir + bet koks garsas\n");
komanda_resume_recog(prisijungimas);
command_loop(prisijungimas,1); //jei 2-as parametras 1 tai loop ciklas client1 progoje praeis 1 karta jei = 0, tai suksis...
gf.setColor(128);
for(;;)
{    demo_zodis=  do_output(prisijungimas);
    gf.gotoxy(1,58); printf("                                          ");
    gf.gotoxy(1,58);printf(" Atpazintas  zodis : %s ",(demo_zodis));//
 if (!strcmp(demo_zodis,"SVEIKAS")||keyPressed==27) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  la'ba'sz",0);
    strcpy(demo_zodis,"+");
    Sleep(2000);
    komanda_resume_recog(prisijungimas);
    Sleep(500);demo_zodis=(char*)" " ;do_output(prisijungimas) ;
    Sleep(500); komanda_pause_recog(prisijungimas);  break;    }
    else if(!strcmp(demo_zodis,"+")) {Sleep(500); strcpy(demo_zodis,"-");}
  if (_kbhit())  {keyPressed=getch(); }
}//ciklas for pabaiga
return 1;
}//Pradzia  pab

//###### Klaidziojimas -algoritmas ############################################################################################################
int  Klaidziojimas()
{
StereoBM();
while(1)
{//kazkas gal bus ...
break;
}//while pab

}

//###### Dialogas Robio Rezimas ############################################################################################################

int  DialogasRobRezimas()
{if (prisijungimas==1)   {return 0;}
//time_t niekonevyksta = clock();
Sleep(1000);
WinExec("eSpeak.exe -v lv+m4 kaaa......daa`ryi`tiii",0);
gf.clrscr();
static char * demo_zodis; int rrez=1;
gf.setColor(96); gf.gotoxy(1,10);printf("Isrink roboto darbo rezima:\n");
 printf("'SURASK' - iesko daikto kuri reikia parodyti \n");
 printf("'KAS'    - iesko pazistamu veidu             \n");
 printf("'PIRMYN' - stereo navigacija (klaidziojimas) \n");
 //printf("'ISIDEMEK'-iesko pazistamu \n");
gf.setColor(72);gf.gotoxy(1,54);
printf(" Paleistas kalbos atpazibimo serveris !!!          \n");
printf("            Ka robotui daryti ???     !!!          \n");
printf(" ISEITI i meniu: spausti 'm' ir + bet koks garsas\n");
printf("            arba  pasakyti dar karta zodi  'SVEIKAS'");
Sleep(2000);
komanda_resume_recog(prisijungimas);
command_loop(prisijungimas,1); //jei 2-as parametras 1 tai loop ciklas client1 progoje praeis 1 karta jei = 0, tai suksis...
gf.setColor(128);
for(;;)
{    demo_zodis=  do_output(prisijungimas);
    gf.gotoxy(1,58); printf("                                          ");
    gf.gotoxy(1,58);printf(" Atpazintas  zodis : %s ",(demo_zodis));//
 if (!strcmp(demo_zodis,"SVEIKAS")||keyPressed=='m') {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4 naa...daarr...la'ba'sz",0);
    strcpy(demo_zodis,"+");
    Sleep(3000);
    komanda_resume_recog(prisijungimas);
    Sleep(500);demo_zodis=(char*)" " ;do_output(prisijungimas) ;
    Sleep(500); komanda_pause_recog(prisijungimas);  break; }
 else    if (!strcmp(demo_zodis,"SURASK")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v de+m4  ijeaschkauu",0);
    strcpy(demo_zodis,"+");
    Sleep(3000);
    komanda_resume_recog(prisijungimas);
    Sleep(500);demo_zodis=(char*)" " ;do_output(prisijungimas) ;
    Sleep(500); komanda_pause_recog(prisijungimas);  return 2;  }
    else   if (!strcmp(demo_zodis,"KAS")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  kaasz",0);
    strcpy(demo_zodis,"+");
    Sleep(1000);
    komanda_resume_recog(prisijungimas);
    Sleep(500);demo_zodis=(char*)" " ;do_output(prisijungimas) ;
    Sleep(500); komanda_pause_recog(prisijungimas);  return 1;  }
    else   if (!strcmp(demo_zodis,"PIRMYN")) {
    komanda_pause_recog(prisijungimas);
    WinExec("eSpeak.exe -v lv+m4  eyjnuu",0);
    strcpy(demo_zodis,"+");
    Sleep(1400);
    komanda_resume_recog(prisijungimas);
    Sleep(500);demo_zodis=(char*)" " ;do_output(prisijungimas) ;
    Sleep(500); komanda_pause_recog(prisijungimas);  return 3;  }

  // else if(!strcmp(demo_zodis,"+")) {Sleep(500); strcpy(demo_zodis,"-");}
  if (_kbhit())  {keyPressed=getch(); }
}//ciklas for pabaiga
return 0;

}
//###### Pazistami Veidai ############################################################################################################
int  PazistamiVeidai()
{VeidoAtpazinimas();
while(1)
{//kazkas gal bus ...
break;
}//while pab

}


//###### Pazistami Daiktai ############################################################################################################
int PazistamiDaiktai()
{
//parasyti daiktu  paieska DB

//daiktas(1,((char**)"Daiktai/1_Minskas.png"));// daiktas is failo

daiktas(0,NULL);//tokia sitakse daiktas realus fiks realiam rezime
}
//###### Mechanika ############################################################################################################
void *Mechanika(void *)
{
  akys_servo();
 ssc32in ();   //daviklisSSC=SSC32 davikliu apklausa
vazdavikliai();//daviklisePD=vaziuokles davikliu apklausa

if (daviklisePD==0){ (daviklisePD=1);}//pakol-kas mirksiukas, kokiu dazniu apklausa...
else {daviklisePD=0;}

 gf.gotoxy(4,58); printf("Daviklis eProDas= %i",daviklisePD);
 gf.gotoxy(4,59);printf("Daviklis SSC32  = %i",daviklisSSC);
     //vazDR=0; //vaziuokles darbo rezimo kintamasis
 vaziuokle (vazDR,1); // pagal esamus x y duomenis paleisti ratus

}
//###### Daikto isskyrimas ############################################################################################################

IplImage* daikto_isskyrimas(void)
{plotis=640 ;  aukstis=480;

IplImage* scenos_foto1= cvCreateImage( cvSize(plotis,aukstis), 8, 3);
IplImage* scenos_foto2= cvCreateImage( cvSize(plotis,aukstis), 8, 3);
IplImage* tmpimage1= cvCreateImage( cvSize(plotis,aukstis), 8, 3);
IplImage* greyImg= cvCreateImage( cvSize(plotis,aukstis), 8, 1);
IplImage* tmpimage= cvCreateImage( cvSize(plotis,aukstis), 8, 1);;
    cvNamedWindow("Daikto isskirimas", CV_WINDOW_AUTOSIZE);
    cvMoveWindow("Daikto isskirimas", 1,aukstis/2+5); //aukstis+1 paslinkim antra langa kad nebutu viens anr kito
       hWnd=FindWindow( NULL,"Daikto isskirimas");
        SetForegroundWindow(hWnd);

            gf.clrscr();
            gf.showRightSide((char *)" Rezoliucija : ",1);printf(" %dx%d.",plotis,aukstis);
            gf.showRightSide((char *)" ");
            printf ("OpenCV  %s (%d.%d.%d)\n", CV_VERSION,CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
            gf.showRightSide((char *)" [m] - uzdaryti  programa");
            gf.showRightSide((char *)" [q]   - Aukstyn");
            gf.showRightSide((char *)" [a]   - Zemyn");
            gf.showRightSide((char *)" [<]   - Kairen");
            gf.showRightSide((char *)" [>]   - Desinin");
            gf.showRightSide((char *)"");
            gf.showRightSide((char *)" [f]   - fono uzfiksavimas");
            gf.showRightSide((char *)" [d]   - daikto isskirimas");
            gf.showRightSide((char *)" [ ]    ");
time_t Taimeris=clock();
while(1){
camImg1 =  getCameraFrame1(kamera2);
	//if (!camImg1) {printf("Nenuskaitytas vaizdas, gal nera kameros \n");exit(1);}//
        if ((prisijungimas>1)){
         if (((clock()-Taimeris)>4000)&&((clock()-Taimeris)<4500)){//leisti kamerom nusistoveti ryskumui
         keyPressed = 'f';
         //for (i = 0; i < 5; i++) {camImg1 =  getCameraFrame1(kamera2);cvCopyImage(camImg1,scenos_foto1);  Sleep(100);  }
            WinExec("eSpeak.exe -v lv+m4  paroodyik.....daaijktaa",0);
            Sleep(3000);}//fono foto
         if ((clock()-Taimeris)>10000){ keyPressed = 'd' ;}
        }

	if (keyPressed == ',') {	servo_x=servo_x-10;akys_servo(); }
	if (keyPressed == '.') {	servo_x=servo_x+10;akys_servo(); }
	if (keyPressed == 'q') {	servo_y=servo_y-10;akys_servo(); }
	if (keyPressed == 'a') {	servo_y=servo_y+10;akys_servo(); }

     if (keyPressed == 'f'){ gf.gotoxy(1,38);  printf("                                  ");
      for (i = 0; i < 5; i++) {cvCopyImage(camImg1,scenos_foto1);
     cvShowImage("Daikto isskirimas", scenos_foto1);   Sleep(500);  }
       gf.gotoxy(1,38); printf("Uzfiksuotas fono paveikslelis !!!"); WinExec("eSpeak.exe -v lv+m4  paroodyik.....daaijktaa",0);}

     if (keyPressed == 'd')//Skirtumo paieska ir regiono isskirimas
        {  int kartoti=1;  gf.gotoxy(1,39);  printf("                                 ");
        while(kartoti){

             if (scenos_foto1){camImg1 =  getCameraFrame1(kamera2);
        CvRect regionas;
        cvCopyImage(camImg1,scenos_foto2);
        CvBGStatModel* bg_model = 0;
        bg_model = cvCreateFGDStatModel( scenos_foto1);  // bg_model = cvCreateGaussianBGModel(scenos_foto1); //gausianas blogiau
        //Skirtumas su fonu
        cvUpdateBGStatModel( scenos_foto2, bg_model, -1);//-1
        cvCopyImage(bg_model->foreground,greyImg);
        //^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
        CBlobResult blobs;//,blobs1
        CBlob blob;
        CBlob blobWithBiggestPerimeter, blobWithLessArea;
        CvPoint p1, p2;
        CvPoint blobumaximumasP10;
        CvPoint blobumaximumasP20;
        int bloboplotisX,bloboplotisY;
         //MAX regiono paieska
         //cvDilate( greyImg,greyImg,0,1 );
         // cvErode( greyImg,greyImg,0,1);

        blobs=CBlobResult( greyImg,NULL,100,true );//nereikia slenkscio nes balta juoda
       //blobs.Filter( blobs, B_EXCLUDE,  CBlobGetArea(), B_LESS, 100 ); //mazus blobus <100 pixeliu nufiltruot
        blobs.Filter( blobs, B_INCLUDE , CBlobGetArea(), B_INSIDE, 50, 100000); //50,50000  B_EXCLUDE B_OUTSIDE

        blobs.GetNthBlob (CBlobGetArea (), blobs.GetNumBlobs () - 1, blobWithLessArea);
        blobWithLessArea.FillBlob (greyImg, CV_RGB (0, 0, 0));
        blobWithBiggestPerimeter.FillBlob(greyImg, CV_RGB (255,255,255));

        for (i = 0; i < blobs.GetNumBlobs (); i++) {
        blob = blobs.GetBlob (i);  p1.x = (int) blob.MinX (); p1.y = (int) blob.MinY ();
        p2.x = (int) blob.MaxX (); p2.y = (int) blob.MaxY ();
        if (bloboplotisX<(p2.x-p1.x)){bloboplotisX=(p2.x-p1.x);bloboplotisY=(p2.y-p1.y);blobumaximumasP10.x=p1.x;blobumaximumasP20.x=p2.x;blobumaximumasP10.y=p1.y;blobumaximumasP20.y=p2.y; }
        }

        for (i = 1; i < 30; i++) {cvDilate( greyImg,greyImg,0,i ); cvErode( greyImg,greyImg,0,i);}//gal reikejo tik viena karta su 30, taip lyg geriau, glotniau?????

        //^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
        if (((blobumaximumasP10.x>0)&&(blobumaximumasP10.y>0)&&(bloboplotisX>50)&&(bloboplotisY >40))&&
        ((blobumaximumasP10.x<plotis-20)&&(blobumaximumasP10.y<aukstis)&&(bloboplotisX<plotis-200)&&(bloboplotisY <aukstis)))
        {regionas=cvRect((blobumaximumasP10.x),(blobumaximumasP10.y),(bloboplotisX),(bloboplotisY));
        tmpimage= cvCreateImage( cvSize(bloboplotisX,bloboplotisY), 8, 1);
        gf.gotoxy(1,42);
        printf("Regionas: \n");printf("blobo.x =  %i \n",blobumaximumasP10.x);
        printf("blobo.y =  %i \n",blobumaximumasP10.y);printf("Bplotis =  %i \n",bloboplotisX);
        printf("Baukstis= %i \n",bloboplotisY); kartoti=0;}
        else{regionas=cvRect(plotis/8,aukstis/6,plotis-plotis/4,aukstis-aukstis/3);
        tmpimage= cvCreateImage( cvSize(plotis-plotis/4,aukstis-aukstis/3), 8, 1);  kartoti=1;
        }

        cvConvertImage(greyImg,tmpimage1);//bg_model->foreground
        cvNot(tmpimage1,tmpimage1);
        //Daikto vaizdo isskyrimas is fono
        cvSub(scenos_foto2,tmpimage1,tmpimage1,greyImg);//cvAbsDiffbg_model->foreground
        cvNormalize(tmpimage1, tmpimage1, 0, 255, CV_MINMAX  );
        greyImg = convertImageToGreyscale(tmpimage1);

            cvSetImageROI(greyImg, regionas);
           // cvResize(greyImg, tmpimage);//, CV_INTER_AREA

            cvNormalize(greyImg, greyImg , 0, 255, CV_MINMAX , CV_8U  );
           cvCopyImage(greyImg,tmpimage);
             cvReleaseBGStatModel(&bg_model);
            bloboplotisX=0,bloboplotisY=0;

        if ((prisijungimas>1)){keyPressed ='m'; }
          cvResetImageROI(greyImg);
           //cvReleaseImage(&bg_model->foreground);
           // cvReleaseImage(&bg_model->background);
       cvShowImage("Daikto isskirimas", tmpimage);
    }
     else {gf.gotoxy(1,38);printf("Nera fono paveikslelio !!!");}
    }//while kartoti pakol bus normalus pav
     gf.gotoxy(1,39);  printf("Uzfiksuotas daikto vaizdas   !!!");
       WinExec("eSpeak.exe -v lv+m4  jisyideameeijaau",0);
    }// (keyPressed d)  pabaiga

    	if (keyPressed == 'm') {

            cvResetImageROI(greyImg);
            cvReleaseImage(&scenos_foto1);
            cvReleaseImage(&scenos_foto2);
            cvReleaseImage(&tmpimage1);
            cvReleaseImage(&greyImg);

            cvDestroyWindow("Daikto isskirimas");
           cvReleaseCapture( &camera1);
			break;}	// Stop

  keyPressed = cvWaitKey(10);


}//while pabaiga

 return tmpimage;
}////###### Daikto isskyrimas pabaiga
