fspycan.h

#ifndef FSPYCAN_H
#define FSPYCAN_H

#include <iostream>
#include <thread> 
#include <mutex>
#include <chrono>
#include <string>
#include <tuple>
#include "fs-ai_api.h"

class FsPyCan
{
private:
    volatile fs_ai_api_vcu2ai vcu2ai_data;
	volatile fs_ai_api_ai2vcu ai2vcu_data;

	fs_ai_api_handshake_send_bit_e getHandshake(const fs_ai_api_vcu2ai data);
	fs_ai_api_mission_status_e getMissionStatus(const fs_ai_api_vcu2ai data);
	fs_ai_api_direction_request_e getDirectionReq(fs_ai_api_mission_status_e mission_status);

	std::thread commandThread;
	volatile bool running = false; 
	std::mutex commandMutex;

	bool as_finished = false;
	bool send_ebs = false ;

public:
	const std::string device;
	const bool debug, simulate;
	const int driving_flag = 1;

	FsPyCan( const std::string &_device, bool _debug, bool _simulate ) : 
		device(_device), debug(_debug), simulate(_simulate) {}
	FsPyCan( const std::string &_device ) : FsPyCan( _device, 1, 1 )  {}
	FsPyCan( const FsPyCan &other ) : FsPyCan( other.device ) {}

	void setupCAN()
	{ 
		
	    set_steering_velocity(0,0);	
		bool signal = true ; 
		while (signal)
		{
			fs_ai_api_vcu2ai_get_data( &vcu2ai_data );

			std::this_thread::sleep_for(std::chrono::milliseconds(4));

			if (ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT==HANDSHAKE_SEND_BIT_ON)	
				ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_OFF;
			else   
				ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_ON;

			ai2vcu_data.AI2VCU_ESTOP_REQUEST = fs_ai_api_estop_request_e::ESTOP_NO;
			ai2vcu_data.AI2VCU_DIRECTION_REQUEST = fs_ai_api_direction_request_e::DIRECTION_NEUTRAL;

			if (vcu2ai_data.VCU2AI_AS_STATE != 0) // step 2
			{
				if (vcu2ai_data.VCU2AI_AMI_STATE != fs_ai_api_ami_state_e::AMI_NOT_SELECTED)
				{//step 3b
					std::cout << "We have selected a mission" << std::endl;
					ai2vcu_data.AI2VCU_MISSION_STATUS = fs_ai_api_mission_status_e::MISSION_SELECTED;//step 4
					std::cout << "We have sent m_1" << std::endl ;	

					if (vcu2ai_data.VCU2AI_AS_STATE == fs_ai_api_as_state_e::AS_READY) // step5
					{
						std::cout << "Orange light is on." << std::endl;
						std::cout<< "AS_STATE == 2, After 5s, please flick grossFunk switch" << std::endl;//step 6
					}
					if(vcu2ai_data.VCU2AI_AS_STATE == fs_ai_api_as_state_e::AS_DRIVING) // step 7
					{
						std::cout << "AS_STATE == 3, finishing up." << std::endl;

						ai2vcu_data.AI2VCU_MISSION_STATUS = fs_ai_api_mission_status_e::MISSION_RUNNING; // 8,9
						ai2vcu_data.AI2VCU_DIRECTION_REQUEST = fs_ai_api_direction_request_e::DIRECTION_FORWARD;//10,11
						
						std::cout << "We have sent m_2 and d_1. Ready to proceed to steering commands" << std::endl ;
						signal = false ;//12
					}
				}	       //step 3
				else std::cout << "Please select mission on side screen" << std::endl ;
			}//step 1										
			else std::cout << "Turn AS switch ON" << std::endl ;

			fs_ai_api_ai2vcu_set_data(&ai2vcu_data);
		}

		if (!signal)
		{
			running = true;
			commandThread = std::thread(&FsPyCan::commandsLoop, this);
			std::cout << "Starting main loop" << std::endl;
		}
	}

	bool exitCAN()
	{ 
		bool signal = true ; 

		running = false ;
		commandThread.join() ;

		while (signal)
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(4));

			fs_ai_api_vcu2ai_get_data( &vcu2ai_data );
			
			if (ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT==HANDSHAKE_SEND_BIT_ON)	
				ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_OFF;
			else   
				ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_ON;
			ai2vcu_data.AI2VCU_ESTOP_REQUEST = fs_ai_api_estop_request_e::ESTOP_NO;

			ai2vcu_data.AI2VCU_DIRECTION_REQUEST = fs_ai_api_direction_request_e::DIRECTION_NEUTRAL;//13
			ai2vcu_data.AI2VCU_MISSION_STATUS = fs_ai_api_mission_status_e::MISSION_FINISHED;//14
			
			if (vcu2ai_data.VCU2AI_AS_STATE == fs_ai_api_as_state_e::AS_FINISHED )//step 15
			{
				signal = false ; 
				std::cout << "We have exited gracefully. Please turn AS switch OFF." << std::endl; //16
			}
			else
				std::cout << "We have sent d_0 and m_5. Waiting for AS_STATE 5." << std::endl;

			fs_ai_api_ai2vcu_set_data(&ai2vcu_data);
		}
	}

	void commandsLoop()
	{
		while (running)
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(4));

			fs_ai_api_vcu2ai_get_data( &vcu2ai_data );

			if (ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT==HANDSHAKE_SEND_BIT_ON)	
				ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_OFF;
			else   
				ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_ON;

			if (! send_ebs)
			{
				ai2vcu_data.AI2VCU_ESTOP_REQUEST = fs_ai_api_estop_request_e::ESTOP_NO;
			}

			ai2vcu_data.AI2VCU_DIRECTION_REQUEST = fs_ai_api_direction_request_e::DIRECTION_FORWARD;

			if (! as_finished)
			{
				ai2vcu_data.AI2VCU_MISSION_STATUS = fs_ai_api_mission_status_e::MISSION_RUNNING;
			}

			//commandMutex.lock();
			fs_ai_api_ai2vcu_set_data(&ai2vcu_data);
			//commandMutex.unlock();
		}
		std::cout << "Exited main loop" << std::endl;
	}

	uint16_t get_left_pulse()
	{
		return vcu2ai_data.VCU2AI_RL_PULSE_COUNT;
	}

	uint16_t get_right_pulse()
	{
		return vcu2ai_data.VCU2AI_RR_PULSE_COUNT;
	}

	float get_yaw()
	{
		return vcu2ai_data.VCU2AI_Yaw_rate_degps;
	}

	void set_AS_Finished()
	{
		std::cout << "Setting AS finished" << std::endl;
		while (1)
		{
			as_finished = true;
			ai2vcu_data.AI2VCU_MISSION_STATUS = fs_ai_api_mission_status_e::MISSION_FINISHED;
			std::cout << "In AS loop" << std::endl;
			if (vcu2ai_data.VCU2AI_AS_STATE == fs_ai_api_as_state_e::AS_FINISHED )//step 15
			{
				std::cout << "AS_FINISHED achieved." << std::endl;
				break;
			}	
		}
	}

	bool checkEBS()
	{
		if (vcu2ai_data.VCU2AI_AS_STATE == fs_ai_api_as_state_e::AS_EMERGENCY_BRAKE)
		{
			std::cout << "Detected emegrency break." << std::endl;
			running = false ; 
			return true ;
		}
		else 
		{
			return false ;
		}
	}

	void deploy_EBS()
	{
		send_ebs = true ;
		while(1)
		{
			ai2vcu_data.AI2VCU_ESTOP_REQUEST = fs_ai_api_estop_request_e::ESTOP_YES;
			if (vcu2ai_data.VCU2AI_AS_STATE == fs_ai_api_as_state_e::AS_EMERGENCY_BRAKE)
			{
				std::cout<< "Emergency break deployed." << std::endl ;
				running = false ;
				std::cout << "Stopping main loop." << std::endl ;
				break ;
			}
		}
	}

	int init()
	{
		int i = fs_ai_api_init( (char*)"can0", 0, 0); //hardcoded, change later
		std::cout << "Init returns" << i << std::endl ;
		return i;
	}

	void set_steering_velocity(int steering, int velocity)
	{
		//std::lock_guard<std::mutex> guard( commandMutex );

		ai2vcu_data.AI2VCU_STEER_ANGLE_REQUEST_deg = steering;
		ai2vcu_data.AI2VCU_WHEEL_SPEED_REQUEST_rpm = velocity;
		
		std::cout << "S: " << steering << std::endl ;
		std::cout << "R: " << velocity << std::endl;
	}
	
};

#endif