      //USBHIDSpaceNavigator code by mlkoch and DRogers96: https://www.3dconnexion.com/forum/viewtopic.php?t=5642
#include <usbhid.h>
#include <hiduniversal.h>
#include <usbhub.h>
#include <Adafruit_MCP4728.h>
#include <Adafruit_MCP4725.h>
#include <Wire.h>
#include <avr/sleep.h>

Adafruit_MCP4728 mcp;
Adafruit_MCP4725 dac;

//Joysticks: Red/Black = wipers, White = GND, Yellow = Vcc
//Gimbal potentiometer: Black = wiper, Red = Vcc, Yellow = GND

int ZrotVal = 2048;
int XtransVal = 2048;
int YtransVal = 2048;
int ZtransVal = 2048;
int gimbal = 2048;
int direction = 1;

int rotPotPin = A0;
int rotPotVal = 0;
float rotSens = 1;

int latPotPin = A1;
int latPotVal = 0;
float latSens = 1;

bool switchRel = 0;


//-----------------------------------------------------------------------------h file
// Declare variables to hold the data coming from USB, maximum 12 variables plus the muxing byte rptid
struct SpaceNavigatorEventData {
  uint8_t rptid, xl, xh, yl, yh, zl, zh, xr, xs, yr, ys, zr, zs;
};

// Create class (collection of functions) to receive data from USB when: Spacemouse is moved or button pressed
class SpaceNavigatorEvent {
public:
    SpaceNavigatorEvent() { Tx=Ty=Tz=Rx=Ry=Rz=B=0; }; //Constructor, setting all variables to 0
    virtual void OnSpaceNavigatorChanged(const SpaceNavigatorEventData *evt); //This program takes two objects, one constant (the a structure with 13 members) and one pointer called evt

    int16_t Tx, Ty, Tz, Rx, Ry, Rz, B; //Define all the data we want to read from the space mouse (translations and rotations three axises, and button pushes)
};

//Create class to parse(intepret) the USB data. Created as derived class to base class HIDReportParser so that it can inheret it's content (such as USBHID and is_rpt_id)
class SpaceNavigatorReportParser : public HIDReportParser {
    SpaceNavigatorEvent *spaceNavigatorEvent;
    
public:
    SpaceNavigatorReportParser(SpaceNavigatorEvent *evt); //Constructor to take two arguments, another class? And the pointer evt
    
    virtual void Parse(USBHID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf); //This code is inhereted from the HIDReportParser. This part is what actually collects the USB data
};


//-----------------------------------------------------------------------------cpp file
SpaceNavigatorReportParser::SpaceNavigatorReportParser(SpaceNavigatorEvent *evt) : //Colon is a member initialization syntax, with spaceNavigatorEvent taking the initial value e
spaceNavigatorEvent(evt)
{
}

void SpaceNavigatorReportParser::Parse(USBHID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) {

    // Calling SpaceNavigator event handler, using pointer ->
    spaceNavigatorEvent->OnSpaceNavigatorChanged((const SpaceNavigatorEventData*)buf);
}

void SpaceNavigatorEvent::OnSpaceNavigatorChanged(const SpaceNavigatorEventData *evt) {
/*    Serial.print("report ID: ");
    PrintHex<uint8_t > (evt->rptid, 0x80);
    Serial.print("\txl: ");
    PrintHex<uint8_t > (evt->xl, 0x80);
    Serial.print("\txh: ");
    PrintHex<uint8_t > (evt->xh, 0x80);
    Serial.print("\tyl: ");
    PrintHex<uint8_t > (evt->yl, 0x80);
    Serial.print("\tyh: ");
    PrintHex<uint8_t > (evt->yh, 0x80);
    Serial.print("\tzl: ");
    PrintHex<uint8_t > (evt->zl, 0x80);
    Serial.print("\tzh: ");
    PrintHex<uint8_t > (evt->zh, 0x80);
    Serial.print("\txr: ");
    PrintHex<uint8_t > (evt->xr, 0x80);
    Serial.print("\txs: ");
    PrintHex<uint8_t > (evt->xs, 0x80);
    Serial.print("\tyr: ");
    PrintHex<uint8_t > (evt->yr, 0x80);
    Serial.print("\tys: ");
    PrintHex<uint8_t > (evt->ys, 0x80);
    Serial.print("\tzr: ");
    PrintHex<uint8_t > (evt->zr, 0x80);
    Serial.print("\tzs: ");
    PrintHex<uint8_t > (evt->zs, 0x80);*/

    // Translation vector
    if (evt->rptid == 1) {
      Tx = evt->xl+(evt->xh<<8);
      Ty = evt->yl+(evt->yh<<8);
      Tz = evt->zl+(evt->zh<<8);
      //Below data is used for other version of space mouse
      //Rx = evt->xr+(evt->xs<<8);
      //Ry = evt->yr+(evt->ys<<8);
      //Rz = evt->zr+(evt->zs<<8);
    }

    // Rotation vector
    else if (evt->rptid == 2) {
      Rx = evt->xl+(evt->xh<<8);
      Ry = evt->yl+(evt->yh<<8);
      Rz = evt->zl+(evt->zh<<8);
    }

     // Buttons
    else if (evt->rptid == 3) {
      B = evt->xl+(evt->xh<<8);
    }
    
/*    Serial.print("\tT:( ");
    Serial.print(Tx);
    Serial.print(" , ");
    Serial.print(Ty);
    Serial.print(" , ");
    Serial.print(Tz);
    Serial.print(" )");
    Serial.print("\tT:( ");
    Serial.print(Rx);
    Serial.print(" , ");
    Serial.print(Ry);
    Serial.print(" , ");
    Serial.print(Rz);
    Serial.print(" )");
    Serial.print("\tB:( ");
    Serial.print(B);
    Serial.print(" )");    
    Serial.println(""); */
    
  if(B == 1){
    direction = direction*(-1);
  }
  
  rotPotVal = analogRead(rotPotPin);
  rotSens = map(rotPotVal, 0, 1023, 10, 150);
  rotSens = rotSens/100.0;
  
  latPotVal = analogRead(latPotPin);
  latSens = map(latPotVal, 0, 1023, 10, 150);
  latSens = latSens/100.0;

//  Serial.print("rotSens: ");
//  Serial.print(rotSens);
//  Serial.print("  ");
//  Serial.print("latSens: ");
//  Serial.println(latSens);
  
  ZrotVal = map(Rz, -350/rotSens, 350/rotSens, 4095, 0);
  XtransVal = map(Tx, direction*(-350), direction*350, 4095, 0);
  YtransVal = map(Ty, direction*(-350), direction*350, 4095, 0);
  ZtransVal = map(Tz, -350, 350, 4095, 0);
  gimbal = map(Rx, direction*(-350), direction*350, 4095, 0);

  ZrotVal = constrain(ZrotVal, 0, 4095);
  XtransVal = constrain(XtransVal, 0, 4095);
  YtransVal = constrain(YtransVal, 0, 4095);
  ZtransVal = constrain(ZtransVal, 0, 4095);
  gimbal = constrain(gimbal, 0, 4095);

  mcp.setChannelValue(MCP4728_CHANNEL_A, ZrotVal, MCP4728_VREF_VDD, MCP4728_GAIN_1X);
  mcp.setChannelValue(MCP4728_CHANNEL_B, ZtransVal, MCP4728_VREF_VDD, MCP4728_GAIN_1X);
  mcp.setChannelValue(MCP4728_CHANNEL_C, YtransVal, MCP4728_VREF_VDD, MCP4728_GAIN_1X);
  mcp.setChannelValue(MCP4728_CHANNEL_D, XtransVal, MCP4728_VREF_VDD, MCP4728_GAIN_1X);
  dac.setVoltage(gimbal, false);

//  Serial.print("Zrot: ");
//  Serial.print(ZrotVal);
//  Serial.print("Xtrans: ");
//  Serial.print(XtransVal);
//  Serial.print(", Ytrans: ");
//  Serial.print(YtransVal);
//  Serial.print(", Ztrans: ");
//  Serial.print(ZtransVal);
//  Serial.print(", Gimbal: ");
//  Serial.print(gimbal);
//  Serial.print(", rotSens: ");
//  Serial.print(rotSens);
//  Serial.print(", Dir: ");
//  Serial.print(direction);
//  Serial.print(", B: ");
//  Serial.print(B);
//  Serial.println("");
  
}

//What is this part doing? Executing the program?
USB Usb;
USBHub Hub(&Usb);
HIDUniversal Hid(&Usb);
SpaceNavigatorEvent SpaceNavigatorEvent;
SpaceNavigatorReportParser SpaceNavigator(&SpaceNavigatorEvent);



//-----------------------------------------------------------------------------ino file
void setup() {
  Serial.begin(9600);  //  set the serial monitor at 9600 too

    if (!mcp.begin()) {
    Serial.println("Failed to find MCP4728 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MCP4728 Found!");

  //Set default output values to joystick middle position and write to EEPROM
  mcp.setChannelValue(MCP4728_CHANNEL_A, 2048, MCP4728_VREF_VDD, MCP4728_GAIN_1X);
  mcp.setChannelValue(MCP4728_CHANNEL_B, 2048, MCP4728_VREF_VDD, MCP4728_GAIN_1X);
  mcp.setChannelValue(MCP4728_CHANNEL_C, 2048, MCP4728_VREF_VDD, MCP4728_GAIN_1X);
  mcp.setChannelValue(MCP4728_CHANNEL_D, 2048, MCP4728_VREF_VDD, MCP4728_GAIN_1X);
  mcp.saveToEEPROM();
  delay(10);
  
  dac.begin(0x62);
  dac.setVoltage(2048, true);
  delay(10);

  pinMode(5, OUTPUT);
  digitalWrite(5, HIGH);
  Serial.println("Switchrealy HIGH");

//Program wont runt with this active, see https://forum.arduino.cc/index.php?topic=568682.0
//#if !defined(__MIPSEL__)
//  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
//#endif
//  Serial.println("Start");

  if (Usb.Init() == -1)
    Serial.println("OSC did not start.");

  if (!Hid.SetReportParser(0, &SpaceNavigator))
    ErrorMessage<uint8_t > (PSTR("SetReportParser"), 1);
    ;
    
}

void loop() {
  Usb.Task(); 

}