#include "ANSPOSE.h"
#include <pipelines/metadata.h>
#include <models/input_data.h>
#include <models/results.h>
namespace ANSCENTER {
	bool ANSPOSE::OptimizeModel(bool fp16, std::string& optimizedModelFolder) {
		std::lock_guard<std::recursive_mutex> lock(_mutex);
		if (!ANSODBase::OptimizeModel(fp16, optimizedModelFolder)) {
			return false;
		}
		if (FileExist(_modelFilePath)) {
			std::string modelName = GetFileNameWithoutExtension(_modelFilePath);
			std::string binaryModelName = modelName + ".bin";
			std::string modelFolder = GetParentFolder(_modelFilePath);
			std::string optimizedModelPath = CreateFilePath(modelFolder, binaryModelName);
			if (FileExist(optimizedModelPath)) {
				this->_logger.LogDebug("ANSPOSE::OptimizeModel", "This model is optimized. No need other optimization.", __FILE__, __LINE__);
				optimizedModelFolder = modelFolder;
				return true;
			}
			else {
				this->_logger.LogFatal("ANSPOSE::OptimizeModel", "This model can not be optimized.", __FILE__, __LINE__);
				optimizedModelFolder = modelFolder;
				return false;
			}
		}
		else {
			this->_logger.LogFatal("ANSPOSE::OptimizeModel", "This model is not exist. Please check the model path again.", __FILE__, __LINE__);
			optimizedModelFolder = "";
			return false;
		}
	}

	bool ANSPOSE::LoadModel(const std::string& modelZipFilePath, const std::string& modelZipPassword) {
		std::lock_guard<std::recursive_mutex> lock(_mutex);
		try {
			bool result = ANSODBase::LoadModel(modelZipFilePath, modelZipPassword);
			if (!result) return false;
			_device = GetOpenVINODevice(_core);
			// 0. Check if the configuration file exist
			if (FileExist(_modelConfigFile)) {
				ModelType modelType;
				std::vector<int> inputShape;
				_classes = ANSUtilityHelper::GetConfigFileContent(_modelConfigFile, modelType, inputShape);
				if (inputShape.size() == 2) {
					if (inputShape[0] > 0)_modelConfig.inpHeight = inputShape[0];
					if (inputShape[1] > 0)_modelConfig.inpWidth = inputShape[1];
				}
			}
			else {// This is old version of model zip file
				std::string xmlfile = CreateFilePath(_modelFolder, "ansposesetimation.xml");
				std::string binaryfile = CreateFilePath(_modelFolder, "ansposesetimation.bin");
				if (std::filesystem::exists(xmlfile)) {
					_modelFilePath = xmlfile;
					this->_logger.LogDebug("ANSPOSE::Initialize. Loading OpenVINO weight", _modelFilePath, __FILE__, __LINE__);
					if (FileExist(binaryfile)) {
						this->_logger.LogDebug("ANSPOSE::Initialize binary weight", binaryfile, __FILE__, __LINE__);
					}
					else {
						this->_logger.LogError("ANSPOSE::Initialize. Model binary file does not exist", binaryfile, __FILE__, __LINE__);
						return false;
					}
				}
				else {
					this->_logger.LogError("ANSPOSE::Initialize. Model file does exist", _modelFilePath, __FILE__, __LINE__);
					return false;
				}
			}
			_isInitialized = true;
			return true;
		}
		catch (std::exception& e) {
			this->_logger.LogFatal("ANSPOSE::LoadModel", e.what(), __FILE__, __LINE__);
			return false;
		}
	}
	bool ANSPOSE::LoadModelFromFolder(std::string licenseKey, ModelConfig modelConfig, std::string modelName, std::string className, const std::string& modelFolder, std::string& labelMap) {
		std::lock_guard<std::recursive_mutex> lock(_mutex);
		try {
			bool result = ANSODBase::LoadModelFromFolder(licenseKey, modelConfig, modelName, className, modelFolder, labelMap);
			if (!result) return false;
			std::string _modelName = modelName;
			if (_modelName.empty()) {
				_modelName = "ansposesetimation";
			}
			std::string xmlFileName = _modelName + ".xml";
			std::string binFileName = _modelName + ".bin";
			_modelConfig = modelConfig;
			_modelConfig.detectionType = ANSCENTER::DetectionType::KEYPOINT;
			_modelConfig.modelType = ModelType::POSE;
			_modelConfig.inpHeight = 640;
			_modelConfig.inpWidth = 640;
			if (_modelConfig.modelMNSThreshold < 0.2)
				_modelConfig.modelMNSThreshold = 0.5;
			if (_modelConfig.modelConfThreshold < 0.2)
				_modelConfig.modelConfThreshold = 0.5;
			_device = GetOpenVINODevice(_core);
			// 0. Check if the configuration file exist
			if (FileExist(_modelConfigFile)) {
				ModelType modelType;
				std::vector<int> inputShape;
				_classes = ANSUtilityHelper::GetConfigFileContent(_modelConfigFile, modelType, inputShape);
				if (inputShape.size() == 2) {
					if (inputShape[0] > 0)_modelConfig.inpHeight = inputShape[0];
					if (inputShape[1] > 0)_modelConfig.inpWidth = inputShape[1];
				}
			}
			else {// This is old version of model zip file
				std::string xmlfile = CreateFilePath(_modelFolder, xmlFileName);
				std::string binaryfile = CreateFilePath(_modelFolder, binFileName);
				if (std::filesystem::exists(xmlfile)) {
					_modelFilePath = xmlfile;
					this->_logger.LogDebug("ANSPOSE::Initialize. Loading OpenVINO weight", _modelFilePath, __FILE__, __LINE__);
					if (FileExist(binaryfile)) {
						this->_logger.LogDebug("ANSPOSE::Initialize binary weight", binaryfile, __FILE__, __LINE__);
					}
					else {
						this->_logger.LogError("ANSPOSE::Initialize. Model binary file does not exist", binaryfile, __FILE__, __LINE__);
						return false;
					}
				}
				else {
					this->_logger.LogError("ANSPOSE::Initialize. Model file does exist", _modelFilePath, __FILE__, __LINE__);
					return false;
				}
			}
			labelMap = "Person";
			_isInitialized = true;
			return true;
		}
		catch (std::exception& e) {
			this->_logger.LogFatal("ANSPOSE::LoadModel", e.what(), __FILE__, __LINE__);
			return false;
		}
	}


	bool ANSPOSE::Initialize(std::string licenseKey, ModelConfig modelConfig, const std::string& modelZipFilePath, const std::string& modelZipPassword, std::string& labelMap) {
		std::lock_guard<std::recursive_mutex> lock(_mutex);
		try {
			std::string openVINOVersion = ov::get_openvino_version().buildNumber;
			this->_logger.LogDebug("ANSPOSE::Initialize. OpenVINO version", openVINOVersion, __FILE__, __LINE__);
			bool result = ANSODBase::Initialize(licenseKey, modelConfig, modelZipFilePath, modelZipPassword, labelMap);
			if (!result) return false;
			// Parsing for ABNORMAL only here
			_modelConfig = modelConfig;
			_modelConfig.detectionType = ANSCENTER::DetectionType::KEYPOINT;
			_modelConfig.modelType = ModelType::POSE;
			_modelConfig.inpHeight = 640;
			_modelConfig.inpWidth = 640;
			if (_modelConfig.modelMNSThreshold < 0.2)
				_modelConfig.modelMNSThreshold = 0.5;
			if (_modelConfig.modelConfThreshold < 0.2)
				_modelConfig.modelConfThreshold = 0.5;
			_device = GetOpenVINODevice(_core);
			// 0. Check if the configuration file exist
			if (FileExist(_modelConfigFile)) {
				ModelType modelType;
				std::vector<int> inputShape;
				_classes = ANSUtilityHelper::GetConfigFileContent(_modelConfigFile, modelType, inputShape);
				if (inputShape.size() == 2) {
					if (inputShape[0] > 0)_modelConfig.inpHeight = inputShape[0];
					if (inputShape[1] > 0)_modelConfig.inpWidth = inputShape[1];
				}
			}
			else {// This is old version of model zip file
				std::string xmlfile = CreateFilePath(_modelFolder, "ansposesetimation.xml");
				std::string binaryfile = CreateFilePath(_modelFolder, "ansposesetimation.bin");
				if (std::filesystem::exists(xmlfile)) {
					_modelFilePath = xmlfile;
					this->_logger.LogDebug("ANSPOSE::Initialize. Loading OpenVINO weight", _modelFilePath, __FILE__, __LINE__);
					if (FileExist(binaryfile)) {
						this->_logger.LogDebug("ANSPOSE::Initialize binary weight", binaryfile, __FILE__, __LINE__);
					}
					else {
						this->_logger.LogError("ANSPOSE::Initialize. Model binary file does not exist", binaryfile, __FILE__, __LINE__);
						return false;
					}
				}
				else {
					this->_logger.LogError("ANSPOSE::Initialize. Model file does exist", _modelFilePath, __FILE__, __LINE__);
					return false;
				}
			}
			labelMap = "Person";
			_isInitialized = true;
			return true;
		}
		catch (std::exception& e) {
			this->_logger.LogFatal("ANSANOMALIB::Initialize", e.what(), __FILE__, __LINE__);
			labelMap = "Normal,Defect";
			return false;
		}
	}
	std::vector<Object> ANSPOSE::RunInference(const cv::Mat& input) {
		return RunInference(input, "CustomCam");
	}

	std::vector<Object> ANSPOSE::RunInference(const cv::Mat& input,
		const std::string& camera_id)
	{
		std::lock_guard<std::recursive_mutex> lock(_mutex);

		try {
			// Validation
			if (!_licenseValid) {
				_logger.LogError("ANSPOSE::RunInference", "Invalid License",
					__FILE__, __LINE__);
				return {};
			}

			if (!_isInitialized) {
				_logger.LogError("ANSPOSE::RunInference", "Model is not initialized",
					__FILE__, __LINE__);
				return {};
			}

			if (input.empty() || input.cols < 10 || input.rows < 10) {
				return {};
			}

			// Convert to BGR if needed
			cv::Mat processedImage;
			if (input.channels() == 1) {
				cv::cvtColor(input, processedImage, cv::COLOR_GRAY2BGR);
			}
			else {
				processedImage = input;  // Shallow copy
			}

			const int width = processedImage.cols;
			const int height = processedImage.rows;

			// Initialize pipeline once
			if (!_poseEstimationPipeline) {
				double aspectRatio = width / static_cast<double>(height);

				auto model = std::make_unique<HPEOpenPose>(
					_modelFilePath, aspectRatio, 0, 0.5f, ""
				);

				try {
					_poseEstimationPipeline = new AsyncPipeline(std::move(model), ConfigFactory::getUserConfig(_device, 1, "", 4), _core);
				}
				catch (const std::exception& e) {
					// Fall back to CPU
					_logger.LogWarn("ANSPOSE::RunInference",
						"GPU init failed, using CPU: " + std::string(e.what()),
						__FILE__, __LINE__);

					model = std::make_unique<HPEOpenPose>(
						_modelFilePath, aspectRatio, 0, 0.5f, ""
					);
					_poseEstimationPipeline = new AsyncPipeline(std::move(model), ConfigFactory::getUserConfig("CPU", 0, "", 0), _core);
				}
			}

			if (!_poseEstimationPipeline) {
				_logger.LogError("ANSPOSE::RunInference", "Pipeline initialization failed",
					__FILE__, __LINE__);
				return {};
			}

			// Run inference
			auto startTime = std::chrono::steady_clock::now();
			int64_t frameNum = _poseEstimationPipeline->submitData(
				ImageInputData(processedImage),
				std::make_shared<ImageMetaData>(processedImage, startTime)
			);

			_poseEstimationPipeline->waitForTotalCompletion();
			std::unique_ptr<ResultBase> result = _poseEstimationPipeline->getResult();

			// Process results
			std::vector<Object> output;
			auto& poses = result->asRef<HumanPoseResult>().poses;
			output.reserve(poses.size());

			const cv::Rect imageBounds(0, 0, width, height);

			for (const auto& pose : poses) {
				if (pose.keypoints.empty()) {
					continue;
				}

				// Calculate bounding box from keypoints
				int x_min = width, y_min = height;
				int x_max = 0, y_max = 0;

				for (const auto& kp : pose.keypoints) {
					if (kp.x > 0) {
						x_min = std::min(x_min, static_cast<int>(kp.x));
						x_max = std::max(x_max, static_cast<int>(kp.x));
					}
					if (kp.y > 0) {
						y_min = std::min(y_min, static_cast<int>(kp.y));
						y_max = std::max(y_max, static_cast<int>(kp.y));
					}
				}

				// Validate bounding box
				if (x_max <= x_min || y_max <= y_min) {
					continue;
				}

				Object obj;
				obj.classId = 0;
				obj.className = "Person";
				obj.confidence = 1.0f;
				obj.cameraId = camera_id;

				// Set bounding box
				obj.box = cv::Rect(x_min, y_min, x_max - x_min, y_max - y_min);
				obj.box &= imageBounds;  // Clamp to image bounds

				// Extract mask ROI (CRITICAL: Clone to avoid dangling reference!)
				cv::Rect maskRect = obj.box & imageBounds;
				if (maskRect.area() > 0) {
					obj.mask = processedImage(maskRect).clone();  // Must clone!
				}

				// Store keypoints efficiently
				obj.kps.reserve(pose.keypoints.size() * 2);
				obj.polygon.reserve(pose.keypoints.size());

				for (const auto& kp : pose.keypoints) {
					obj.kps.push_back(kp.x);
					obj.kps.push_back(kp.y);
					obj.polygon.emplace_back(kp.x, kp.y);
				}

				// Build extraInfo string efficiently (if really needed)
				if (!pose.keypoints.empty()) {
					std::string keypointX, keypointY;
					keypointX.reserve(pose.keypoints.size() * 10);  // Estimate
					keypointY.reserve(pose.keypoints.size() * 10);

					for (size_t i = 0; i < pose.keypoints.size(); i++) {
						keypointX += std::to_string(pose.keypoints[i].x);
						keypointY += std::to_string(pose.keypoints[i].y);

						if (i < pose.keypoints.size() - 1) {
							keypointX += ";";
							keypointY += ";";
						}
					}

					obj.extraInfo = keypointX + "|" + keypointY;
				}

				output.push_back(std::move(obj));
			}

			if (_trackerEnabled) {
				output = ApplyTracking(output, camera_id);
				if (_stabilizationEnabled) output = StabilizeDetections(output, camera_id);
			}
			return output;

		}
		catch (const std::exception& e) {
			_logger.LogFatal("ANSPOSE::RunInference", e.what(), __FILE__, __LINE__);
			return {};
		}
	}

	ANSPOSE::~ANSPOSE() {
		if (_poseEstimationPipeline) {
			delete _poseEstimationPipeline;
			_poseEstimationPipeline = nullptr;
		}
		if (FolderExist(_modelFolder)) {
			if (!DeleteFolder(_modelFolder)) {
				this->_logger.LogError("ANSPOSE::~ANSPOSE", "Failed to release ANSPOSE Models", __FILE__, __LINE__);
			}
		}
	}
	bool ANSPOSE::Destroy() {
		if (_poseEstimationPipeline) {
			delete _poseEstimationPipeline;
			_poseEstimationPipeline = nullptr;
		}
		if (FolderExist(_modelFolder)) {
			if (!DeleteFolder(_modelFolder)) {
				this->_logger.LogError("ANSPOSE::Destroy", "Failed to release ANSPOSE Models", __FILE__, __LINE__);
			}
		}
		return true;
	}

}



// std::vector<Object> ANSPOSE::RunInference(const cv::Mat& input, const std::string& camera_id) {
 //     std::lock_guard<std::recursive_mutex> lock(_mutex);
 //     std::vector<Object> output;
 //     output.clear();
 //     if (!_licenseValid) {
 //         this->_logger.LogError("ANSPOSE::RunInference", "Invalid License", __FILE__, __LINE__);
 //         return output;
 //     }
 //     if (!_isInitialized) {
 //         this->_logger.LogError("ANSPOSE::RunInference", "Model is not initialized", __FILE__, __LINE__);
 //         return output;
 //     }
 //     try {
 //         if (input.empty()) return output;
 //         if ((input.cols < 10) || (input.rows < 10)) return output;
 //         // Convert grayscale to 3-channel BGR if needed
 //         cv::Mat processedImage;
 //         if (input.channels() == 1) {
 //             cv::cvtColor(input, processedImage, cv::COLOR_GRAY2BGR);
 //         }
 //         else {
 //             processedImage = input;
 //         }

 //         cv::Mat im = processedImage;
 //         int width = im.cols;
 //         int height = im.rows;
 //         if (!_poseEstimationPipeline) {
 //             // Load Model from Here
 //             std::unique_ptr<ModelBase> model;
 //             double aspectRatio = width / static_cast<double>(height);
 //             model.reset(new HPEOpenPose(_modelFilePath, aspectRatio, 0, static_cast<float>(0.5), ""));
 //             //ov::Core core;
 //             try {
 //                 _poseEstimationPipeline = new AsyncPipeline(std::move(model), ConfigFactory::getUserConfig(_device, 1, "", 4), _core);
		  //	}
		  //	catch (const std::exception& e) {
		  //		// Fall back to CPU
 //                 _poseEstimationPipeline = new AsyncPipeline(std::move(model), ConfigFactory::getUserConfig("CPU", 0, "", 0), _core);

 //             }
 //         }
 //         if (_poseEstimationPipeline) {
 //             auto startTime = std::chrono::steady_clock::now();
 //             int64_t frameNum = _poseEstimationPipeline->submitData(ImageInputData(im), std::make_shared<ImageMetaData>(im, startTime));
 //             _poseEstimationPipeline->waitForTotalCompletion();
 //             std::unique_ptr<ResultBase> result = _poseEstimationPipeline->getResult();


 //             /*    static const std::pair<int, int> keypointsOP[] = { 
 //                                                                    {1, 2},
 //                                                                    {1, 5},
 //                                                                    {2, 3},
 //                                                                    {3, 4},
 //                                                                    {5, 6},
 //                                                                    {6, 7},
 //                                                                    {1, 8},
 //                                                                    {8, 9},
 //                                                                    {9, 10},
 //                                                                    {1, 11},
 //                                                                    {11, 12},
 //                                                                    {12, 13},
 //                                                                    {1, 0},
 //                                                                    {0, 14},
 //                                                                    {14, 16},
 //                                                                    {0, 15},
 //                                                                    {15, 17}
 //                                                                   };*/
 //             const cv::Point2f absentKeypoint(-1.0f, -1.0f);
 //             for (auto& obj : result->asRef<HumanPoseResult>().poses) {
 //                 Object result;
 //                 float confidence = 1.0;
 //                 if (confidence > 0)
 //                 {
 //                     // Intepret keypoints 
 //                     int x_min = 10000;
 //                     int y_min = 10000;
 //                     int x_max = 0;
 //                     int y_max = 0;
 //                     std::stringstream keypointXss;
 //                     std::stringstream keypointYss;
		  //			std::vector<cv::Point2f> kps;
		  //			std::vector<float> keypoints;
 //                     if (obj.keypoints.size() > 0) {
 //                         for (size_t keypointIdx = 0; keypointIdx < obj.keypoints.size(); keypointIdx++) {
 //                             if (obj.keypoints[keypointIdx].x > 0) {
 //                                 if (x_min > obj.keypoints[keypointIdx].x) x_min = obj.keypoints[keypointIdx].x;
 //                                 if (x_max < obj.keypoints[keypointIdx].x) x_max = obj.keypoints[keypointIdx].x;
 //                             }
 //                             if (obj.keypoints[keypointIdx].y > 0) {
 //                                 if (y_min > obj.keypoints[keypointIdx].y) y_min = obj.keypoints[keypointIdx].y;
 //                                 if (y_max < obj.keypoints[keypointIdx].y) y_max = obj.keypoints[keypointIdx].y;
 //                             }
 //                             keypointXss << obj.keypoints[keypointIdx].x; // Add semicolon after each x value
 //                             keypointYss << obj.keypoints[keypointIdx].y; // Add semicolon after each y value
 //                             // Add semicolon after each value except the last one
 //                             if (keypointIdx < obj.keypoints.size() - 1) 
 //                             {
 //                                 keypointXss << ";";
 //                                 keypointYss << ";";
 //                             }
 //                             cv::Point2f kp;
		  //					kp.x = obj.keypoints[keypointIdx].x;
		  //					kp.y = obj.keypoints[keypointIdx].y;
		  //					kps.push_back(kp);
		  //					keypoints.push_back(kp.x);
		  //					keypoints.push_back(kp.y);
 //                         }

 //                         // Retrieve strings from string streams
 //                         std::string keypointXString = keypointXss.str();
 //                         std::string keypointYString = keypointYss.str();
 //                         std::string keypointString = keypointXString + "|" + keypointYString;

 //                         result.classId = 0; // human keypoint
 //                         result.className = "Person";
 //                         result.confidence = confidence;
 //                         result.box.x = x_min;
 //                         result.box.y = y_min;
 //                         result.box.width = x_max - x_min;
 //                         result.box.height = y_max - y_min;

		  //				result.box.x = std::max(0, result.box.x);
		  //				result.box.y = std::max(0, result.box.y);
		  //				result.box.width = std::min(width - result.box.x, result.box.width);
		  //				result.box.height = std::min(height - result.box.y, result.box.height);

 //                         cv::Rect maskImage(cv::Point(x_min, y_min), cv::Point(x_max, y_max));
 //                         result.mask = im(maskImage);
		  //				result.polygon = kps;
		  //				result.kps = keypoints;
 //                         result.extraInfo = keypointString;//Convert keypoint to st;
		  //				result.cameraId = camera_id;
 //                         output.push_back(result);
 //                     }
 //                 }
 //             }
 //             im.release();
 //         }
		  ////EnqueueDetection(output,camera_id);
 //         return output;
 //     }
 //     catch (std::exception& e) {
 //         this->_logger.LogFatal("ANSPOSE::RunInference", e.what(), __FILE__, __LINE__);
 //         return output;
 //     }
 // }