//
// Created by nano on 4/26/25.
//

#include <stdlib.h>

#include "utils.h"

#include <stdio.h>

#include "communication.h"

#define ACK 16
#define NOTHING_RECV 0

/**Helper functions to convert from bits to bytes*/
void byte_to_bit_array(byte bit_array[], byte data) {
	for (int i = 0; i < 8; i++) {
		bit_array[i] = (data >> i) & 1; //shift through all bits and check if they're 1
	}
}

/**2's complement notation*/
void bit_array_to_byte(const byte bit_array[], byte *data) {
	*data = 0;
	byte currentpow = 1;
	for (int i = 0; i < 8; i++) {
		*data += bit_array[i] * currentpow;
		currentpow *= 2;
	}
}

void comm_write_until_successful(byte d, int timeout) {
	//while (comm_await_comm_awaready_for_tx(50)!=SP_OK) {} //wait indefinitely till you can send data
	REWRITE:
	comm_blocking_write(&d,1,timeout);

	byte data;
	int counter =0;
	REREAD:
	while (comm_blocking_read(&data,1,timeout) != ALL_BYTES) {counter ++; if (counter == 10) goto REWRITE;} //idk if this does anything but the code works so im not touching it

	//data ? printf("RECV: %d \n",data) : 0;
	counter++;
	printf("Times Read %d",counter);
	if (data == ACK) {
		printf("ACK\n");
	}
	else if (counter !=10)
		goto REREAD;
	else
		goto REWRITE;
}

#define CROSSING 0x20

/**
 * await data from the robot
 * @param data data gotten from robot
 * @return 1 if correct, -1 if retry required
 */
int comm_await_crossing(int timeout){
	byte data;
	int run =1;
	printf("AWAITING CROSSING\n");
	while (run){
		comm_blocking_read(&data, 1,timeout);
		printf("Read while expecting crossing: %d\n",data);
		if (data==CROSSING) {
			//printf("CROSSING\n");
			run=0;
			return 1;
			break;
		}
	}
	return -1;
}

#define SENSOR_NOTHING_AHEAD 5
#define SENSOR_1_CROSSING_AHEAD 1
#define SENSOR_2_CROSSING_AHEAD 2
#define SENSOR_3_CROSSING_AHEAD 3
#define SENSOR_4_CROSSING_AHEAD 4

int comm_get_distance_sensor(int *len, int timeout) {
	byte *data = malloc(sizeof(byte));
	while (comm_blocking_read(data,1,timeout) == ALL_BYTES) {
		printf("Got while expecting distance sensor: %d \n", *data);
		switch (*data) {
			case SENSOR_NOTHING_AHEAD:
				*len=0;
				free(data);
				return 1;
				break;
			case SENSOR_1_CROSSING_AHEAD:
				*len=1;
				free(data);
				return 1;
				break;
			case SENSOR_2_CROSSING_AHEAD:
				*len=3;
				free(data);
				return 1;
				break;
			case SENSOR_3_CROSSING_AHEAD:
				*len=5;
				free(data);
				return 1;
				break;
			case SENSOR_4_CROSSING_AHEAD:
				*len=7;
				free(data);
				return 1;
				break;
			default:
				printf("wat\n");
				break;
		}
	}
	free(data);
	return 0;
}