package gohumantrack

/*
#cgo CFLAGS: -I${SRCDIR}/sdk/include -I/usr/local/cuda/include -w -g
#cgo LDFLAGS: -L/usr/local/cuda/lib64 -L${SRCDIR}/sdk/libs
#cgo LDFLAGS: -Wl,-rpath,${SRCDIR}/sdk/libs
#cgo LDFLAGS: -lHumanTracker -lutools -ldl -lpthread -lm

#include <stdlib.h>
#include "sy_human_tracker.h"

typedef void* hdl_human_tracker;

void *create_batch_image(const int size){
	sy_img *imgs = (sy_img*)malloc(size * sizeof(sy_img));
	return imgs;
}
int fill_images(void *imgs, const int size, const int index, void *data, const int w, const int h, const int c){
	if(!imgs || !data || size <= index) return -1;
	sy_img *images = (sy_img*)imgs;
	images[index].data_ = data;
	images[index].w_ = w;
	images[index].h_ = h;
	images[index].c_ = c;
	return index;
}
void *init_fgres(const int size){
	fgRet *ret = (fgRet*)malloc(size * sizeof(fgRet));
	for(int i = 0; i < size; i++){
		ret[i].fgNum = 0;
	}
}
void *process(void *handle, void *imgs, const int size){
	sy_img *images = (sy_img*)imgs;
	fgRet *res = init_fgres(size);
	int ret = human_tracker_process(handle, images, size, res);
	if (ret != 0)
		return NULL;
	return res;
}
*/
import "C"
import (
	"errors"
	"math"
	"unsafe"
)

// ImageHumanTracker image
type ImageHumanTracker struct {
	Data    []byte
	Width   int
	Height  int
	Channel int
}

// int left;//行人包围框左上角横坐标
// int right;//行人包围框右下角横坐标
// int top;//行人包围框左上角纵坐标
// int bottom;//行人包围框右下角纵坐标
// float confidence;//行人检测框置信度得分
// int center_x;//行人包围框中心点x
// int center_y;//行人包围框中心点y
// int ID;//行人跟踪ID
// float feature[FEATURESIZE];//行人专属特征，可用来做ReID

const (
	MAX_BG_NUM  = 2000
	FEATURESIZE = 128
)

// FgInfo info
type FgInfo struct {
	Left       int
	Right      int
	Top        int
	Bottom     int
	Confidence float32
	CenterX    int
	CenterY    int
	Feature    [FEATURESIZE]float32
}

// fgInfo fginfo[MAX_BG_NUM];//上述结构体，代表一幅图中所有的框，最大支持2000个框，暂不可修改
// int fgNum;//一幅图中框的数量（fginfo中的有效框数）

// FgResult result
type FgResult struct {
	Infos [MAX_BG_NUM]FgInfo
	FgNum int
}

// HumanTracker struct
type HumanTracker struct {
	handle    C.hdl_human_tracker
	batchSize int
}

// NewHumanTracker new
func NewHumanTracker(gpu, batchSize, flag int) *HumanTracker {
	if gpu == -1 {
		return nil
	}
	p := C.human_tracker_create(C.int(gpu), C.int())
	if p != nil {
		return &HumanTracker{p, batchSize}
	}
	return nil
}

// Free free
func (h *HumanTracker) Free() {
	if h.handle != nil {
		C.human_tracker_release(&h.handle)
	}
}

// GetVersion ver
func (h *HumanTracker) GetVersion() string {
	if h.handle == nil {
		return "human tracker handle null"
	}
	ver := C.getVersion()
	if ver == nil {
		return "get version string null"
	}
	return C.CString(ver)
}

// Process process
func (h *HumanTracker) Process(imgs []ImageHumanTracker) ([]FgResult, error) {
	if len(imgs) != h.batchSize {
		return nil, errors.New("input images count doesn't equalize to batchsize")
	}
	cImgs := C.create_batch_image(h.batchSize)
	if cImgs == nil {
		return nil, errors.New("create C images error")
	}
	defer C.free(cImgs)
	for k, v := range imgs {
		ret := C.fill_images(cImgs, h.batchSize, k, unsafe.Pointer(&v.Data[0]), C.int(v.Width), C.int(v.Height), C.int(v.Channel))
		if ret != k {
			return nil, errors.New("fill C images error")
		}
	}

	cRet := C.process(h.handle, cImgs, h.batchSize)
	if cRet == nil {
		return nil, errors.New("create C results error")
	}
	defer C.free(cRet)

	var result []FgResult
	p := uintptr(cRet)
	for i := 0; i < h.batchSize; i++ {
		j := *(*FgResult)(unsafe.Pointer(p))
		FgResult = append(FgResult, j)
		p += unsafe.Sizeof(j)
	}
	return
}

// FFSimilarity similarity
func FFSimilarity(feaA, feaB [FEATURESIZE]float32) float64 {
	var norm1, norm2 float64
	var score float64

	for i := 0; i < FEATURESIZE; i++ {
		norm1 += float64(feaA[i]) * float64(feaA[i])
		norm2 += float64(feaB[i]) * float64(feaB[i])
		score += float64(feaA[i]) * float64(feaB[i])
	}

	norm1 = math.Sqrt(norm1)
	norm2 = math.Sqrt(norm2)

	score = score / (norm1 * norm2)

	if score < 0 {
		score = 0
	}
	return score

}