// Libraries
#include <Joystick.h>     // https://github.com/MHeironimus/ArduinoJoystickLibrary
#include "HX711-multi.h"  // https://github.com/compugician/HX711-multi
#include <Mouse.h>

// Settings
boolean is_joystick = false;        // true if you want this to be a 2-axis joystick instead of mouse
boolean use_nesw = false;           // if true, vertical axis controlled by NW/SE, horizontal by NE/SW
boolean read_pot_values = true;     // adjust magnitude and mouse speed (gain) with potentiometers
boolean send_serial_output = false; // send data with serial communication

// Time & frame variables
unsigned long dt_frame = 15;      // [ms] duration of frame
unsigned long t_now;              // [ms]
unsigned long t_next;             // [ms]
unsigned long dt_wait;            // [ms] time remaining before wait
unsigned long k;                  // [index] frame number
unsigned long k_click;            // [index] frame of last click

// Pins
#define CLK 9           // clock pin for both load cell amplifiers (LCAs)
#define DOUT1 6         // data pin for the first LCA
#define DOUT2 10        // data pin for the second LCA
const int POT1 = A6;    // potentiometer to adjust magnitude
const int POT2 = A0;    // potentiometer to adjust mouse speed gain
const int CLEFT = 3;    // left-click pin
const int CMID = 5;     // middle-click pin
const int CRIGHT = 7;   // right-click pin

// Mouse commands & movement
const float max_pot = 1023.0;         // max potentiometer value
const float max_mouse = 20;           // max pixels per frame to move mouse
float gain = max_mouse/2.0;           // initial value to move mouse when u = 1
long int diff_max = 200000;           // reject differences greater than this (ignore reading)
float sensitivity = 0.6;              // determines velocity control smoothness and size of deadzone
unsigned long int mag_factor = 6;
unsigned long int magnitude = mag_factor*500*500; // upper boundary to LCA value
int L_click_prev = 0;                 // store value of left click from previous timestep
int M_click_prev = 0;                 // store value of middle click from previous timestep
int R_click_prev = 0;                 // store value of right click from previous timestep

float uXY[2] = {0.0, 0.0};            // mouse command (scaled by gain)
int dXY[2] = {0, 0};                  // mouse pixels to move

// Load cell amplifier (LCA) variables
#define TARE_TIMEOUT_SECONDS 4
byte DOUTS[2] = {DOUT1, DOUT2};
#define CHANNEL_COUNT sizeof(DOUTS)/sizeof(byte)
long int results[CHANNEL_COUNT];
long int NW[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};   // {now, -1, -2, -3, -4... timesteps back}
long int NE[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
long int N[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
long int E[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

// Objects
HX711MULTI scales(CHANNEL_COUNT, DOUTS, CLK);   // HX711 object
Joystick_ JOYSTICK;                             // create joystick object


void tare() {
  bool tareSuccessful = false;
  unsigned long tareStartTime = millis();
  while (!tareSuccessful && millis() < (tareStartTime + TARE_TIMEOUT_SECONDS * 1000)) {
    tareSuccessful = scales.tare(20, 10000);    // reject 'tare' if still ringing
  }
}


void read_load_cells() {

  // Shift old LCA data
  for (int k = 0; k < 9; k++) {
    NW[9 - k] = NW[8 - k];
    NE[9 - k] = NE[8 - k];
    N[9 - k] = N[8 - k];
    E[9 - k] = E[8 - k];
  }

  // Get new LCA data
  scales.read(results);
  NW[0] = results[0];
  NE[0] = results[1];
}


void read_pots() {  
  if (read_pot_values) {
    unsigned long int pot1_val = long(analogRead(POT1));
    magnitude = mag_factor * pot1_val * pot1_val;
    gain = float(max_mouse * (1024.0 - analogRead(POT2)) / (max_pot + 1.0));
  }
}


void process_data() {

  // Reject PREVIOUS NW reading if it seems like a glitch
  // (we can only really check it one timestep later...)
  if (abs(NW[0] - NW[1]) > diff_max) {
    if (abs(NW[1] - NW[2]) > diff_max) {
      NW[1] = NW[2];
    }
  }

  // Reject PREVIOUS NE reading if it seems like a glitch
  if (abs(NE[0] - NE[1]) > diff_max) {
    if (abs(NE[1] - NE[2]) > diff_max) {
      NE[1] = NE[2];
    }
  }

  // Compute N and E values for PREVIOUS timestep
  if (use_nesw) {
    N[1] = NW[1];
    E[1] = NE[1];
  } else {
    N[1] = NE[1] + NW[1];
    E[1] = NE[1] - NW[1];
  }

  // Use Ctrl+Shift+L in Arduino IDE to view real-time plot
  if (send_serial_output) {
    Serial.print(NW[0]);  Serial.print("\t");
    Serial.print(NE[0]);  Serial.print("\t");
    Serial.print(NW[2]);  Serial.print("\t");
    Serial.print(NE[2]);  Serial.print("\t");
//    Serial.print(magnitude);  Serial.print("\t");
//    Serial.print(gain);       Serial.print("\t");
    Serial.print("\n");
  }
}


float LCAval_to_command(long int low_bound, long int lca_val, long int high_bound) {

  // Define deadzone and saturation zones
  float deadzone[2] = {float(low_bound * (1.0 - sensitivity) / 2.0), float(high_bound * (1.0 - sensitivity) / 2.0)};
  float saturation[2] = {float(low_bound * (1.0 + sensitivity) / 2.0), float(high_bound * (1.0 + sensitivity) / 2.0)};
  float slope[2] = {float(-1.0 / (low_bound * sensitivity)), float(1.0 / (high_bound * sensitivity))};
  float u_out = 0.0;

  // Calculate u based on zone
  if (lca_val < saturation[0]) {
    u_out = -1.0;
  } else if (lca_val < deadzone[0]) {
    u_out = float(-1.0 + slope[0] * (lca_val - saturation[0]));
  } else if (lca_val < deadzone[1]) {
    u_out = 0.0;
  } else if (lca_val < saturation[1]) {
    u_out = float(slope[1] * (lca_val - deadzone[1]));
  } else {
    u_out = 1.0;
  }

  return u_out;
}


void mouse_or_joystick() {

  // Compute horizontal and vertical commands (continuous between -1.0 and 1.0)
  uXY[0] = LCAval_to_command(-magnitude, E[1], magnitude);
  uXY[1] = LCAval_to_command(-magnitude, N[1], magnitude);

  if (is_joystick) {
    // Convert to 10-bit command for joystick emulation
    int uXjoy = round(1023 * (uXY[0] + 1.0) / 2.0);
    int uYjoy = round(1023 * (uXY[1] + 1.0) / 2.0);

    JOYSTICK.setXAxis(uXjoy);
    JOYSTICK.setYAxis(uYjoy);

  } else {
    // Convert to pixels to move cursor during this frame (integer)
    dXY[0] = round(gain * uXY[0]);
    dXY[1] = round(gain * uXY[1]);

    // Move the cursor
    if (dXY[0] != 0 || dXY[1] != 0) {
      Mouse.move(dXY[0], -dXY[1], 0);
    }

    // Left click
    if ((digitalRead(CLEFT) == 0) && (L_click_prev == 0)) {
      L_click_prev = 1;               // the button was pressed this time through the loop
      Mouse.press(MOUSE_LEFT);        // press and hold the mouse left-click
    } else if ((digitalRead(CLEFT) == 1) && (L_click_prev == 1)) {
      L_click_prev = 0;               // the button was released this time through the loop
      Mouse.release(MOUSE_LEFT);      // release the mouse left-click
    }

    // Middle click
    if ((digitalRead(CMID) == 0) && (M_click_prev == 0)) {
      M_click_prev = 1;           // the button was pressed this time through the loop
      Mouse.press(MOUSE_MIDDLE);    // press and hold the mouse middle-click
    } else if ((digitalRead(CMID) == 1) && (M_click_prev == 1)) {
      M_click_prev = 0;           // the button was released this time through the loop
      Mouse.release(MOUSE_MIDDLE);  // release the mouse middle-click
    }

    // Right click
    if ((digitalRead(CRIGHT) == 0) && (R_click_prev == 0)) {
      R_click_prev = 1;           // the button was pressed this time through the loop
      Mouse.press(MOUSE_RIGHT);    // press and hold the mouse right-click
    } else if ((digitalRead(CRIGHT) == 1) && (R_click_prev == 1)) {
      R_click_prev = 0;           // the button was released this time through the loop
      Mouse.release(MOUSE_RIGHT);  // release the mouse right-click
    }
  }
}


void wait_for_next_frame() {
  if (millis() < t_next) {
    dt_wait = t_next - millis();
    delay(dt_wait);
  } else {
    dt_wait = 0;
  }

  t_now = millis();
  t_next = t_next + dt_frame;
  k = k + 1;
}


void setup() {
  if (is_joystick) {
    JOYSTICK.begin();       // activate joystick object
  } else {
    pinMode(CLEFT, INPUT_PULLUP);   // set click pins as inputs
    pinMode(CMID, INPUT_PULLUP);
    pinMode(CRIGHT, INPUT_PULLUP);
  }

  if (send_serial_output) {
    Serial.begin(115200);
    Serial.flush();
  }

  tare();
  read_pots();

  t_now = millis();
  t_next = t_now + dt_frame;
  k = 0;
}


void loop() {
  read_pots();
  read_load_cells();
  process_data();
  mouse_or_joystick();
  wait_for_next_frame();
}