#include <Arduino_BMI270_BMM150.h>
#include <HIDMouse.h>
#include <MadgwickAHRS.h>


#define MOUSE_BUTTON_LEFT     1
#define MOUSE_BUTTON_RIGHT    2
#define MOUSE_BUTTON_MIDDLE   4

HIDMouse bleMouse;
Madgwick filter;

const float DEADZONE = 3.0;

const float SENSITIVITY = 0.20;

const int   MAX_SPEED = 35;

float

gx, gy, gz,

ax, ay, az,

pitch, roll, yaw;

float gx_offset=0;

float gy_offset=0;

int Movx = 0;

int Movy = 0;

const float SENSOR_RATE = 104.0;

float centerPitch;

float centerRoll;

const float NOD_ANGLE        = 18.0;

const float NOD_RETURN_ANGLE = 7.0;

const float NOD_DIRECTION    = -1.0;

const float ROLL_ANGLE        = 18.0;

const float ROLL_RETURN_ANGLE = 7.0;

const float ROLL_DIRECTION    = 1.0;

const unsigned long GESTURE_COOLDOWN = 600;

enum GestureState
{

  GESTURE_NONE,

  CHIN_DOWN,

  CHIN_UP,

  ROLL_RIGHT,

  ROLL_LEFT

};

GestureState activeGesture = GESTURE_NONE;

bool nodClickLocked = false;

bool gestureStarted = false;

bool leftlock       = false;

unsigned long lastNodTime = 0;




float angleDifference(float angle, float reference)
{

  float difference = angle - reference;

  while (difference > 180.0)  difference -= 360.0;

  while (difference < -180.0) difference += 360.0;

  return difference;

}




void setup()
{
  
  Serial.begin(115200);

  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(2,  INPUT_PULLUP);

  pinMode(3,  OUTPUT);

  pinMode(4,  OUTPUT);

  if (!IMU.begin())
    {
      
      Serial.println("IMU Failed");

        while (1);

    }

    filter.begin(SENSOR_RATE);

    bleMouse.setDeviceName("Air Mouse");

    bleMouse.begin();

  for(int i=0;i<500;i++)
  {

    IMU.readGyroscope(gx,gy,gz);

    gx_offset+=gx;

    gy_offset+=gy;

    delay(2);

  }

  gx_offset/=500;

  gy_offset/=500;

}




void detectGesture()
{

  digitalWrite(4, HIGH);
  
  if (nodClickLocked)

    return;

  float pitchChange = angleDifference(pitch, centerPitch) * NOD_DIRECTION;

  float rollChange  = angleDifference(roll, centerRoll) * ROLL_DIRECTION;




  if (activeGesture == GESTURE_NONE)
  {
    
    if (millis() - lastNodTime < GESTURE_COOLDOWN)

      return;

    if (pitchChange > NOD_ANGLE)
    {
    
      activeGesture = CHIN_DOWN;

      Serial.println("Chin down started");
    
    }
    else if (pitchChange < -NOD_ANGLE)
    {

      activeGesture = CHIN_UP;

      Serial.println("Chin up started");

    }
    else if (rollChange > ROLL_ANGLE)
    {

      activeGesture = ROLL_RIGHT;

      Serial.println("Roll right started");

    }
    else if (rollChange < -ROLL_ANGLE)
    {

      activeGesture = ROLL_LEFT;

      Serial.println("Roll left started");

    }

    return;

  }

  uint8_t mouseButton = 0;

  bool gestureComplete = false;




  if (activeGesture == CHIN_DOWN && 
      fabsf(pitchChange) < NOD_RETURN_ANGLE)
  {
  
    mouseButton = MOUSE_BUTTON_LEFT;
  
    gestureComplete = true;
  
    Serial.println("Chin down: Left Click");
  
  }
  else if (activeGesture == CHIN_UP &&  fabsf(pitchChange) < NOD_RETURN_ANGLE)
  {
  
    mouseButton = MOUSE_BUTTON_RIGHT;
  
    gestureComplete = true;
  
    Serial.println("Chin up: Right Click");
  
  }
  else if (activeGesture == ROLL_RIGHT && fabsf(rollChange) < ROLL_RETURN_ANGLE)
  {
  
    mouseButton = MOUSE_BUTTON_MIDDLE;
  
    gestureComplete = true;
  
    Serial.println("Roll right: Middle");
  
  }
  else if (activeGesture == ROLL_LEFT &&  fabsf(rollChange) < ROLL_RETURN_ANGLE)
  {
  



    if (!leftlock)
    {

      bleMouse.press(MOUSE_BUTTON_LEFT);

      leftlock = true;

      Serial.println("Left Locked");

    }
    else
    {

      bleMouse.release(MOUSE_BUTTON_LEFT);
      
      leftlock = false;

      Serial.println("Left Unlocked");

  }
  
  activeGesture = GESTURE_NONE;

  nodClickLocked = true;
  
  lastNodTime = millis();

  }




  if (gestureComplete)
  {
  
    bleMouse.press(mouseButton);
  
    delay(20);
  
    bleMouse.release(mouseButton);

    activeGesture = GESTURE_NONE;
  
    nodClickLocked = true;
  
    lastNodTime = millis();
  
  }

}




void loop()
{

  if (!bleMouse.isConnected())
  {
    
    digitalWrite(LED_BUILTIN, LOW);
    
    digitalWrite(3, LOW);

      bleMouse.release(MOUSE_BUTTON_LEFT);
      
      leftlock = false;

      gestureStarted = false;
      
      activeGesture = GESTURE_NONE;
      
      nodClickLocked = false; 

    delay(5);

    return;

  }
  else
  {

    digitalWrite(3, HIGH);

  }

  digitalWrite(LED_BUILTIN, HIGH);

  IMU.readGyroscope(gx, gy, gz);

  IMU.readAcceleration(ax, ay, az);

  filter.updateIMU(gx, gy, gz, ax, ay, az);

  pitch = filter.getPitch();

  roll  = filter.getRoll();

  yaw   = filter.getYaw();

  Movx = 0;

  Movy = 0;




  if (digitalRead(2) == LOW)
  {




    if (!gestureStarted)
    {

      centerPitch = pitch;

      centerRoll  = roll;

      activeGesture = GESTURE_NONE;

      nodClickLocked = false;

      gestureStarted = true;

      Serial.println("Gesture mode started");

    }

    detectGesture();

  }
  else
  {

    digitalWrite(4, LOW);

    gestureStarted = false;

    activeGesture = GESTURE_NONE;

    nodClickLocked = false;

    gx -= gx_offset;

    gy -= gy_offset;





    if (fabsf(gx) > DEADZONE)
    {
      Movx = gx * SENSITIVITY;
    }




    if (fabsf(gy) > DEADZONE)
    {

      Movy = gy * SENSITIVITY;

    }
    
    Movx = constrain(Movx, -MAX_SPEED, MAX_SPEED);

    Movy = constrain(Movy, -MAX_SPEED, MAX_SPEED);

  }




  if (Movx != 0 || Movy != 0)
  {

    bleMouse.move(Movx, -Movy);
    
  }


  delay(5);

}