package face

/*
#cgo LDFLAGS: -Wl,-rpath,${SRCDIR}/libface/sdk/lib
#cgo LDFLAGS: -L${SRCDIR}/libs -lwface
#cgo LDFLAGS: -Wl,-rpath,/opt/toolkits/cuda-10.2/lib64
#include <stdlib.h>
#include "cface.h"
*/
import "C"
import (
	"errors"
	"unsafe"

	"basic.com/libgowrapper/sdkstruct.git"
	"basic.com/pubsub/protomsg.git"
	"github.com/gogo/protobuf/proto"
)

// SDKFace sdk
type SDKFace struct {
	handle      unsafe.Pointer
	detector    bool
	extractor   bool
	propertizer bool
	tracker     bool
	fnLogger    func(...interface{})
}

// NewSDK sdk
func NewSDK(fn func(...interface{})) *SDKFace {
	h := C.create_sdkface()
	if h == nil {
		return nil
	}

	return &SDKFace{
		handle:      h,
		detector:    false,
		extractor:   false,
		propertizer: false,
		tracker:     false,
		fnLogger:    fn,
	}
}

// Free free
func (s *SDKFace) Free() {
	if s != nil && s.handle != nil {
		C.release(s.handle)
	}
}

func (s *SDKFace) printLog(l ...interface{}) {
	if s.fnLogger != nil {
		s.fnLogger(l)
	}
}

// Detector detector
func (s *SDKFace) Detector(minFaces, rollAngles, threadMax, gpu int) bool {

	if s.detector {
		return true
	}
	ret := C.init_detector(s.handle, C.int(minFaces), C.int(rollAngles), C.int(threadMax), C.int(gpu))
	if ret <= 0 {
		s.printLog("->face--> CREATE Detector ERROR: ", ret)
		return false
	}
	s.detector = true
	return true
}

// Extractor ext
func (s *SDKFace) Extractor(threadMax, gpu int) bool {

	if s.extractor {
		return true
	}
	ret := C.init_extractor(s.handle, C.int(threadMax), C.int(gpu))
	if ret <= 0 {
		s.printLog("->face--> CREATE Extractor ERROR: ", ret)
		return false
	}
	s.extractor = true
	return true
}

// Propertizer prop
func (s *SDKFace) Propertizer(threadMax int) bool {

	if s.propertizer {
		return true
	}
	ret := C.init_propertizer(s.handle, C.int(threadMax))
	if ret <= 0 {
		s.printLog("->face--> CREATE Propertizer ERROR: ", ret)
		return false
	}
	s.propertizer = true
	return true
}

// Tracker track
func (s *SDKFace) Tracker(w, h, maxFaces, interval, sampleSize, threadMax, gpu int) bool {

	if s.tracker {
		return s.tracker
	}
	ret := C.init_tracker(s.handle, C.int(w), C.int(h), C.int(maxFaces), C.int(interval), C.int(sampleSize), C.int(threadMax), C.int(gpu))

	if ret <= 0 {
		s.printLog("->face--> CREATE Tracker ERROR: ", ret)
		return false
	}
	s.tracker = true
	return true
}

// CFacePosArrayToGoArray convert cFacePos array to go
func CFacePosArrayToGoArray(cArray unsafe.Pointer, count int) (goArray []sdkstruct.CFacePos) {
	p := uintptr(cArray)

	for i := 0; i < count; i++ {
		j := *(*sdkstruct.CFacePos)(unsafe.Pointer(p))

		goArray = append(goArray, j)

		p += unsafe.Sizeof(j)
	}
	return
}

// Detect det
func (s *SDKFace) Detect(data []byte, w, h, c int, ch int) []sdkstruct.CFacePos {
	if !s.detector {
		return nil
	}

	var cfpos unsafe.Pointer
	var count C.int
	ret := C.detect(s.handle, unsafe.Pointer(&data[0]), C.int(w), C.int(h), C.int(c), C.int(ch), &cfpos, &count)
	if ret > 0 {
		return CFacePosArrayToGoArray(cfpos, int(count))
	}
	s.printLog("->face--> Detect No One, Ret: ", ret)
	return nil
}

// Extract extract
func (s *SDKFace) Extract(fpos sdkstruct.CFacePos, data []byte, w, h, c int, ch int) []byte {

	pos := (*C.cFacePos)(unsafe.Pointer(&fpos))

	//(void *handle, const cFacePos *pos, const void*data, const int w, const int h, const int c, const int chan, void **feat, int *featLen);
	var feat unsafe.Pointer
	var featLen C.int
	ret := C.extract(s.handle, pos, unsafe.Pointer(&data[0]), C.int(w), C.int(h), C.int(c), C.int(ch), &feat, &featLen)
	if ret > 0 {
		return C.GoBytes(feat, featLen)
	}
	s.printLog("->face--> Extract Nothing, Ret: ", ret)
	return nil
}

// #if FEATURE_NORMALIZE
// float CosineDistance(const BYTE* fea1, const BYTE* fea2, int length)
// {
// 	int i;
// 	const float* jfea1 = (const float*)fea1;
// 	const float* jfea2 = (const float*)fea2;

// 	float score = 0.0f;
// 	for (i = 0; i < length; i++)
// 	{
// 		score += jfea1[i] * jfea2[i];
// 	}

// 	return score;
// }
// #else
// float CosineDistance(const BYTE* fea1, const BYTE* fea2, int length)
// {
// 	int i;
// 	const float* jfea1 = (const float*)fea1;
// 	const float* jfea2 = (const float*)fea2;
// 	double normTemp1 = 0.0;
// 	double normTemp2 = 0.0;
// 	double normTemp = 0.0;

// 	double score = 0.0f;
// 	for (i = 0; i < length; i++) {
// 		score += jfea1[i] * jfea2[i];
// 		normTemp1 += jfea1[i] * jfea1[i];
// 		normTemp2 += jfea2[i] * jfea2[i];
// 	}
// 	normTemp = sqrt(normTemp1)*sqrt(normTemp2);

// 	score = score / normTemp;

// 	return score;
// }
// #endif

// int feaDim = FEATURE_RAW_SIZE / 4;

// THFEATURE_API float EF_Compare(BYTE* pFeature1,BYTE* pFeature2)
// {
// 	if(pFeature1==NULL||pFeature2==NULL) return 0.0f;

// 	float fscore;

// 	BYTE* pFea1=pFeature1;
// 	BYTE* pFea2=pFeature2;

// 	fscore = CosineDistance(pFea1, pFea2, feaDim);
// 	fscore+=0.05f;

// 	if(fscore>0.9999f) fscore=0.9999f;
// 	if(fscore<0.0001f) fscore=0.0001f;

//		return  fscore;
//	}
//

func byteSlice2float32Slice(src []byte) []float32 {
	if len(src) == 0 {
		return nil
	}

	l := len(src) / 4
	ptr := unsafe.Pointer(&src[0])

	return (*[1 << 26]float32)(ptr)[:l:l]
}

// Compare face compare
func (s *SDKFace) Compare(feat1 []byte, feat2 []byte) float32 {

	ffeat1 := byteSlice2float32Slice(feat1)
	ffeat2 := byteSlice2float32Slice(feat2)
	if len(ffeat1) != len(ffeat2) {
		return 0
	}
	// normalize
	var score float32
	for i := 0; i < len(ffeat1); i++ {
		score += ffeat1[i] * ffeat2[i]
	}
	score += 0.05
	if score > 0.9999 {
		score = 0.9999
	}
	if score < 0.0001 {
		score = 0.0001
	}
	return score

	// if !s.extractor {
	// 	return 0
	// }
	// res := C.compare(s.handle, (*C.uchar)(unsafe.Pointer(&feat1[0])), (*C.uchar)(unsafe.Pointer(&feat2[0])))
	// return float32(res)
}

// Propertize prop
func (s *SDKFace) Propertize(fpos sdkstruct.CFacePos, data []byte, w, h, c int, ch int) *sdkstruct.CThftResult {
	if !s.propertizer {
		return nil
	}

	pos := (*C.cFacePos)(unsafe.Pointer(&fpos))

	var thft unsafe.Pointer
	ret := C.propertize(s.handle, pos, unsafe.Pointer(&data[0]), C.int(w), C.int(h), C.int(c), C.int(ch), &thft)
	if ret == 0 {
		gothft := *(*sdkstruct.CThftResult)(thft)
		C.free(thft)
		return &gothft
	}
	s.printLog("->face--> Propertize Nothing, Ret: ", ret)
	return nil
}

// CFaceInfoArrayToGoArray convert cFaceInfo array to go
func CFaceInfoArrayToGoArray(cArray unsafe.Pointer, count int) (goArray []sdkstruct.CFaceInfo) {
	p := uintptr(cArray)

	for i := 0; i < count; i++ {
		j := *(*sdkstruct.CFaceInfo)(unsafe.Pointer(p))
		goArray = append(goArray, j)
		p += unsafe.Sizeof(j)
	}
	return
}

// Track track
func (s *SDKFace) Track(data []byte, w, h, c int, ch int) []sdkstruct.CFaceInfo {
	if !s.tracker {
		return nil
	}

	//img, const int chan, void **fInfo, int *fcnt);

	var fCount C.int
	var finfos unsafe.Pointer
	ret := C.track(s.handle, unsafe.Pointer(&data[0]), C.int(w), C.int(h), C.int(c), C.int(ch), &finfos, &fCount)

	if ret > 0 {
		faces := CFaceInfoArrayToGoArray(finfos, int(fCount))
		//if len(faces) > 0{
		// fmt.Println("faces detected:", len(faces))
		//}
		return faces
	}
	return nil
}

// FaceInfo2FacePos info -> pos
func FaceInfo2FacePos(face sdkstruct.CFaceInfo) (fPos sdkstruct.CFacePos) {
	fPos.RcFace = face.RcFace
	fPos.PtLeftEye = face.PtLeftEye
	fPos.PtRightEye = face.PtRightEye
	fPos.PtNose = face.PtNose
	fPos.PtMouth = face.PtMouth
	fPos.FAngle.Yaw = face.FAngle.Yaw
	fPos.FAngle.Pitch = face.FAngle.Pitch
	fPos.FAngle.Roll = face.FAngle.Roll
	fPos.FAngle.Confidence = face.FAngle.Confidence

	copy(fPos.PFacialData[:], face.PFacialData[:512])

	return fPos
}

// TrackerResize init face tracker
func (s *SDKFace) TrackerResize(w, h, ch int) bool {

	if !s.tracker {
		s.printLog("->face--> TrackerResize Failed, No Tracker Init")
		return false
	}
	ret := C.track_resize(s.handle, C.int(w), C.int(h), C.int(ch))
	if ret == 1 {
		return true
	}
	s.printLog("->face--> TrackerResize Failed, Ret: ", ret, " SDK Channel: ", ch, " Size: ", w, "x", h)
	return false
}

// Run run
func (s *SDKFace) Run(data []byte, w, h, c, dchan int) (int, []byte, error) {
	if data == nil || w <= 0 || h <= 0 {
		return 0, nil, errors.New("->face--> Face Input Image Error")
	}

	if !s.tracker || !s.extractor || !s.propertizer {
		return 0, nil, errors.New("->face--> Face SDK No Init Correctly")
	}

	channel := c
	if channel == 0 {
		channel = 3
	}

	var fInfo []sdkstruct.CFaceInfo

	fInfo = s.Track(data, w, h, c, dchan)
	if len(fInfo) == 0 {
		return 0, nil, errors.New("->face--> Face Track No One")
	}

	var faces []*protomsg.ResultFaceDetect

	for _, d := range fInfo {

		//运行sd
		dec := FaceInfo2FacePos(d)
		p := s.Propertize(dec, data, w, h, c, dchan)
		feat := s.Extract(dec, data, w, h, c, dchan)

		/// filter rules
		// sdkid := rMsg.Msg.Tasklab.Sdkinfos[rMsg.Msg.Tasklab.Index].Ipcid
		// size := (d.RcFace.Right - d.RcFace.Left) * (d.RcFace.Bottom - d.RcFace.Top)
		// angle := d.FAngle
		// if !filter(rMsg.Msg.Tasklab.Taskid, sdkid, angle.Confidence, float32(angle.Yaw), int(size)) {
		// 	continue
		// }
		/// filter rules

		prop := (*protomsg.ThftResult)(unsafe.Pointer(&p))
		fpos := tconvert2ProtoFacePos(d)

		//组成结果并序列化
		res := &protomsg.ResultFaceDetect{Pos: fpos, Result: prop, Feats: feat}
		faces = append(faces, res)

	}
	facePos := protomsg.ParamFacePos{Faces: faces}
	d, e := proto.Marshal(&facePos)

	return len(faces), d, e
}

func tconvert2ProtoFacePos(dec sdkstruct.CFaceInfo) *protomsg.FacePos {

	crect := dec.RcFace
	rect := protomsg.Rect{Left: crect.Left, Top: crect.Top, Right: crect.Right, Bottom: crect.Bottom}
	leftEye := (*protomsg.Point)(unsafe.Pointer(&dec.PtLeftEye))
	rightEye := (*protomsg.Point)(unsafe.Pointer(&dec.PtRightEye))
	mouth := (*protomsg.Point)(unsafe.Pointer(&dec.PtMouth))
	nose := (*protomsg.Point)(unsafe.Pointer(&dec.PtNose))
	angle := (*protomsg.FaceAngle)(unsafe.Pointer(&dec.FAngle))
	faceID := uint64(dec.NFaceID)

	facialData := dec.PFacialData[:512]

	// facialData := make([]byte, 512)
	// copy(facialData[:], dec.PFacialData[:512])

	return &protomsg.FacePos{
		RcFace:     &rect,
		PtLeftEye:  leftEye,
		PtRightEye: rightEye,
		PtMouth:    mouth,
		PtNose:     nose,
		FAngle:     angle,
		Quality:    dec.NQuality,
		FacialData: facialData,
		FaceID:     faceID,
	}
}