#include "main.h"
#include "pros/serial.hpp"
#include <bitset>

#include <sstream>

// extern "C" int32_t vexGenericSerialReceive( uint32_t index, uint8_t *buffer, int32_t length );
// extern "C" void vexGenericSerialTransmit( uint32_t index, uint8_t *buffer, int32_t length );
// extern "C" void vexGenericSerialWriteChar( uint32_t index, uint8_t chara );
// extern "C" void vexGenericSerialFlush( uint32_t index);
// extern "C" void vexGenericSerialEnable(  uint32_t index, uint32_t nu );
// extern "C" void vexGenericSerialBaudrate(  uint32_t index, uint32_t rate );

/**
 * A callback function for LLEMU's center button.
 *
 * When this callback is fired, it will toggle line 2 of the LCD text between
 * "I was pressed!" and nothing.
 */

#define SERIALPORT 11
// Variable to put the gyro value into
double gyroValue = 0;

// code bleow here works! for recv
// void serialRead(void* params) {
    
//     // Start serial on desired port
//     vexGenericSerialEnable( SERIALPORT - 1, 0 );
    
//     // Set BAUD rate
//     vexGenericSerialBaudrate( SERIALPORT - 1, 9600 );
    
//     // Let VEX OS configure port
//     pros::delay(10);
    
//     // Serial message format:
//     // D[LIDAR DIST]I[IR DATA]A[GYRO ANGLE]E
//     // Example Message:
//     // D50.2I128A12.32E
    
//     while (true) {
        
//         // Buffer to store serial data
//         uint8_t buffer[256];
//         int len = 256;
//         uint8_t data[3];
// 		data[0] = 2;
// 		data[1] = 25;
// 		data[2] = 44;
//         // Get serial data
// 		//vexGenericSerialTransmit(SERIALPORT - 1, data, 3);
// 		vexGenericSerialWriteChar(SERIALPORT - 1, data[2]);
// 		vexGenericSerialFlush(SERIALPORT - 1);
// 		pros::delay(50);
//         int32_t nRead = vexGenericSerialReceive(SERIALPORT - 1, buffer, len);
//         pros::lcd::print(3,"%d", nRead);

//         // Delay to let serial data arrive
//         pros::delay(50);
// 	}
// }

void on_center_button() {
	static bool pressed = false;
	pressed = !pressed;
	if (pressed) {
		pros::lcd::set_text(2, "I was pressed!");
	} else {
		pros::lcd::clear_line(2);
	}
}

/**
 * Runs initialization code. This occurs as soon as the program is started.
 *
 * All other competition modes are blocked by initialize; it is recommended
 * to keep execution time for this mode under a few seconds.
 */
void initialize() {
	pros::lcd::initialize();
	pros::lcd::set_text(1, "Hello PROS User!");

	pros::lcd::register_btn1_cb(on_center_button);
}

/**
 * Runs while the robot is in the disabled state of Field Management System or
 * the VEX Competition Switch, following either autonomous or opcontrol. When
 * the robot is enabled, this task will exit.
 */
void disabled() {}

/**
 * Runs after initialize(), and before autonomous when connected to the Field
 * Management System or the VEX Competition Switch. This is intended for
 * competition-specific initialization routines, such as an autonomous selector
 * on the LCD.
 *
 * This task will exit when the robot is enabled and autonomous or opcontrol
 * starts.
 */
void competition_initialize() {}

/**
 * Runs the user autonomous code. This function will be started in its own task
 * with the default priority and stack size whenever the robot is enabled via
 * the Field Management System or the VEX Competition Switch in the autonomous
 * mode. Alternatively, this function may be called in initialize or opcontrol
 * for non-competition testing purposes.
 *
 * If the robot is disabled or communications is lost, the autonomous task
 * will be stopped. Re-enabling the robot will restart the task, not re-start it
 * from where it left off.
 */
void autonomous() {}

/**
 * Runs the operator control code. This function will be started in its own task
 * with the default priority and stack size whenever the robot is enabled via
 * the Field Management System or the VEX Competition Switch in the operator
 * control mode.
 *
 * If no competition control is connected, this function will run immediately
 * following initialize().
 *
 * If the robot is disabled or communications is lost, the
 * operator control task will be stopped. Re-enabling the robot will restart the
 * task, not resume it from where it left off.
 */
void opcontrol() {
	pros::Controller master(pros::E_CONTROLLER_MASTER);
	pros::Motor left_mtr(1);
	pros::Motor right_mtr(2);
	// Start serial task
    //pros::Task gyroTask (serialRead);
	pros::Serial mySerial(SERIALPORT, 9600);
	mySerial.flush();
	while (true) {
		//pros::lcd::print(4,"%f", gyroValue);
		while (mySerial.get_read_avail() == 0) {
      		pros::delay(1);
    	}
		pros::lcd::print(3, "%x", std::bitset<32>(mySerial.read_byte()));
        mySerial.write_byte(44);
		pros::delay(20);
	}
}