#include "Arduino.h"
#include <algorithm>					// std::swap
#include <ESP32Servo.h>				// Servo drive support for ESP32
#include "esp32-hal-ledc.h"		// Part of ESP32 board files - Analog output
#include "BluetoothSerial.h"	// Bluetooth enablement - part of ESP or standart Arduino lib
#include <vector>							// support for vectors
#include <string>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#ifndef __CodeRacer_MKII_H__
#define __CodeRacer_MKII_H__

//----- Fun stuff ---------
#define FUN_MIN_PAUSE_MS 120000								// minimum and maximum pause between to rounds fun
#define FUN_MAX_PAUSE_MS 300000
#define LED_SWITCH_MS 50											// speed of knight rider lights
//----- Button ------------
#define H_BUTTON_PIN_LEFT 13
#define H_BUTTON_PIN_RIGHT 16
#define BUTTON_BOUNCING_TIME_MS 200						// bouncing delay
//----- Servo -----
#define H_SERVO_PIN 18								
#define H_SERVO_LEFT_POS 145									// left position of the servo
#define H_SERVO_CENTER_LEFT 100								// left-center position of the servo
#define H_SERVO_RIGHT_POS 35									// right position of the servo
#define H_SERVO_CENTER_RIGHT 80								// right-center position of the servo
#define H_SERVO_CENTER_POS 90									// center position of the servo
#define H_SERVO_SWEEP_LEFT_POS 140						// most left sweep position of the servo
#define H_SERVO_SWEEP_RIGHT_POS 40						// most right sweep position of the servo
#define SERVO_SWEEP_TO_LEFT_STEP 5						// sweep step to the left
#define SERVO_SWEEP_TO_RIGHT_STEP -5					// sweep step to the right
#define SERVO_SWEEP_MS 10											// duration of time betwee to sweep steps
#define SERVO_MAX_POSITION 170								// maximum servo position
#define SERVO_MIN_POSITION 10									// minimum servo position
#define SERVO_SET_1TICK_POSITION_DELAY_MS 3		// minimum duration of time between two servo steps

//----- Ultrasonic sensor -----
#define H_US_TRIG_PIN 21
#define H_US_ECHO_PIN 17
#define H_US_STOP_DISTANCE_CM 25							// if the measured distance is smaller the racer maybe stopped
#define US_MAX_ECHO_TIME_US 6000							// timeout for ultrasonic sensor measurements - this is about 100cm

//----- Drives -----
#define H_DRIVE_RIGHT_SPEED 128							// default speed of right side drive.  0 ...255
#define H_DRIVE_LEFT_SPEED 128								// default speed of left side drive.  0 ...255
#define H_DRIVE_RIGHT_ENABLE_PIN 5
#define H_DRIVE_RIGHT_FWRD_BKWRD_PIN 19
#define H_DRIVE_LEFT_ENABLE_PIN 23
#define H_DRIVE_LEFT_FWRD_BKWRD_PIN 22
#define H_RACER_TURN_LEFT_FOR_MS 400					// duration of time the racer will turn to left
#define H_RACER_TURN_RIGHT_FOR_MS 400					// duration of time the racer will turn to right

#define DRIVE_PWM_LEFT_CHANNEL 5							// PWM-channel for left side drive
#define DRIVE_PWM_RIGHT_CHANNEL 6							// PWM-channel for right side drive

//----- LEDs -----
#define H_LED_FRWD_PIN 27 
#define H_LED_STOP_PIN 25 
#define H_LED_LEFT_PIN 32
#define H_LED_RIGHT_PIN 33
#define H_LED_BLUE_PIN 14
#define H_LED_WHITE_PIN 12


using namespace std;

static volatile bool coderracer_activ = false;
static volatile bool right_button_pressed =false;
static volatile unsigned int right_button_count=0;
static volatile unsigned long left_button_last_pressed_at_ms = millis();
static volatile unsigned long right_button_last_pressed_at_ms = millis();

enum ledstate {
	LEDOFF,
	LEDON
};

enum drivestate {
	DRIVESTOP,
	DRIVEFRWD,
	DRIVEBACK
};

//--- this is as preparation of the class creation
class CodeRacerMKII {

  private:

		//bluetooth
		std::vector<String> _bt_ignoremsgs;
		std::vector<String> _bt_onlymsgs;
		bool _bluetoothcreated;
		unsigned long _bt_stopOnLostConnection_timeout_ms;
		unsigned long _bt_lastmessagereceived;
		BluetoothSerial* _BTSerial;

		//pins
		uint8_t _button_pin_left;
		uint8_t _button_pin_right;
		uint8_t _servo_pin;
		uint8_t _us_trigger_pin;
		uint8_t _us_echo_pin;
		uint8_t _drive_left_frwd_bkwrd_pin;
		uint8_t _drive_left_enable_pin;
		uint8_t _drive_right_frwd_bkwrd_pin;
		uint8_t _drive_right_enable_pin;
		uint8_t _led_frwd_pin;
		uint8_t _led_stop_pin;
		uint8_t _led_left_pin;
		uint8_t _led_right_pin;
		uint8_t _led_blue_pin;
		uint8_t _led_white_pin;

		//servo variables
		int8_t _servo_sweep_step;
		uint8_t _servo_position;
		unsigned long _servo_position_set_at_ms;
		unsigned long _servo_position_eta_in_ms;

		//drives variables
		uint8_t _drive_left_speed;
		uint8_t _drive_right_speed;
		unsigned long _turn_left_for_ms;
		unsigned long _turn_right_for_ms;

		// ultrasonic variables
		bool _coderacer_stopped_at_min_distance;
		bool _coderacer_stop_at_distance_enabled;
		unsigned long _usonic_stop_distance_cm;
		unsigned long _usonic_stop_distance_us;
		unsigned long _usonic_distance_us;
		unsigned long _usonic_distance_cm;

		//fun stuff variables
		unsigned long _last_led_switched_at_ms;
		uint8_t  _led_count;
		uint8_t  _last_led_on;
		unsigned long _servo_look_around_at_ms;

		unsigned long _min_distance_cm;
		bool _drive;
		unsigned long _drive_set_at_ms;
		bool _servo_sweep;
		bool _coderracer_activ;

		//objects
		Servo* _servo;
		Servo* _servo_dummy;

		static void _set_button_state_left();
		static void _set_button_state_right();
		void _analog_write(uint8_t pin, uint8_t speed);
		unsigned long _servo_set_position(uint8_t position);
				
	public:
		//properties
		bool coderacer_fun_enabled;

		uint8_t servo_center_pos;						/**< The position the servo is looking straight forward. Default is 90 . Allowed are values 10<=pos<=170 */
		uint8_t servo_left_pos;							/**< The position the servo is looking to the left. Default is 170 . Allowed are values 10<=pos<=170 */
		uint8_t servo_right_pos;						/**< The position the servo is looking to the right. Default is 0 . Allowed are values 10<=pos<=170 */
		uint8_t servo_sweep_left_pos;				/**< When the servo is sweeping this is the left most position */
		uint8_t servo_sweep_right_pos;			/**< When the servo is sweeping this is the right most position */

		//methods
		CodeRacerMKII();
		
		CodeRacerMKII(uint8_t button_pin_left, uint8_t button_pin_right, uint8_t servo_pin,
			uint8_t us_trigger_pin, uint8_t us_echo_pin,
			uint8_t drive_left_frwd_bkwrd_pin, uint8_t drive_left_enable_pin,
			uint8_t drive_right_frwd_bkwrd_pin, uint8_t drive_right_enable_pin,
			uint8_t led_frwd_pin, uint8_t led_stop_pin, uint8_t led_left_pin, uint8_t led_right_pin, uint8_t led_blue_pin, uint8_t led_white_pin);
		
		void set_inactive();
		void set_active();

		void begin();

		// getters
		bool is_active();
		bool is_driving();
		bool stopped_at_min_distance();
		unsigned long usonic_distance_cm();
		unsigned long usonic_distance_us();
		uint8_t servo_position();
		unsigned long servo_position_set_at_ms();
		unsigned long servo_position_eta_in_ms();
		uint8_t drive_left_speed();
		uint8_t drive_right_speed();
		unsigned long turn_left_for_ms();
		unsigned long turn_right_for_ms();
		bool right_button_state();
		unsigned int button_count();
		

		// higher level {code}racer services
		void stop_driving();
		void drive_forward();
		void drive_forward(uint8_t left_speed, uint8_t right_speed);
		void drive_backward();
		void drive_backward(uint8_t left_speed, uint8_t right_speed);
		void turn_left();
		void turn_left(unsigned long turn_for_ms);
		void turn_left(unsigned long turn_for_ms, uint8_t left_speed, uint8_t right_speed);
		void turn_right();
		void turn_right(unsigned long turn_for_ms);
		void turn_right(unsigned long turn_for_ms, uint8_t left_speed, uint8_t right_speed);

		void start_stop_at_min_distance();
		void start_stop_at_min_distance(unsigned long min_distance_cm);
		void stop_stop_at_min_distance();
		bool start_stop();
		// Bluetooth
		void bt_enable_stopOnLostConnection();
		void bt_enable_stopOnLostConnection(unsigned long timeout);
		void bt_disable_stopOnLostConnection();
		void bt_start(String name);
		String bt_getString();
		String bt_getString(uint8_t delimiterbyte);
		bool bt_msgavailable();
		void bt_addStringToIgnoreList(String stringtoignore);
		void bt_clearIgnoreList();
		void bt_removeStringFromIgnoreList(String stringtoignore);

		// LEDs
		void set_leds_left_stop_frwd_right(ledstate leftled, ledstate stopled, ledstate frwdled, ledstate rightled, ledstate blueled, ledstate whiteled);
		void set_leds_all(ledstate alleds);
		void set_leds_all_off();
		void set_leds_all_on();

		// Drives
		void drives_settings(uint8_t drive_left_speed, uint8_t drive_right_speed, unsigned long turn_left_ms, unsigned long turn_right_ms);
		void set_drives_states_left_right(drivestate stateleft, drivestate stateright);
		void set_drive_left_state(drivestate state);
		void set_drive_right_state(drivestate state);
		void set_drive_state(drivestate state, uint8_t fwrdbackpin, uint8_t enable_pin);
		void set_drives_speed_left_right(uint8_t speedleft, uint8_t speedright);
		void set_drive_left_speed(uint8_t speed);
		void set_drive_right_speed(uint8_t speed);
		void set_drive_speed(uint8_t speed, uint8_t enablepin);
		void set_drives_stop_left_right();

		// Ultrasonic sensor
		unsigned long usonic_measure_cm();
		unsigned long usonic_measure_us();
		unsigned long usonic_measure_cm(unsigned long max_echo_run_time_us);
		unsigned long usonic_measure_us(unsigned long max_echo_run_time_us);
		unsigned long usonic_measure_single_shot_cm();
		unsigned long usonic_measure_single_shot_us();
		unsigned long usonic_measure_single_shot_cm(unsigned long max_echo_run_time_us);
		unsigned long usonic_measure_single_shot_us(unsigned long max_echo_run_time_us);
		void usonic_set_stop_distance_cm(unsigned long stop_distance_cm);
		void usonic_set_stop_distance_us(unsigned long stop_distance_us);

		// Servo drive
		void servo_settings(uint8_t pos_center, uint8_t pos_left, uint8_t pos_right, uint8_t sweep_left_pos, uint8_t sweep_right_pos);
		uint8_t servo_set_position_wait(uint8_t position);
		unsigned long servo_set_position(uint8_t position);
		void servo_set_to_right();
		void servo_set_to_left();
		void servo_set_to_center();
		void servo_sweep();

		// just for fun
		void kitt();
		void look_around();

		

};



#endif