/***************************************************************************************
 *
 * detect.h is part of SimpleSkyCam.
 *
 *****************************************************************************************/


#ifndef _DETECT_H_
#define _DETECT_H_

#ifdef BUILD_WINDOWS
#define WIN32_LEAN_AND_MEAN

#define _NTDDI_VERSION_FROM_WIN32_WINNT2(ver)    ver##0000
#define _NTDDI_VERSION_FROM_WIN32_WINNT(ver)     _NTDDI_VERSION_FROM_WIN32_WINNT2(ver)

#ifndef _WIN32_WINNT
#  define _WIN32_WINNT 0x501
#endif
#ifndef NTDDI_VERSION
#  define NTDDI_VERSION _NTDDI_VERSION_FROM_WIN32_WINNT(_WIN32_WINNT)
#endif

#include <winsock2.h>
#include <io.h>
#include <ws2tcpip.h>
#include <windows.h>
#endif

#include "pid.h"
#include "gui.h"
#include "config.h"
#include "video.h"
#include "videoframe.h"

#define DET_MAXSHIFT 20

enum {
	AUTODETECT_OFF = 0,
	AUTODETECT_BRIGHT
};

enum {
	AUTOFOLLOW_OFF = 0,
	AUTOFOLLOW_BRIGHT,
	AUTOFOLLOW_CROSSC
};

enum {
	POSCTL_MODE_OFF = 0,
	POSCTL_MODE_CALIB,
	POSCTL_MODE_CONTROL,
	POSCTL_MODE_2STEPWAIT,
	POSCTL_MODE_2STEPRUN
};

enum {
	POSCTL_CALIB_MODE_OFF = 0,
	POSCTL_CALIB_MODE_START,
	POSCTL_CALIB_MODE_STOP1,
	POSCTL_CALIB_MODE_DELTAM,
	POSCTL_CALIB_MODE_AXIS1START,
	POSCTL_CALIB_MODE_AXIS1M,
	POSCTL_CALIB_MODE_AXIS2START,
	POSCTL_CALIB_MODE_AXIS2M,
	POSCTL_CALIB_MODE_FINISH
};

enum {
	POSCTL_DEVTYPE_TTY = 0,
	POSCTL_DEVTYPE_SIM
};


struct PosCtl_2Step_Data {
	int mode;
	double pv[2];
	double op[2];
	double npv[2];
	double ppv[2];
	double nop[2];
	double pop[2];
};

struct PosCtl_ThreadData {
	position_2d c[2];
	position_2d a1[2];
	position_2d a2[2];
	position_2d target_pos;
	int calib_mode;
};


class PosCtl {
	private:
		GMutex mutex;
		int mode;
		PID pid_axis[2];
		std::string device;
		int device_type;
#ifdef BUILD_WINDOWS
                HANDLE device_fd;
#else
		int device_fd;
#endif
		struct timeval calib_timestamp;
		int calib_mode;
		position_2d calib_pos;
		position_f_2d pos;
		double npv[2];
		double ppv[2];
		double nop[2];
		double pop[2];
		double posfilter;

		struct PosCtl_ThreadData threaddata;	// will be copied to gtk thread
		void NotifyGtk();
		void NotifyGtk2Step();

		void CalibModeStart(int x, int y);
		void CalibModeDelta(int x, int y);
		void CalibModeAxis(int x, int y);
		void CalibModeFinish();
		void CalibModeWait(int x, int y);

		int OutputClose();
		int OutputOpen();
		int WriteTTY(char *);
		int ReadTTY(char *, int);

		static double filteredvalue(double old_value, double new_value, double dt, double filter);

	public:
		PosCtl();
		~PosCtl() {};

		position_f_2d	calib_rot_v;
		vector_2d calib_rot;
		position_f_2d	calib_axis1_v;
		vector_2d calib_axis1;
		position_f_2d	calib_axis2_v;
		vector_2d calib_axis2;
		position_f_2d target_pos;
		position_f_2d current_pos;
		double axis_pv[2];
		double axis_op[2];

		void LockMutex() { g_mutex_lock(&mutex); };
		void UnLockMutex() { g_mutex_unlock(&mutex); };

		void Stop ();
		void StartCalibration ();
		void StartControl ();
		void Run2Step ();
		void Reset2Step ();
		int GetMode () { return mode; };
		void SetAxisParam (int axis, double min, double max, double k, double i, double d);
		void GetAxisParam (int axis, double *min, double *max, double *k, double *i, double *d);
		void SetDevice(std::string d);
		void SetFilter(double f) { posfilter = f; };
		double GetFilter() { return posfilter; };
		std::string GetDevice() { return device; };

		void Loop (int posx, int posy, double dt);
		int OutputWriteValue (int axis, double value);
		int OutputWriteStop ();
		int OutputWriteStart ();
};


struct  {
	VideoFrame *image;	// detected image
	uint32_t *detmatrix;
	int posx;			// position of the detected object
	int posy;			// position of the detected object
	int valid;			// valid position detected
} typedef DetectOutput;


class Detect {
private:
	int running;
	VideoFrame inFrame;			// input frame
	VideoFrame oldFrame;		// oldinput frame
	int inFrameNew;				// new input frame;
	VideoFrame image;			// output image
	VideoFrame imagegtk;		// output image -- send to gtk
	GMutex muteximage;
	GMutex mutexin;
	GMutex mutex;				// general mutex for changing settings
	GThread *thread;

	float *maxx;
	float *maxy;
	int posmaxx;
	int posmaxy;
	uint32_t *detmatrix;

	int objectX;				// current object position
	int objectY;				// current object position
	int objectW;				// object Image Size Width
	int objectH;				// object Image Size Height
	int minBright;				// min brightness

	int autodetecttype;			// input detection type to use
	int autofollowtype;			// input detection type to use

	void InputDetect (int *posx, int *posy);
	void InputDetectCrossC (int *posx, int *posy);
	void SetInputSize (int nw, int nh);
	void CopyObjectImage(int objx, int objy);

public:
	Detect();
	~Detect();

	int NewFrame(VideoFrame *newframe);

	//
	// Thread Releated Functions (maybe soon private?)
	int TryLockInMutex() { return g_mutex_trylock(&mutexin); };
	void LockInMutex() { g_mutex_lock(&mutexin);};
	void UnLockInMutex() { g_mutex_unlock(&mutexin);};

	int TryLockImageMutex() { return g_mutex_trylock(&muteximage); };
	void LockImageMutex() { g_mutex_lock(&muteximage);};
	void UnLockImageMutex() { g_mutex_unlock(&muteximage);};

	void LockMutex() { g_mutex_lock(&mutex);};
	void UnLockMutex() { g_mutex_unlock(&mutex);};

	//
	// Object functions
	void SetObjectSize (int neww, int newh);
	void GetObjectSize (int *ow, int *oh);

	void SetMinBrightness (int value);
	int GetMinBrightness();

	void SetFollowType(int intype);
	int GetFollowType();

	void SetDetectType(int intype);
	int GetDetectType();

	void SetObjectPos (int x, int y);
	void GetObjectPos (int *x, int *y);

	void Thread();
};

extern struct PosCtl_ThreadData posctl_threaddata;

#endif