#include "udpl_dll_imp.h"
#include <tchar.h>

// *****************************************************************************************
// コンストラクタ
// *****************************************************************************************
UdplSocketIf::UdplSocketIf(SOCKET s, const char *address, uint16_t ms, uint32_t bsize, uint8_t hb, uint8_t bl) {

	// メンバ変数セット
	sock = s;
	bufsize = bsize;		// ボードのバッファサイズ
	max_packet_size = (uint32_t)(bufsize * 0.8f);	// とりあえず最大送信サイズ

	cycle_time = std::chrono::milliseconds(hb);			// ハートビートサイクル
	max_wait_time = std::chrono::milliseconds(bl);		// 待ち時間 
	blank_time = bl;

	setWait(ms);

	cur_time = std::chrono::steady_clock::now();
	pre_time = cur_time;

	memset(addressIpv6, '\0', sizeof(addressIpv6));
	strcpy_s(addressIpv6, sizeof(addressIpv6), address);

	// 送信先アドレス用意
	memset(&dest_addr, 0, sizeof(dest_addr));
	dest_addr.sin6_family = AF_INET6;
	dest_addr.sin6_port = htons(udpport);
	inet_pton(dest_addr.sin6_family, address, &dest_addr.sin6_addr);	// 送信先セット

	int size = bsize;
	setsockopt(sock, SOL_SOCKET, SO_SNDBUF, (char*)&size, sizeof(size));	// socket の送信バッファサイズをボードのバッファサイズと一緒にする
	
	// バッファ確保
	rbuf = new RINGBUF<uint8_t>((bufsize << 2) * ( (wait.load(std::memory_order_acquire)/10 == 0) ? 1 : (wait.load(std::memory_order_acquire)/10)));	// リングバッファ 実際には 2^n サイズに繰り上げされる
	sendbuf = new uint8_t[max_packet_size << FILLEDBUF_SHIFT];		// 大きめのサイズ

	// ラインバッファ
	filledbuf = new uint8_t[max_packet_size << FILLEDBUF_SHIFT];	// packetsize 未満用バッファ
	filled_size = 0;
	filled_top_timing = cur_time;					//

	filled_cycle_break = false;

	timing_list.clear();

	pktthreadOn.store(true, std::memory_order_release);		// send_th は動作する
	bufthreadOn.store(true, std::memory_order_release);

	sendStat.store(false, std::memory_order_release);	// 送信状態 = 送信してない
	heartBeatEn.store(true, std::memory_order_release);	// ハートビート on -> socket を常に活性化させて遅延を起こさせない

	send_pkt_th = std::thread(&UdplSocketIf::sendPktData, this);	// スレッド生成
	fill_buf_th = std::thread(&UdplSocketIf::fillLineBuf, this);	// スレッド生成

	// ハードウェアリセットは chip のコンストラクタでやらせる
}

// *****************************************************************************************
// デストラクタ
// *****************************************************************************************
UdplSocketIf::~UdplSocketIf() {

	heartBeatEn.store(false, std::memory_order_release);	// ハートビート OFF
	waitBufClear();		// ボードのバッファクリアを待つ

	// スレッドを終了させる
	pktthreadOn.store(false, std::memory_order_release);	// send_th 停止
	bufthreadOn.store(false, std::memory_order_release);

	uint8_t enddata = 0x00;	// パケットを送信してスレッドを動かす
	pushData(&enddata, 1);

	if (send_pkt_th.joinable()) {
		send_pkt_th.join();		// スレッド終了待ち
	}
	if (fill_buf_th.joinable()) {
		fill_buf_th.join();		// スレッド終了待ち
	}

	delete rbuf;
	rbuf = nullptr;

	delete[] sendbuf;
	sendbuf = nullptr;

	delete[] filledbuf;
	filledbuf = nullptr;
}

// *****************************************************************************************
// socket を専有して、すべての slot を reset する
// *****************************************************************************************
bool UdplSocketIf::reset(void)
{
	bool ret;
	uint8_t bufs[16];
	uint8_t *p = bufs;
	uint8_t bufr[16];

	memcpy(p, COMMAND_RESET, sizeof(COMMAND_RESET));
	p += sizeof(COMMAND_RESET);
	*p = 0xff;

	ret = pushRecv(bufs, COMMAND_RESET[0] + 1, bufr, sizeof(bufr));
	if (ret) {
		if (memcmp(bufr, RESPONSE_RESET, sizeof(RESPONSE_RESET)) == 0) {
			ret = true;
		}
		else {
			ret = false;
		}
	}

	return ret;
}

// *****************************************************************************************
// バッファに書き込んで、recv を待つ
// select 待機中にこのコマンドが複数回実行されるとどうなるのか調べてない->mutexする？
// pushData したデータに対する response が取得できる保証はない -> レスポンスを工夫するとか、
// 送信と返信を ID 付けるとか根本的に変えないといけない
// *****************************************************************************************
uint32_t UdplSocketIf::pushRecv(const uint8_t* buf, uint32_t size, uint8_t* res, uint32_t res_size, uint32_t sec)
{
	bool back_heartbeat = heartBeatEn.exchange(false, std::memory_order_release);
	uint32_t ret = 0;

	fd_set sets;
	int num = 0;
	int n = 0;

	FD_ZERO(&sets);
	FD_SET(sock, &sets);
	struct timeval time = { (long)sec, 500000 + ((long)wait.load(std::memory_order_acquire) * 1000) };	// 500+wait[ms] + sec[s]

	pushData(buf, size);

	n = select(0, &sets, NULL, NULL, &time);
	if (n != 0) {	// タイム・アウトしなかった
		{
			std::lock_guard<std::mutex> lock(sock_m);
			num = recvfrom(sock, (char*)res, res_size, 0, nullptr, nullptr);
		}
		if (num != SOCKET_ERROR) {
			ret = num;
		}
	} 
	// dbg
	else if (n == 0) {
		MessageBox(NULL, _T("timeout"), _T("push recv"), MB_OK);
	} else if (n < 0) {
		MessageBox(NULL, _T("error"), _T("push recv"), MB_OK);
	}
	
	heartBeatEn.store(back_heartbeat, std::memory_order_release);

	return ret;
}

// ******************************************************************************************************
// リングバッファにコマンドを書き込む
// !! TBD. コマンドが絶え間なく来て、ラインバッファが溢れてしまう状態を回避する実装が必要
// ******************************************************************************************************
void UdplSocketIf::pushData(const uint8_t *buf, uint32_t size)
{
	pre_time = cur_time;
	cur_time = std::chrono::steady_clock::now();	// 現在時間

	{
		std::lock_guard<std::mutex> lock(filled_buf_m);

		if (filled_size == 0) {	// ラインバッファの先頭。時刻を取得する

			// 送信時間
			filled_top_timing = cur_time + std::chrono::milliseconds(wait.load(std::memory_order_acquire));

			// コマンドをバッファにコピー
			memcpy(filledbuf, buf, size);
			filled_size = size;

			filled_top_c.notify_one();	// ハートビートループを抜ける
		}
		else {
			// コマンドの空き時間に応じてウエイトを入れる
			auto time = (std::chrono::duration_cast<std::chrono::microseconds>(cur_time - pre_time)).count();	// micro sec
			
			uint32_t micro = (uint32_t)time % 1000U;
			uint32_t milli = (uint32_t)time / 1000U;
				
			uint8_t *wp = filledbuf + filled_size;

			if (milli != 0) {
				memcpy(wp, COMMAND_MS_WAIT, sizeof(COMMAND_MS_WAIT));
				wp += sizeof(COMMAND_MS_WAIT);
				*wp++ = milli & 0xff;
				*wp++ = (milli >> 8) & 0xff;
				filled_size += COMMAND_MS_WAIT[0] + 1;

				if (micro != 0) {
					memcpy(wp, COMMAND_US_WAIT, sizeof(COMMAND_US_WAIT));
					wp += sizeof(COMMAND_US_WAIT);
					*wp++ = micro & 0xff;
					*wp++ = (micro >> 8) & 0xff;
					filled_size += COMMAND_US_WAIT[0] + 1;
				}

			} else if (micro > IGNORE_DELAY) {	// IGNORE_DELAY[us] 以上間が空くようだったら wait を突っ込む。
				memcpy(wp, COMMAND_US_WAIT, sizeof(COMMAND_US_WAIT));
				wp += sizeof(COMMAND_US_WAIT);
				*wp++ = micro & 0xff;
				*wp++ = (micro >> 8) & 0xff;
				filled_size += COMMAND_US_WAIT[0] + 1;
			}

			memcpy(wp, buf, size);	// コマンドをラインバッファにコピー
			filled_size += size;

			filled_cycle_break = true;
			filled_cycle_c.notify_one();
		}
	}
}


// *******************************************************************************************************
// 別スレッドで動作し、pushdata で追加されるデータ量と経過時間を監視する 
// *******************************************************************************************************
void UdplSocketIf::fillLineBuf(void)
{
	bool cret;		// condition variable の返り値
	TIMING_ELEM timing_local;	// timing情報
	uint8_t np = 0x00;	// nop command

	do {

		// なにもコマンドを受けない間はハートビートを送信する
		{
			std::unique_lock<std::mutex> lock(filled_buf_m);

			cret = filled_top_c.wait_for(lock, cycle_time, [this] {return (filled_size != 0); });	// ラインバッファトップ書き込みを待つ

			if (!cret) {	// タイムアウト	: cycle time の間何も受信がない

				if (heartBeatEn.load(std::memory_order_acquire)) {	// ハートビート指定 -> nop 送信

					timing_local.len = 1;		// 送信長さ
					timing_local.timing = std::chrono::steady_clock::now() + std::chrono::milliseconds(wait.load(std::memory_order_acquire));
					{
						std::lock_guard<std::mutex> lock(timing_m);
						{
							std::lock_guard<std::mutex> lock(rbuf_m);
							rbuf->push(np);		// リングバッファへ
						}
						timing_list.push_back(timing_local);	// リストに突っ込む
					}
					list_wait_c.notify_one();	// sendPktData スレッドを動かす
				}

				continue;	// ラインバッファトップの書き込みを再度待つ
			}
		}


		// トップは受信した -> max_wait_time の空き時間を待つ
		do {
			{
				std::unique_lock<std::mutex> lock(filled_buf_m);

				cret = filled_cycle_c.wait_for(lock, max_wait_time, [this] {return filled_cycle_break; });	// タイミングを待つ

				filled_cycle_break = false;

				if (cret) {	// 書き込まれた
					
					continue;

				} else {	// タイムアウト : max_wait_time の間何も受信がない

					timing_local.len = filled_size;				// 送信長さ
					timing_local.timing = filled_top_timing;	// 送信タイミング
					{
						std::lock_guard<std::mutex> lock(timing_m);
						{
							std::lock_guard<std::mutex> lock(rbuf_m);
							rbuf->push_buf(filledbuf, filled_size);		// ラインバッファからリングバッファへ移動
						}
						timing_list.push_back(timing_local);	// リストに突っ込む
					}
					list_wait_c.notify_one();	// sendPktData スレッドを動かす
					filled_size = 0;							// ラインバッファクリア
				}
			}
			break;

		} while (true);

	} while (bufthreadOn.load(std::memory_order_acquire));
}

// *******************************************************************************************************
// 別スレッドで動作し、list から送信タイミングと送信長を読み、リングバッファから送信データを読んで socket でデータ送信する 
// *******************************************************************************************************
void UdplSocketIf::sendPktData(void)
{
	int ret;		// socket 返り値
	TIMING_ELEM timing_local;	// timing情報
	do {
		// timing_list 待ち
		{
			std::unique_lock<std::mutex> lock(timing_m);

			list_wait_c.wait(lock, [this] {return !timing_list.empty(); });	// timing_list に何か入るのを待つ

			timing_local = timing_list.front();		// なにか入ってきたので、先頭要素を取り出す
			timing_list.pop_front();				// 先頭を消去
		
			// リングバッファ読み出し
			{
				std::lock_guard<std::mutex> lock(rbuf_m);
				rbuf->pop_buf(sendbuf, timing_local.len);
			}
		}

		// 時刻待ち
		std::this_thread::sleep_until(timing_local.timing);

		// ソケット送信
		{
			std::lock_guard<std::mutex> lock(sock_m);
			ret = sendto(sock, (char *)sendbuf, timing_local.len, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
		}

		// heart beat でなければタスクトレイアイコンを光らす
		if (timing_local.len != 1) {	// 1 は NOP だろう
			sendStat.store(true, std::memory_order_release);
		}

	} while (pktthreadOn.load(std::memory_order_acquire));
}

// ********************************
// ボードのバッファがクリアされるのを待つ
// ********************************
bool UdplSocketIf::waitBufClear(unsigned int add_wait)
{
	uint8_t recv[16];

	// ボード問い合わせの応答を待つ
	return pushRecv(COMMAND_INQUIRY, sizeof(COMMAND_INQUIRY), recv, sizeof(recv), add_wait);
}