#define F_CPU 16000000UL
#define F_SCL 400000UL // 400kHz
#include <avr/io.h>
#include <util/delay.h>
#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include "SerialEvdm.h" //for serial communication and serial port display

// === I2C ===

#define TWBR_VAL ((F_CPU/F_SCL - 16)/2)
#define BNO080_ADDRESS 0x4B
#define MAX_PACKET_SIZE 128
#define MAX_METADATA_SIZE 9
bool sendPacket(uint8_t channelNumber, uint8_t* data, uint8_t dataLength);
bool receivePacket();

void I2C_Init() {
	TWSR = 0;
	TWBR = (uint8_t)TWBR_VAL;
}

bool I2C_Start(uint8_t address, bool read) {
	TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
	while (!(TWCR & (1 << TWINT)));
	if ((TWSR & 0xF8) != 0x08 && (TWSR & 0xF8) != 0x10)
	return false;

	TWDR = (address << 1) | (read ? 1 : 0);
	TWCR = (1 << TWINT) | (1 << TWEN);
	while (!(TWCR & (1 << TWINT)));

	uint8_t status = TWSR & 0xF8;
	return (read ? status == 0x40 : status == 0x18);
}

void I2C_Stop() {
	TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
	while (TWCR & (1 << TWSTO));
}

bool I2C_WriteByte(uint8_t data) {
	TWDR = data;
	TWCR = (1 << TWINT) | (1 << TWEN);
	while (!(TWCR & (1 << TWINT)));
	return (TWSR & 0xF8) == 0x28;
}

uint8_t I2C_ReadByte(bool ack) {
	TWCR = (1 << TWINT) | (1 << TWEN) | (ack ? (1 << TWEA) : 0);
	while (!(TWCR & (1 << TWINT)));
	return TWDR;
}

bool I2C_Write(uint8_t address, uint8_t* data, uint8_t length) {
	if (!I2C_Start(address, false)) return false;
	for (uint8_t i = 0; i < length; i++) {
		if (!I2C_WriteByte(data[i])) {
			I2C_Stop();
			return false;
		}
	}
	I2C_Stop();
	return true;
}

bool I2C_Read(uint8_t address, uint8_t* data, uint8_t length) {
	if (!I2C_Start(address, true)) return false;
	for (uint8_t i = 0; i < length; i++) {
		data[i] = I2C_ReadByte(i < (length - 1));
	}
	I2C_Stop();
	return true;
}

// === BNO080 / SHTP ===
#define CHANNEL_COMMAND  0
#define CHANNEL_EXECUTABLE 1
#define CHANNEL_CONTROL 2
#define CHANNEL_REPORTS 3
#define CHANNEL_WAKE_REPORTS 4
#define CHANNEL_GYRO 5

#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD

#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08

#define FRS_RECORDID_ROTATION_VECTOR 0xE30B

#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD

#define FRS_RECORDID_ROTATION_VECTOR 0xE30B

#define EXECUTABLE_RESET_COMPLETE 0x1

typedef struct {
	uint16_t packetLength;
	uint8_t channelNumber;
	uint8_t sequenceNumber;
} SHTP_Header;

uint8_t shtpData[128];
SHTP_Header shtpHeader;
uint8_t sequenceNumber[6] = {0};

float quatI = 0, quatJ = 0, quatK = 0, quatReal = 0;

float qToFloat(int16_t fixed, uint8_t q) {
	return fixed * powf(2, -q);
}

void softReset (void) {
	
	shtpData[0] = 1;
	sendPacket(CHANNEL_CONTROL, shtpData, 1);

	// Read all incoming data and flush it
	_delay_ms(50);
	while (receivePacket() == true)
	; //delay(1);
	_delay_ms(50);
	while (receivePacket() == true)
	; //delay(1);
	
}

bool begin() {
	
	// BNO080 reset
	softReset();
	_delay_ms(1000);
	
	I2C_Init();
	MyInitSerial();
	_delay_ms(1000);

	PrintString("Init...\n");

	if (!I2C_Start(BNO080_ADDRESS, false)) {
		PrintString("BNO080 not found!\n");
		while (1);
	}
	
	I2C_Stop();
	PrintString("BNO080 detected\n");

	// Clear junk packets
	for (uint8_t i = 0; i < 3; i++) {
		receivePacket();
		_delay_ms(100);
	}
	
	// Request product ID
	shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST;
	shtpData[1] = 0; // Reserved
	sendPacket(CHANNEL_CONTROL, shtpData, 2);
	
	PrintString("Waiting for Product ID Response...\n");
	for (uint8_t i = 0; i < 20; i++) {
		if (receivePacket()) {
			char debugMsg[64];
			snprintf(debugMsg, sizeof(debugMsg), "Channel: %d, ReportID: 0x%02X\n", shtpHeader.channelNumber, shtpData[0]);
			PrintString(debugMsg);
			
			 // Afficher le contenu du paquet reçu
			 PrintString("Received Data: ");
			 PrintHex(shtpData, shtpHeader.packetLength - 4); // Afficher les données en hexadécimal
			 
			// Check for correct response
			if (shtpHeader.channelNumber == CHANNEL_CONTROL && shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) {
				PrintString("Received Product ID Response!\n");
				// Parse and print product info
				uint32_t partNumber = (shtpData[4]) | (shtpData[5] << 8) | (shtpData[6] << 16) | (shtpData[7] << 24);
				PrintString("SW Part Number: ");
				PrintHex((uint8_t*)&partNumber, 4);
				break;
			}
		}
		_delay_ms(200);
	}




	// Wait for response
	if ((receivePacket()) == true) {
		if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) {
			// Display product info (optional, for debugging)
			PrintString("Product ID Response:\n");
			uint32_t SW_Part_Number = ((uint32_t)shtpData[7] << 24) | ((uint32_t)shtpData[6] << 16) |
			((uint32_t)shtpData[5] << 8) | ((uint32_t)shtpData[4]);
			PrintString("SW Part Number: ");
			PrintHex((uint8_t*)&SW_Part_Number, sizeof(SW_Part_Number));
			return true;
		}
	}
	PrintString("not received\n");
	return false; // If the response was not received or is incorrect
}

bool sendPacket(uint8_t channelNumber, uint8_t* data, uint8_t dataLength) {
    uint8_t packetLength = dataLength + 4; // 4 bytes for header

    if (packetLength > MAX_PACKET_SIZE) {
        PrintString("Erreur: packet trop grand\n");
        return false;
    }

    // Démarrer la transmission I2C
    if (!I2C_Start(BNO080_ADDRESS, false)) {
        PrintString("Erreur: I2C_Start a échoué\n");
        return false;
    }

    // Écriture de l'en-tête du paquet
    if (!I2C_WriteByte(packetLength & 0xFF)) return false;           // LSB de la longueur
    if (!I2C_WriteByte((packetLength >> 8) & 0xFF)) return false;    // MSB de la longueur
    if (!I2C_WriteByte(channelNumber)) return false;                 // Numéro de canal
    if (!I2C_WriteByte(sequenceNumber[channelNumber]++)) return false; // Numéro de séquence

    // Écriture des données utilisateur (depuis data[])
    for (uint8_t i = 0; i < dataLength; i++) {
        if (!I2C_WriteByte(data[i])) {
            I2C_Stop();
            PrintString("Erreur: I2C_WriteByte a échoué\n");
            return false;
        }
    }

    I2C_Stop();
    return true;
}


bool receivePacket() {
	uint8_t header[4];

	// Lire l'en-tête (4 octets)
	if (!I2C_Read(BNO080_ADDRESS, header, 4)) return false;
	// Stocker les informations d'en-tête
	shtpHeader.packetLength = ((uint16_t)header[1] << 8) | header[0];
	shtpHeader.channelNumber = header[2];
	shtpHeader.sequenceNumber = header[3];
	
	// Supprimer le bit MSB (continuation bit) s’il est présent
	shtpHeader.packetLength &= ~(1 << 15);
	// Vérifier si le paquet est vide
	if (shtpHeader.packetLength <= 4) return false;
	uint16_t dataLength = shtpHeader.packetLength - 4;

	// Sécurité : ne pas dépasser la taille du buffer
	if (dataLength > sizeof(shtpData)) dataLength = sizeof(shtpData);
	// Lire les données du paquet
	if (!I2C_Read(BNO080_ADDRESS, shtpData, dataLength)) return false;
	// (Optionnel) Vérifier si c'est un "reset complete"
	if (shtpHeader.channelNumber == CHANNEL_EXECUTABLE && shtpData[0] == EXECUTABLE_RESET_COMPLETE) {
		// Tu pourrais ajouter un flag ici
	}
	return true;
}



void enableRotationVector(uint16_t interval_ms) {
	uint32_t interval_us = interval_ms * 1000UL;
	uint8_t command[17] = {
		SHTP_REPORT_SET_FEATURE_COMMAND,
		SENSOR_REPORTID_ROTATION_VECTOR,
		0x00, 0x00, 0x00,
		interval_us & 0xFF,
		(interval_us >> 8) & 0xFF,
		(interval_us >> 16) & 0xFF,
		(interval_us >> 24) & 0xFF,
		0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00
	};
	sendPacket(CHANNEL_CONTROL, command, sizeof(command));
}

void parseRotationVector() {
	if (shtpHeader.channelNumber == CHANNEL_REPORTS &&
	shtpData[0] == SENSOR_REPORTID_ROTATION_VECTOR) {

		// décalage de 5 octets après le reportID
		uint8_t offset = 5;

		int16_t i = (int16_t)(shtpData[offset + 0] | (shtpData[offset + 1] << 8));
		int16_t j = (int16_t)(shtpData[offset + 2] | (shtpData[offset + 3] << 8));
		int16_t k = (int16_t)(shtpData[offset + 4] | (shtpData[offset + 5] << 8));
		int16_t real = (int16_t)(shtpData[offset + 6] | (shtpData[offset + 7] << 8));

		quatI = qToFloat(i, 14);
		quatJ = qToFloat(j, 14);
		quatK = qToFloat(k, 14);
		quatReal = qToFloat(real, 14);
	}
}


void printQuaternion() {
	char buf[64];
	snprintf(buf, sizeof(buf), "Quat: i=%.2f j=%.2f k=%.2f real=%.2f\n", quatI, quatJ, quatK, quatReal);
	PrintString(buf);
}

int main(void) {
	int count = 0;
	
	
	begin();

	enableRotationVector(50); // 50ms = 20Hz

	while (count <10) {
		if (receivePacket()) {
			// Display Channel and Report ID
			char debugMsg[64];
			snprintf(debugMsg, sizeof(debugMsg), "Channel: %d, ReportID: 0x%02X\n", shtpHeader.channelNumber, shtpData[0]);
			PrintString(debugMsg); // Uses your PrintString function to display

			// Display raw data
			PrintString("Data: ");
			PrintHex(shtpData, shtpHeader.packetLength - 4); // Displays the received data in hex

			// If it's a quaternion report, parse it
			parseRotationVector();
			printQuaternion(); // Displays the quaternion values
			count++;
		}
		_delay_ms(10); // Small delay for more responsive reading
	}
}