package gohumantrack

/*
#cgo CFLAGS: -I${SRCDIR}/sdk -w -g
#cgo LDFLAGS: -L/usr/local/cuda/lib64 -L${SRCDIR}/sdk/libs
#cgo LDFLAGS: -L${SRCDIR}/opencv-2.4.13/lib -L${SRCDIR}/TensorRT-5.1.2.2/lib
#cgo LDFLAGS: -Wl,-rpath,${SRCDIR}/sdk/libs:${SRCDIR}/TensorRT-5.1.2.2:${SRCDIR}/opencv-2.4.13
#cgo LDFLAGS: -lmidHumanTrack -lHumanTracker -lutools
#cgo LDFLAGS: -lcudart -lcublas -lcurand -lcusolver -lcudnn
#cgo LDFLAGS: -lnppc -lnppicom
#cgo LDFLAGS: -lnvrtc -lnvinfer -lnvinfer_plugin -lnvparsers -lnvonnxparser
#cgo LDFLAGS: -lopencv_video -lopencv_highgui
#cgo LDFLAGS: -ldl -lpthread -lm

#include <stdlib.h>
#include "c_include/c_human_tracker.h"

void *create_batch_image(const int size){
	c_img *imgs = (c_img*)malloc(size * sizeof(c_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;
	c_img *images = (c_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){
	c_fgRet *ret = (c_fgRet*)malloc(size * sizeof(c_fgRet));
	for(int i = 0; i < size; i++){
		ret[i].fgNum = 0;
	}
	return ret;
}
void *process(void *handle, void *imgs, const int size){
	c_img *images = (c_img*)imgs;
	c_fgRet *res = init_fgres(size);
	int ret = c_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

// FgInfo info
type FgInfo struct {
	Left       int32
	Right      int32
	Top        int32
	Bottom     int32
	Confidence float32
	X          int32
	Y          int32
	ID         int32
	Feature    [128]float32
}

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

// FgResult result
type FgResult struct {
	Fginfo [2000]FgInfo
	FgNum  int32
}

// HumanTracker struct
type HumanTracker struct {
	handle    unsafe.Pointer
	batchSize int
}

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

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

// GetVersion ver
func (h *HumanTracker) GetVersion() string {
	if h.handle == nil {
		return "human tracker handle null"
	}
	ver := C.c_getVersion()
	if ver == nil {
		return "get version string null"
	}
	return C.GoString(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(C.int(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, C.int(h.batchSize), C.int(k), unsafe.Pointer(&v.Data[0]), C.int(v.Width), C.int(v.Height), C.int(v.Channel))
		if int(ret) != k {
			return nil, errors.New("fill C images error")
		}
	}

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

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

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

	for i := 0; i < 128; 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

}

// CFFSimilarity use c code
func CFFSimilarity(feaA, feaB []float32) (float64, error) {
	if len(feaA) != len(feaB) {
		return 0, errors.New("Must be same length")
	}
	if len(feaA) != 128 || len(feaB) != 128 {
		return 0, errors.New("Must be 128 size")
	}

	s := C.c_FF_Similarity((*C.float)(&feaA[0]), (*C.float)(&feaB[0]))
	return float64(s), nil
}