//  V4 2/30/26
//  V5 3/20/26 

// I2C Communication
#include <Wire.h>

// Basic demo for readings from Adafruit BNO08x
#include <Adafruit_BNO08x.h>

// set up IMU
#define BNO08X_RESET -1

struct euler_t {
  float yaw;
  float pitch;
  float roll;
} ypr;

Adafruit_BNO08x  bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;

#define SDA_W1 (D0) //TX
#define SCL_W1 (D1) //RX

#define ADDR_0 (64) //peripheral address
uint8_t state = 0x00;

// CANBUS Communication
#include <Adafruit_MCP2515.h>
#define CS_PIN    (D19)
#define CAN_BAUDRATE (1e6)
#define MTR_ADDR (0x01)
#define MTR_FREQ (100)
#define CAN_LEN (8)

Adafruit_MCP2515 mcp(CS_PIN);

uint8_t rpi_rec[CAN_LEN] = {0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0};
uint8_t can_recv[CAN_LEN] = {0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0};

uint8_t enable_motor[CAN_LEN]  = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFC};
uint8_t disable_motor[CAN_LEN] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFD};
uint8_t zero_motor[CAN_LEN]    = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE};

uint32_t mtr_cmd = 0x0;

uint cur_time = micros();
bool motor_enabled {false};

char x = 0; 

#define i2c_send_LEN 10                  // new
uint8_t i2c_send[i2c_send_LEN];           // new
int16_t roll16 = 0;           // new


void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr) {
    float sqr = qr * qr;
    float sqi = qi * qi;
    float sqj = qj * qj;
    float sqk = qk * qk;

    ypr->yaw   = atan2(2.0 * (qi*qj + qk*qr), (sqi - sqj - sqk + sqr));
    ypr->pitch = asin(-2.0 * (qi*qk - qj*qr) / (sqi + sqj + sqk + sqr));
    ypr->roll  = atan2(2.0 * (qj*qk + qi*qr), (-sqi - sqj + sqk + sqr));

    ypr->yaw   *= RAD_TO_DEG;
    ypr->pitch *= RAD_TO_DEG;
    ypr->roll  *= RAD_TO_DEG;
}

void setReports() {
  if (!bno08x.enableReport(SH2_GAME_ROTATION_VECTOR)) {
    Serial.println("Could not enable game rotation vector");
  }
}

void setup() {
    Wire1.setSDA(SDA_W1);
    Wire1.setSCL(SCL_W1);

    Serial.begin(115200);

    Wire1.begin(ADDR_0);
    Wire1.onRequest(requestEvent);
    Wire1.onReceive(receiveEvent);

    if (!mcp.begin(CAN_BAUDRATE)) {
        while(1) delay(10);
    }

    cur_time = micros();

    // while (!Serial) delay(10);
    delay(100);

    if (!bno08x.begin_I2C(BNO08x_I2CADDR_DEFAULT, &Wire)) {
        while (1) { delay(10); }
    }

    setReports();
    delay(100);
}

void loop() {
    
    delay(10);

    if (bno08x.wasReset()) {
        setReports();
    }

    if (!bno08x.getSensorEvent(&sensorValue)) {
        return;
    }

    switch (sensorValue.sensorId) {
        case SH2_GAME_ROTATION_VECTOR:
            quaternionToEuler(
                sensorValue.un.gameRotationVector.real,
                sensorValue.un.gameRotationVector.i,
                sensorValue.un.gameRotationVector.j,
                sensorValue.un.gameRotationVector.k,
                &ypr
            );

            // roll conversion to 16 bit
            roll16 = (int16_t)((ypr.roll + 180.0) * (65535.0 / 360.0)); 
            

            Serial.print("Yaw: "); Serial.print(ypr.yaw);
            Serial.print(" Pitch: "); Serial.print(ypr.pitch);
            Serial.print(" Roll: "); Serial.println(ypr.roll);
            break;
    }
//end comment here
    int peak_time = micros();
    if ((peak_time - cur_time) > 1e6 / MTR_FREQ) {
        cur_time = peak_time;

        switch(state){
            case 0x00 :
                if (motor_enabled == true) {
                    motor_enabled  = false ;
                    CAN_Send(MTR_ADDR,disable_motor,8);
                }
                break;

            case 0x01:
                if (motor_enabled == false)  {
                    motor_enabled = true ;
                    CAN_Send(MTR_ADDR,enable_motor,8);
                }
                break ;

            case 0x02:
                if(motor_enabled == true)  {
                    CAN_Send(MTR_ADDR, rpi_rec ,8);
                }
                break;

            case 0x03:
                CAN_Send(MTR_ADDR,zero_motor,8);
                break;
        }

        int packetSize = mcp.parsePacket();
        if (packetSize) {
            byte i = 0;
            while (mcp.available()) {
                can_recv[i] = mcp.read();
                i++;
                if(i >= CAN_LEN){
                    break;
                }
            }
        }
    }
}

void CAN_Send(uint32_t cmd_id, uint8_t *data_addr, int len){
    mcp.beginPacket(cmd_id, len, false);
    for(byte i = 0; i < len; i++){
        mcp.write(*(data_addr+i));
    }
    mcp.endPacket();
}

void requestEvent() {

    // sending over 10 bytes (new)h16
    for (int i = 0; i < 8; i++) {
        i2c_send[i] = can_recv[i];   // original data
    }
// adding new roll
    i2c_send[8] = (roll16 >> 8) & 0xFF; //high
    i2c_send[9] =  (roll16) & 0xFF;        //low

    Wire1.write(i2c_send, i2c_send_LEN); 

    Serial.print("Send to Rpi : ");
    for(int i = 0; i < i2c_send_LEN; i++) { 
        Serial.print(i2c_send[i], HEX);
        Serial.print(' ');
    }
}

void receiveEvent(int howMany){
    int i = 0;
    while(Wire1.available()) {
        char c = Wire1.read();
        if (i == 0){
            state = c;
        } else if (i > 0){
            rpi_rec[i-1] = c;
        }
        i++;
    }
}