BPI-CM9-Linux/aml-comp/vendor/amlogic/aml_commonlib/aml_csi/main.cpp

// Copyright (C) 2024 Amlogic, Inc. All rights reserved.
//
// All information contained herein is Amlogic confidential.
//
// This software is provided to you pursuant to Software License
// Agreement (SLA) with Amlogic Inc ("Amlogic"). This software may be
// used only in accordance with the terms of this agreement.
//
// Redistribution and use in source and binary forms, with or without
// modification is strictly prohibited without prior written permission
// from Amlogic.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <string.h>
#include <semaphore.h>
#include <time.h>

#include <thread>
#include <string>
#include <iostream>
#include <chrono>

#include "alg_lib/include/iva/iva_wisense_ppd.hpp"

extern "C" {
#include "csi_lib/aml_csi.h"
}

static int cnt = 0;
static int motion_status = -1;
static WiSenseIn g_csi_buffer;
static WiSenseIn *g_csi_detail = &g_csi_buffer;

static aai_iva_wisense_ppd_handle_t iva_wisense_ppd_handle;
static aai_iva_wisense_ppd_output_t wi_out_ret;
static aai_iva_wisense_ppd_input_t wi_in;
static aai_iva_wisense_ppd_mlevel_t mlevel;
static aai_iva_wisense_ppd_param_t params;
static std::string g_ctimes = "";

static volatile bool keep_running = true;
static sem_t semaphore;

/*print different colors*/
#define RED     "\033[31m"      /* Red */
#define GREEN   "\033[32m"      /* Green */
#define YELLOW  "\033[33m"      /* Yellow */
#define BLUE    "\033[34m"      /* Blue */
#define MAGENTA "\033[35m"      /* Magenta */
#define CYAN    "\033[36m"      /* Cyan */
#define WHITE   "\033[37m"      /* White */

/*
copy csi to WiSenseIn struct,when get csi about 20 packets, send them to algorithm api
*/
void csi_cb(unsigned char *csi_data, int csi_data_lean) {
	csi_stream *c = (csi_stream *)csi_data;

	if (keep_running) {
		if (cnt < 20) {
			g_csi_detail->time_stamp[cnt] = c->time_stamp;
			g_csi_detail->rssi[cnt][0] = c->rssi[0];
			g_csi_detail->rssi[cnt][1] = c->rssi[1];
			g_csi_detail->bw[cnt] = static_cast<int>((unsigned char)c->bw);
			g_csi_detail->ntx[cnt] = c->ntx;
			g_csi_detail->channel[cnt] = c->channel;
			g_csi_detail->protocol_mode[cnt] = c->protocol_mode;
			memcpy(g_csi_detail->csi[cnt], c->csi, 4 * 1024);

			cnt++;
			if (cnt == 20) {
				sem_post(&semaphore);
			}
		}
	}
}

void csi_data_collection_start(void)
{
	int interval = 50;
	int period = 0;

	csi_enable_debug(0);
	ping_start(NULL, 50);
	csi_register_data_recv_func(NULL, csi_cb);
	csi_set_config(NULL, interval, 1);
	csi_set_filter(0, 0, 0, 0, NULL, NULL,0, NULL);
	csi_start(&period);

	return;
}

void csi_data_collection_end(void)
{
	csi_stop(NULL);
	ping_stop();
}

/*
 * @brief If you want to turn on the light when motion is wisense detected,
 * you can define WISENSE_LED_ACTION macro and specify the gpio num on your board.
 * */
void wisense_led_init(void)
{
#if WISENSE_LED_ACTION
	system("echo 449 > /sys/class/gpio/export");
	system("echo out > /sys/class/gpio/gpio449/direction");
#endif
}

/*
 * @brief If you want to turn on the light when wisense motion is detected,
 * you can define WISENSE_LED_ACTION macro and specify the gpio num on your board.
 * */
void wisense_led_set(int value)
{
#if WISENSE_DEBUG
	std::chrono::milliseconds ms = std::chrono::duration_cast< std::chrono::milliseconds >(
		std::chrono::system_clock::now().time_since_epoch()
	);
	std::cout << ms.count()/100 << std::endl;
#endif

	std::cout << "--->motion status is " << value << std::endl;
#if WISENSE_LED_ACTION
	if (value) {
		system("echo 1 > /sys/class/gpio/gpio449/value"); // Red led
	else
		system("echo 0 > /sys/class/gpio/gpio449/value"); // Green led
#endif
}

void update_current_time(void) {
	auto now = std::chrono::system_clock::now();
	auto now_ms = std::chrono::time_point_cast<std::chrono::milliseconds>(now);
	std::time_t t = std::chrono::system_clock::to_time_t(now_ms);
	std::tm *now_tm = std::localtime(&t);
	char buffer[100];
	std::strftime(buffer, sizeof(buffer), "%Y%m%d%H%M%S", now_tm);
	g_ctimes = buffer;
}

void* wisense_processing_thread(void* arg)
{
	while (keep_running) {
		sem_wait(&semaphore);
		if (cnt >= 20) {
			cnt = 0;
			update_current_time();
			wi_in.wisenseIn = g_csi_detail;
			aai_iva_wisense_ppd_detect(iva_wisense_ppd_handle, wi_in, wi_out_ret, params);
			motion_status = wi_out_ret.motion_status;
			wisense_led_set(motion_status);
			switch (wi_out_ret.motion_class) {
				case 1:
					std::cout << RED << "Motion class is 1. Large human activity in close range[TX/RX] at " << g_ctimes << "\033[0m\n";
					break;
				case 2:
					std::cout << MAGENTA << "Motion class is 2. Human activity at " << g_ctimes << "\033[0m\n";
					break;
				case 3:
					std::cout << GREEN << "Motion class is 3. Human activity at " << g_ctimes << "\033[0m\n";
					break;
				case 4:
					std::cout << GREEN << "Motion class is 4. Lightweight motion. Human activity at " << g_ctimes << "\033[0m\n";
					break;
				case 5:
					std::cout << GREEN << "Motion class is 5. Lightweight motion. Human activity at " << g_ctimes << "\033[0m\n";
					break;
				default:
					std::cout << YELLOW << "Motion class is 0, No person activity detect at " << g_ctimes << "\033[0m\n";
					break;
			}
		}
	}

	return NULL;
}

int main(int argc, char** argv)
{
	int opt;
	int mlevel = 4;

	while ((opt = getopt(argc, argv, "l:")) != -1) {
		switch (opt)
		{
		case 'l':
			mlevel = atoi(optarg);
			break;
		default:
			break;
		}
	}


	sem_init(&semaphore, 0, 0);

	wisense_led_init();
	csi_data_collection_start();

	iva_wisense_ppd_handle = aai_iva_wisense_ppd_init(params);

	if (iva_wisense_ppd_handle != NULL) {
		params.e_mlevel = static_cast<aai_iva_wisense_ppd_mlevel_t>(mlevel);
		aai_iva_wisense_ppd_param_set(iva_wisense_ppd_handle, params);
		std::thread wisense_thread(wisense_processing_thread, nullptr);
		wisense_thread.join();
	}

	csi_data_collection_end();

	if (iva_wisense_ppd_handle != NULL) {
		aai_iva_wisense_ppd_deinit(iva_wisense_ppd_handle);
	}

	sem_destroy(&semaphore);

	return 0;
}