#pragma once
#include "Wheelchair.h"
#include "StateControl.h"

enum velocityLevels_t {VELOCITY_0, VELOCITY_1, VELOCITY_2, VELOCITY_3, VELOCITY_4, VELOCITY_5};
enum obstacleDir_t {RIGHT_SIDE, RIGHT_CORNER, FRONT, LEFT_CORNER, LEFT_SIDE};

class SharedControl
{
friend class StateControl;
private:
	CWheelchairApp* m_pMainApp;
	StateControl* m_pStateControl; //Pointer to state control class

	int state; //present state of wheelchair
	int correctionCount; //to know if state has been modified by the shared controller
	int wallSide; //right (0) or left (1) wall
	double threshold; //for sonar
	double sideThreshold; //for sonar
	double proximityThreshold; //for IR sensors
	double wallDistance; //distance to wall for detecting it

	int obstacleDir[5]; //to denote presence of obstacles in a given direction
	double velocity[3];	//velocity level
	double sonarSensor[9]; //current sonar sensor readings
	double prevIRSensor[4]; //previous IR sensor readings
	double curIRSensor[4]; //current IR sensor readings

	void SetMode(void); //to handle the three assistance modes
	int VelocityLevel(void); //to detect current speed of the wheelchair
	void CheckObstacleDir(void); //check directions obstructed by objects
	void ResetObstacleDir(void);
	
	//Routines related to obstacle avoidance
	void AvoidObstacle(void); //obstacle avoidance routines
	bool NavigateCorner(void); //negotiate corner in wall following mode
	bool FrontBigObstacle(void); //handle obstacles wider than 50cm or spanning more than 120 degrees to the front of the wheelchair
	bool FrontSmallObstacle(void); //handle obstacles not wider than 30cm to the front of the wheelchair
	bool CornerObstacle(void); //handle all other obstacles as corner obstacles

	//Routine related to Collision Avoidance
	void AvoidCollision(void); //pass on information about risk of collision to StateControl

	//Routines related to Wall Following
	void DetectWall(void); //check for wall to enter wall following mode
	void AutoWallFollow(void); //perform wall following


public:

	bool modeWallFollow; //set if wall is detected

	void SetSensorRange(CString msg); //parse message strings to retrieve sensor information
	SharedControl(CWheelchairApp* mainApp); //constructor
	~SharedControl(void); //destructor

};
