#include <stdint.h>
#include <math.h>
#include "error.h"
#include "unit_test_framework.h"
#include "time_ctrl/time_ctrl.h"

#define HARDWARE_VERSION 0x00000001
#define MAX_OPT_DELAY 100 // 1us = 100*10ns


TIMECTRL_CONFIG tmcConfig = {
	.baseAddr = 0/*TODO: fill withe real base address*/,
	.reset = 10
};

TIMECTRL_INST* tmcInstPtr = NULL;

void tmc_setup(void){
	int32_t Status;
	Status = TimeCtrl_Init(&tmcInstPtr, &tmcConfig);
	ASSERT_TRUE(SUCCESS == Status);
}

void tmc_teardown(void){
	TimeCtrl_Destroy(tmcInstPtr);
}

TEST(tmc_version) {
	ASSERT_EQUAL(HARDWARE_VERSION, TimeCtrl_GetVersion(tmcInstPtr));
	return;
}

TEST(tmc_read_time) {
	TIME_WORD timeStamp;
	TIME_INT timeH;
	TIME_FRAC timeL;

	// Test time stamp read operation
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStamp);
	timeH = TimeCtrl_GetIntCnt(tmcInstPtr);
	timeL = TimeCtrl_GetFracCnt(tmcInstPtr);
	if(timeL >= timeStamp.frac_part) {
		ASSERT_EQUAL(timeH, timeStamp.int_part);
		ASSERT_TRUE(timeL - timeStamp.frac_part < MAX_OPT_DELAY);
	}
	else {
		ASSERT_EQUAL(timeH, timeStamp.int_part+1);
		ASSERT_TRUE(timeL + COUNTE_FRAC_MAX - timeStamp.frac_part < MAX_OPT_DELAY);
	}
}

TEST(tmc_reset) {
	TIME_WORD timeStamp;

	TimeCtrl_Reset(tmcInstPtr);
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStamp);
	ASSERT_EQUAL(0, timeStamp.int_part);
	ASSERT_TRUE(timeStamp.frac_part < MAX_OPT_DELAY);
}

TEST(tmc_adjustment) {
	TIME_WORD timeStampPre, timeStampPost, timeAdj;

	// Adjustment -- set
	timeAdj.frac_part = 0;
	timeAdj.int_part = 0x00000203;
	TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_SET);
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
	ASSERT_EQUAL(timeAdj.int_part, timeStampPost.int_part);
	ASSERT_TRUE(timeStampPost.frac_part < MAX_OPT_DELAY);

	timeAdj.frac_part = COUNTE_FRAC_MAX;
	timeAdj.int_part = 0x00000203;
	TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_SET);
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
	ASSERT_EQUAL(timeAdj.int_part, timeStampPost.int_part+1);
	ASSERT_TRUE(timeStampPost.frac_part < MAX_OPT_DELAY);

	timeAdj.frac_part = COUNTE_FRAC_MAX*2;
	timeAdj.int_part = 0x00000203;
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPre);
	TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_SET);
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
	if(timeStampPost.frac_part >= timeStampPre.frac_part) {
		ASSERT_EQUAL(timeStampPost.int_part, timeStampPre.int_part);
		ASSERT_TRUE(timeStampPost.frac_part - timeStampPre.frac_part < MAX_OPT_DELAY);
	}
	else {
		ASSERT_EQUAL(timeStampPost.int_part, timeStampPre.int_part + 1);
		ASSERT_TRUE(timeStampPost.frac_part + COUNTE_FRAC_MAX - timeStampPre.frac_part < MAX_OPT_DELAY);
	}

	// Adjustment -- add
	timeAdj.frac_part = 253;
	timeAdj.int_part = 253;
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPre);
	TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_ADD);
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
	if(timeStampPre.frac_part + timeAdj.frac_part > COUNTE_FRAC_MAX) {
		ASSERT_TRUE(timeStampPost.int_part <= timeStampPre.int_part + timeAdj.int_part + 2);
		ASSERT_TRUE(timeStampPost.frac_part + COUNTE_FRAC_MAX - timeStampPre.frac_part 
					< MAX_OPT_DELAY + timeAdj.frac_part);
	}

	timeAdj.frac_part = -253;
	timeAdj.int_part = -253;
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPre);
	TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_ADD);
	TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
	if(timeStampPre.frac_part + timeAdj.frac_part > COUNTE_FRAC_MAX) {
		ASSERT_TRUE(timeStampPost.int_part <= timeStampPre.int_part + timeAdj.int_part + 2);
		ASSERT_TRUE(timeStampPost.frac_part + COUNTE_FRAC_MAX - timeStampPre.frac_part 
					< MAX_OPT_DELAY + timeAdj.frac_part);
	}
}

TEST(tmc_setopt) {
	// TODO:
	return;
}

TEST_SUITE(tmc_unit_test) {
	TEST_SUITE_CONFIGURE(tmc_setup, tmc_teardown);
	RUN_TEST(tmc_version);
	RUN_TEST(tmc_read_time);
	RUN_TEST(tmc_reset);
	RUN_TEST(tmc_adjustment);
	RUN_TEST(tmc_setopt);
}

int main(void) {
	RUN_TEST_SUITE(tmc_unit_test);
	TEST_REPORT();
	return TEST_EXIT_CODE;
}