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

#include "RBControl.hpp"

#include <Arduino.h>
//#include "./robot.hpp"

#include <Wire.h>  // must be manually included before #include "Adafruit_TCS34725.h"

#include "Adafruit_TCS34725.h"
Adafruit_TCS34725 adatcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_1X);

#include <thread>
#include <mutex>

std::mutex mtx;

struct GayTector{
    int SDA;
    int SCL;
    int LED;
};
struct Sanic{
    int TRIG;
    int ECHO;
};
/*Robot& get_robot(){
    static Robot robot(168, 34);
    return robot;
}*/

struct Sack{
	int RED;
	int GREEN;
	int BLUE;
};

#define armDelay 1000
#define baseWidth 168
#define wheelRadius 34.
#define motorEncSpin 450.
#define mmToEnc (motorEncSpin / (2. * 3.1415926535 * wheelRadius))

int r, g, b;
int cubeGlobal = -1;
int mm = 0;

int buttons[] = {33, 15};

int assOn = 4;

GayTector col = {23, 22, 5};
Sanic son = {16, 14};
Sack backpack = {0, 0, 0};

constexpr rb::MotorId MotorR = rb::MotorId::M6;
constexpr rb::MotorId MotorL = rb::MotorId::M1;

bool cubeDetect();
inline int clampSpeed(int speed){return speed < -100 ? -100 : speed > 100 ? 100 : speed;}
void milidelay(int ms){std::this_thread::sleep_for(std::chrono::milliseconds(ms));}
inline float lerp(float a, float b, float t){return a + t * (b - a);}
double fsign(double n){ return n ? n < 0 ? -1 : 1 : 0; }
inline int maxColor(){
	int out;
	mtx.lock();
	if( r >= 120 ) out = 0;
	else if( g >= 100 ) out = 1;
	else if( b >= 90 ) out = 2;
	else out = -1;
	mtx.unlock();
	cubeGlobal = out;
	return out;
};

/*


	return r > 170 || g > 90 || b > 80; // 150 | 90 | 95
}

	Soudruh - 186, 50, 40
	Shrunk - 69, 105, 67
	Jew -  73, 95, 85



	Comunist - 194, 50, 46
	Shrek - 75, 107, 75
	Capitalistic - 65, 94, 105
*/
std::string cubeColor(int num){
	switch(num){
	case 0:
		return " RED";
	case 1:
		return " GREEN";
	case 2:
		return " BLUE";
	default:
		return " \e[31mERROR\e[0m";
	}
}

int timeStart = 0;
//# CLOCK (the L is silent)
void clockReset(){
	timeStart = millis();
}
int getTime(){
	return millis() - timeStart;
}

//# Gaydar
void colorPrint(int& r, int& g, int& b){
    if(r!=-1 && g!=-1 && b!=-1)
        std::cout<<"\033[48;2;"<<r<<';'<<g<<';'<<b<<"m   \033[0m"
                <<"\033[38;2;"<<r<<';'<<g<<';'<<b<<"m "
                <</*hex(r,g,b)<<*/" "<<r<<", "<<g<<", "<<b
				<<cubeColor(maxColor())<<"\033[0m"<<std::endl;
}
void getRgbThread(){
	// Wire1.begin(col.SDA, col.SCL, 100000);
	// digitalWrite(col.LED, 1);
	while(true){
		if(!adatcs.begin(TCS34725_ADDRESS, &Wire1)) {
			std::cout << "Can not connect to the RGB sensor" << std::endl;
			r = g = b = -1;
			return;
		}
		// read + send RGB
		float rr = 0, gg = 0, bb = 0;
		adatcs.getRGB(&rr, &gg, &bb);
		mtx.lock();

		r = rr;
		g = gg;
		b = bb;

		mtx.unlock();
		milidelay(200);
		colorPrint(r, g, b);
	}
}
void colorInit(int& SDA, int& SCL, int& LED){
    pinMode(SDA, PULLUP);
	pinMode(SCL, PULLUP);
	pinMode(LED, GPIO_MODE_OUTPUT);
	Wire1.begin(SDA, SCL, 100000);
	if(!adatcs.begin(TCS34725_ADDRESS, &Wire1)){
		std::cout<<"Sensor failed to initialize\n";
	}else{
		std::cout<<"Sensor OK\n";
	}
	digitalWrite(LED, 1);
}
void const getRGB(int& r, int& g, int& b){
	// Wire1.begin(col.SDA, col.SCL, 100000);
	// digitalWrite(col.LED, 1);
	if(!adatcs.begin(TCS34725_ADDRESS, &Wire1)) {
		std::cout << "Can not connect to the RGB sensor" << std::endl;
		r = g = b = -1;
		return;
	}
	// read + send RGB
	float rr = 0, gg = 0, bb = 0;
	adatcs.getRGB(&rr, &gg, &bb);
	
	r = rr;
	g = gg;
	b = bb;
}
std::string hex(int& r, int& g, int& b){
    std::string hex="0123456789ABCDEF", out="#000000";
    out[1]=hex[r>>4]; out[2]=hex[r&15];
    out[3]=hex[g>>4]; out[4]=hex[g&15];
    out[5]=hex[b>>4]; out[6]=hex[b&15];
    return out;
}

//# Sanic
constexpr double SOUND_SPEED = 0.34;  // mm / uS
double const getDistance(int trig, int echo) {
	digitalWrite(trig, LOW);  // reset
	delayMicroseconds(2);
	digitalWrite(trig, HIGH);  // send pulse
	delayMicroseconds(10);
	digitalWrite(trig, LOW);
	int duration = pulseIn(echo, HIGH);  // get duration
	double distance = duration * SOUND_SPEED / 2;
	return distance;
}

//# Butt
void btnsInit(){
    for(auto& b : buttons)
        pinMode(b, INPUT_PULLUP);
}
void btnsPrint(){
    if(!digitalRead(buttons[0])){
        std::cout << "Right Button" << std::endl;
    }
    if(!digitalRead(buttons[1])){
        std::cout << "Left Button" << std::endl;
    }
}
bool btns(){
	return (!digitalRead(buttons[0]) && !digitalRead(buttons[1]));
}

//# Paws
/*
    # Beans
    *Štartýren - 140
    *Opium - 115
    *Copium - 150
*/
void pawArmInit(){
	rb::Manager& man = rb::Manager::get();
    man.initSmartServoBus(2, GPIO_NUM_32);
    rb::Angle a0 = rb::Angle::deg(40);
    man.servoBus().set(0, a0);
    rb::Angle a1 = rb::Angle::deg(140);
    man.servoBus().set(1, a1);
    delay(armDelay);
}
void armDown(){
	rb::Manager& man = rb::Manager::get();
    rb::Angle ang = rb::Angle::deg(40);
    man.servoBus().set(0, ang);
    delay(armDelay);
}
void armUp(){
	rb::Manager& man = rb::Manager::get();
    rb::Angle ang = rb::Angle::deg(69);
    man.servoBus().set(0, ang);
    delay(armDelay);
}
void pawsOpen(){
	rb::Manager& man = rb::Manager::get();
    rb::Angle ang = rb::Angle::deg(115);
    man.servoBus().set(1, ang);
    delay(armDelay);
}
void pawsClose(){
	rb::Manager& man = rb::Manager::get();
    rb::Angle ang = rb::Angle::deg(150);
    man.servoBus().set(1, ang);
    delay(armDelay);
}
void pawArmAutoStop(){
    rb::Manager& man = rb::Manager::get();
    man.servoBus().setAutoStop(0);
    man.servoBus().setAutoStop(1);
}

//# Špinny Špin
int getEncoder(rb::MotorId id){
	rb::Manager& man = rb::Manager::get();
	switch(id){
		case MotorL: return (man.motor(MotorL).encoder()->value());
		case MotorR: return (-man.motor(MotorR).encoder()->value());
		default:
			std::cout << "motor not found" << std::endl;
			return 0;
	}
}
void const setSpeeds(int speed){
	rb::Manager& man = rb::Manager::get();
	man.motor(MotorL).power(speed);
	man.motor(MotorR).power(-speed);
}
void const setSpeeds(int speedL, int speedR){
	rb::Manager& man = rb::Manager::get();
	man.motor(MotorL).power(clampSpeed(speedL));
	man.motor(MotorR).power(clampSpeed(-speedR));
}
void const drive(int dis, int speed){
	int startL = getEncoder(MotorL);
	int startR = getEncoder(MotorR);
	int leftEnc = 0;
	int rightEnc = 0;
	while(abs(dis * mmToEnc) > abs(leftEnc)) {
		leftEnc = getEncoder(MotorL) - startL;
		rightEnc = getEncoder(MotorR) - startR;
		int diff = (rightEnc - leftEnc);// /4;
		setSpeeds(speed + diff, speed - diff);
		
		milidelay(10);
	}
	setSpeeds(0);
}
void const driveCollect(int dis, int speed){
	int startL = getEncoder(MotorL);
	int startR = getEncoder(MotorR);
	int leftEnc = 0;
	int rightEnc = 0;
	mm = 0;
	while(mm < dis && !cubeDetect()) {
		leftEnc = getEncoder(MotorL) - startL;
		rightEnc = getEncoder(MotorR) - startR;
		int diff = (rightEnc - leftEnc);// /4;
		setSpeeds(speed + diff, speed - diff);
		mm = abs(leftEnc) / mmToEnc;
		milidelay(10);
	}
	setSpeeds(0);
	drive(20, speed);
	milidelay(300);
	pawsClose();
	milidelay(333);
	armUp();
	milidelay(333);
	drive(mm, -speed);
	milidelay(333);
}
void const driveFancy(int dis, int startSpeed, int endSpeed){
	int startL = getEncoder(MotorL);
	int startR = getEncoder(MotorR);
	int leftEnc = 0;
	int rightEnc = 0;
	int speed = startSpeed;
	while(abs(dis * mmToEnc) > abs(leftEnc)) {
		leftEnc = getEncoder(MotorL) - startL;
		rightEnc = getEncoder(MotorR) - startR;
		int diff = (rightEnc - leftEnc)/4;
		speed = lerp(startSpeed, endSpeed, abs(leftEnc) / (dis * mmToEnc));
		setSpeeds(speed + diff, speed - diff);
		
		milidelay(10);
	}
	setSpeeds(0);
}
void const driveSans(int dis, int speed){
	int startL = getEncoder(MotorL);
	int startR = getEncoder(MotorR);
	int leftEnc = 0;
	int rightEnc = 0;
	while(getDistance(son.TRIG, son.ECHO)>dis){
		leftEnc = getEncoder(MotorL) - startL;
		rightEnc = getEncoder(MotorR) - startR;
		int diff = (rightEnc - leftEnc)/4;
		setSpeeds(speed + diff, +speed - diff);
		milidelay(10);
	}
}
void const back(int speed, int timeout = 5){
	int startL = getEncoder(MotorL);
	int startR = getEncoder(MotorR);
	int leftEnc = 0;
	int rightEnc = 0;
	int time = 0;
	while(digitalRead(buttons[0]) == 1 && digitalRead(buttons[1])) {
		leftEnc = getEncoder(MotorL) - startL;
		rightEnc = getEncoder(MotorR) - startR;
		int diff = (rightEnc - leftEnc)/4;
		setSpeeds(-speed + diff, -speed - diff);
		time++;
		if(time > timeout*100) break;
		milidelay(10);
	}
}
void const front(int speed, int timeout = 1){
	int startL = getEncoder(MotorL);
	int startR = getEncoder(MotorR);
	int leftEnc = 0;
	int rightEnc = 0;
	int time = 0;
	while(digitalRead(buttons[0]) == 1 && digitalRead(buttons[1])) {
		leftEnc = getEncoder(MotorL) - startL;
		rightEnc = getEncoder(MotorR) - startR;
		int diff = (rightEnc - leftEnc)/4;
		setSpeeds(speed + diff, speed - diff);
		time++;
		if(time > timeout*100) break;
		milidelay(10);
	}
}
void const turn(double angle, int speed, int turnRadius = 0){
	angle = angle * PI / 180;
	int startL = getEncoder(MotorL);
	int startR = getEncoder(MotorR);
	double disl = (turnRadius + fsign(angle) * baseWidth / 2.) * fabs(angle);
	double disr = (turnRadius - fsign(angle) * baseWidth / 2.) * fabs(angle);

    //std::cout << "dis " << disl * mmToEnc << " " << disr * mmToEnc << std::endl;
    //std::cout << "angle " << angle << " " << fsign(angle) << std::endl;
	
	int leftEnc = 0;
	int rightEnc = 0;
	while(true){
		leftEnc = getEncoder(MotorL) - startL;
		rightEnc = getEncoder(MotorR) - startR;
        // std::cout << "dis " << disl * mmToEnc << " " << disr * mmToEnc << std::endl;
        // std::cout << "enc " << leftEnc << " " << rightEnc << std::endl;
        double percl = fabs((double)leftEnc / (disl * mmToEnc));
        double percr = fabs((double)rightEnc / (disr * mmToEnc));
        double diff = (percl - percr) * 20;
        setSpeeds(fsign(disl) * speed * (1-diff), fsign(disr) * speed * (1+diff));
        if(abs(disl * mmToEnc) <= abs(leftEnc)) break;
		milidelay(10);
	}
	setSpeeds(0);
}

//# CUBE
bool cubeDetect(){
	maxColor();
	mtx.lock();
	bool result = (r >= 120 || g >= 100 || b >= 90);
	mtx.unlock();
	return result; // 150 | 90 | 95
}
void sortCubeDeposit(){
	turn(-90, 50);
	front(50, 2);
	pawsOpen();
	milidelay(200);
	drive(200, -60);
	pawsClose();
	milidelay(200);
	armDown();
}
void sortCubeRed(){
	driveFancy(425, 40, 90);
	driveFancy(425, 90, 40);
	drive(45*(backpack.RED%3), 40);
	sortCubeDeposit();
	turn(-90, 50);
	milidelay(100);
	back(50);
	milidelay(300);
	drive(200, 30);
	milidelay(300);
	turn(180, 30);
	milidelay(300);
	back(50, 7);
	milidelay(333);
	backpack.RED++;
}
void sortCubeGreen(){
	driveFancy(200, 40, 90);
	driveFancy(200, 90, 40);
	drive(45*(backpack.GREEN%3), 40);
	sortCubeDeposit();
	turn(90, 50);
	milidelay(300);
	back(50);
	milidelay(300);
	backpack.GREEN++;
}
void sortCubeBlue(){
	drive(45*(backpack.BLUE%3), 40);
	sortCubeDeposit();
	turn(90, 50);
	back(50);
	milidelay(300);
	backpack.BLUE++;
}
void sortCube(int cube){
	armUp();
	switch(cube){
		case 0:
			sortCubeRed();
			break;
		case 1:
			sortCubeGreen();
			break;
		case 2:
			sortCubeBlue();
			break;
		default:
			armDown();
			break;
	}
}
void checkCube(){
	armDown();
	milidelay(200);
	pawsOpen();
	milidelay(200);
	drive(100, 30);
	milidelay(200);
	pawsClose();

	// cubeGlobal = maxColor();
	// if(cubeGlobal != -1)
		sortCube(cubeGlobal);
		cubeGlobal = -1;
	// else
	// 	back(50);
}





void innitInnit(){
	rb::Manager& man = rb::Manager::get();
    man.install(rb::ManagerInstallFlags::MAN_DISABLE_MOTOR_FAILSAFE);

	pinMode(assOn, INPUT);

    // Sanic
    pinMode(son.TRIG, OUTPUT);
    pinMode(son.ECHO, INPUT);

    // Gaydar
    colorInit(col.SDA, col.SCL, col.LED);

    // Butt
    btnsInit();

    // Paw + Arm
    pawArmInit();
}

void mainSS(){
	driveFancy(650, 40, 90);
	turn(-180, 90, 250);
	turn(180, 90, 250);
	driveFancy(500, 90, 60);
	driveFancy(200, 60, 40);
	milidelay(350);
	turn(45, 30);
	milidelay(300);
	turn(-90, 30);
	milidelay(300);
	turn(45, 30);
	milidelay(300);
}
void toysPrep(){
	pawsOpen();
	milidelay(300);
	driveFancy(550, 50, 90);
	if(cubeDetect())
		pawsClose();

	turn(45, 30);
	milidelay(325);
	turn(-135, 30);
	milidelay(325);
	turn(90, 30);
	milidelay(325);

	driveFancy(250, 50, 90);
	milidelay(325);

	if(cubeDetect())
		pawsClose();

	armUp();
	front(80, 2);
	milidelay(300);
	drive(200, -70);
	milidelay(300);
	turn(90, 30);
	milidelay(300);
	back(70);
	if(cubeDetect()) checkCube();
	armDown();
}
void eating(int cube){
	for(int i = 0; i < cube; i++){
		int dis = 350 + 88 * (i%8);
		driveFancy(dis, 40, 90);
		turn(90, 30);
		milidelay(200);

		pawsOpen();
		armDown();
		milidelay(200);
		driveCollect(750, 40);

		turn(-90, 30);
		milidelay(200);
		back(50);
		milidelay(200);
		if(getTime() > 4.2 * 60 * 1000){ // min * sec * mil (in mills)
			i = cube;
			continue;
		}
		checkCube();
		if(getTime() > 4.2 * 60 * 1000){ // min * sec * mil (in mills)
			i = cube;
			continue;
		}
	}
}
void goToDen(){
	drive(200, 40);
	turn(90, 50);
	milidelay(500);
	driveFancy(450, 50, 90);
	// drive(1600, 90);
	drive(950, 90);
	turn(-90, 30);
	milidelay(300);
	back(50);

	drive(200, 40);
	turn(90, 50);
	milidelay(500);
	drive(650, 90);
	milidelay(450);
	turn(-90, 30);
	milidelay(450);
	back(80);
	milidelay(500);
	drive(500, 80);
	turn(-90, 80, 230);
	drive(100, 100);
	turn(180, 90, 250);
	drive(10000, 100);
}




//Serial.begin (115200);
void setup(){
	// rb::Manager& man = rb::Manager::get();
    // man.initSmartServoBus(2, GPIO_NUM_32);
	innitInnit();
	if(!adatcs.begin(TCS34725_ADDRESS, &Wire1)){
		std::cout<<"Sensor failed to initialize\n";
	}else{
		std::cout<<"Sensor OK\n";
	}
	std::thread t(getRgbThread);

	// pawsOpen();

	while(digitalRead(assOn)){
		// colorPrint(r, g, b);
		// std::cout << ":D" << std::endl;
	}

	milidelay(500);

	clockReset();

	// pawsOpen();
	// driveCollect(2000, 30);

	back(80);
	mainSS();
	toysPrep();/*
	if(!stop){
		back(30);
		steal();
	}*/
	eating(60);
	goToDen();

	// while(digitalRead(buttons[0])){

    // 	colorPrint(r, g, b);
    // delay(500);
	// }


	//milidelay(200);
	// turn(45, 50);
	// milidelay(200);
	// turn(-90, 50);
	// milidelay(200);
	// turn(45, 50);
	// milidelay(200);

    //armUp();
    //pawsOpen();
	//setSpeeds(50);
    //delay(2000);
    //setSpeeds(0);
    //driveFancy(300, 20, 100);
    //back(50, 99999);
    //turn(90, 50, 0);

	t.detach();
}

void loop(){
	// std::thread t(getRgbThread);
	
	//rb::Manager& man = rb::Manager::get();
    // man.install(rb::ManagerInstallFlags::MAN_DISABLE_MOTOR_FAILSAFE);
/*
    armDown();
    pawsClose();
    armUp();
    pawsOpen();*/

	// mtx.lock();
    // colorPrint(r, g, b);
    // mtx.unlock();
	if(!digitalRead(buttons[0])){
		armUp();
	}else{
		armDown();
	}
	if(!digitalRead(buttons[1])){
		pawsOpen();
	}else{
		pawsClose();
	}

	// delay(1);
    // btnsPrint();
    // std::cout << "Angle: " << man.servoBus().pos(1).deg() << std::endl;
    
    // std::cout << man.servoBus().pos(0).deg() << std::endl;
    // std::cout << man.servoBus().pos(1).deg() << std::endl;
    // std::cout << getDistance(son.TRIG, son.ECHO) << " mm" << std::endl;
    // getRGB(r, g, b);
    // colorPrint(r, g, b);
    /*printf("GetId: %d %f\n", 0, servoBus.posOffline(0).deg());
    printf("GetId: %d %f\n", 1, servoBus.posOffline(1).deg());
*/
    // for(int i = 0; i < 2; ++i){
    //     try{
    //         float deg = man.servoBus().pos(i).deg();
    //         std::cout << i << ": " << deg << "\t";
    //     }catch(...){
    //         std::cout << "Servo " << i << " not responding" << std::endl;
    //     }
	// 	if(i == 1) std::cout  << std::endl;
    // }

    delay(500);
}