// sgn 2022 
// for sgn samd21 board, using 3 encoders, sending data via serial to Processing
// https://forum.arduino.cc/t/rotary-encoder-using-interrupts/469066/10
// got a lot of help on math, process from: http://blog.dzl.dk/2018/08/21/3d-digitizer/

#include "Arduino.h"
#include <Adafruit_NeoPixel.h>

// The below just prepares a whole bunch of variables
// for receving data from pins, leds,
// interrupt handling, sending data, etc. 

// the numbers on this pinout in order
// ENC_A = PinA and PinB : rotary base Encoder
// ENC_B = PinC and PinD : First Arm (mid arm)
// ENC_C = PinE and PinF : Second arm (end effector)

// the below are pin assignments for sgn samd21 board
const byte encoderPinA = 19;  //outputA digital pin19
const byte encoderPinB = 22;  //outoutB digital pin22
const byte encoderPinC = 4;  //outputC digital pin4
const byte encoderPinD = 5;  //outoutD digital pin5
const byte encoderPinE = 9; //outputE digital pin9
const byte encoderPinF = 14; //outoutF digital pin14

// the counts are collecting number of changes in rotary encoders
// there are three encoders, I just made three counts
volatile int count = 0;
int protectedCount = 0;
int previousCount = 0;

volatile int count2 = 0;
int protectedCount2 = 0;
int previousCount2 = 0;

volatile int count3 = 0;
int protectedCount3 = 0;
int previousCount3 = 0;

volatile int encoderCounter;

// Prepare the button
int buttonState = 1;
#define BUTTON_PIN 7

//prepare neopixels and led
#define NEOPIXEL_PIN  27 // sgn SAMD21 Board 
int LED_PIN = 23;  // normal led pin
#define NUMPIXELS 1 // only 1 on board

Adafruit_NeoPixel pixels(NUMPIXELS, NEOPIXEL_PIN, NEO_RGBW + NEO_KHZ800); // RGBW neopixel

// This is looking for data on encoder pins
#define readA digitalRead(encoderPinA)
#define readB digitalRead(encoderPinB)
#define readC digitalRead(encoderPinC)
#define readD digitalRead(encoderPinD)
#define readE digitalRead(encoderPinE)
#define readF digitalRead(encoderPinF)

//length of arms (mm)
#define ARM1 175       // Arm E1-R2 length
#define ARM2 175       // Arm E2-tip length

//Offsets (mm) from mechanical set-up:
#define X_OFFSET -25.0   // Distance from E0 axis to preset position mm
#define Y_OFFSET 25    // Distance from E0 axis to preset position  mm
#define Z_OFFSET 200   // E1 axis height above table mm

//Angles (DEGREES) from mechanical set-up:
#define E0_PRESET 0           // base rotary encoder (encoder A)
#define E1_PRESET -32.048     // first arm (encoder B)
#define E2_PRESET -135.174    // second arm (encoder C)

// in order to convert above into radians for use with 
// everything else here that is in Radians.
float E0_PRESET_RADS = 0;
float E1_PRESET_RADS = 0;
float E2_PRESET_RADS = 0;


#define STATES_PER_REV 4000   // this is how many counts we can expect from encoders

void setup() {
  delay(10);
  SerialUSB.begin(0);   // get USB serial ready
  delay(100);           // give a bit of time to setup USB serial.

  // prepare pins for encoders
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  pinMode(encoderPinC, INPUT);
  pinMode(encoderPinD, INPUT);
  pinMode(encoderPinE, INPUT);
  pinMode(encoderPinF, INPUT);
  delay(20);

  // "attachs" the interrupts in case a pin sees a changing signal
  attachInterrupt(digitalPinToInterrupt(encoderPinA), isrA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinB), isrB, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinC), isrC, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinD), isrD, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinE), isrE, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinF), isrF, CHANGE);

  delay(20);
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  pinMode(LED_PIN, OUTPUT);
  pixels.begin(); // INITIALIZE NeoPixel strip object (REQUIRED)
  pixels.setBrightness(35); // about 1/3 brightness
  pixels.show(); //turn off pixels

}

void loop() {

  noInterrupts();
  protectedCount = count;
  protectedCount2 = count2;
  protectedCount3 = count3;
  interrupts();

  buttonState = digitalRead(BUTTON_PIN); // read button pin  
  
  pixels.clear();  //clear pixels, prepare for next color
  
  if (buttonState == 0) {  // if button pressed, turn on normal LED/neopixel to white
    //digitalWrite(LED_PIN, HIGH);
    pixels.clear();
    pixels.setPixelColor(0, pixels.Color(0, 0, 0, 250));
    pixels.show();   // Send the updated pixel colors to the hardware.
  } else {
    //digitalWrite(LED_PIN, LOW);
    pixels.clear();
    pixels.setPixelColor(0, pixels.Color(240, 0, 80, 0));
    pixels.show();   // Send the updated pixel colors to the hardware.
  }

  
  //read encoders in radians
  // I don't understand why I have to do the encoderCounter = protectedCount 3 times.
  // but it works with it, doesn't without it.
  //encoderCounter = protectedCount;
  double A = radians(protectedCount);   // rotary base encoder
  //encoderCounter = protectedCount2;
  double B = getRadians(protectedCount2);   // first arm
  //encoderCounter = protectedCount3;
  double C = getRadians(protectedCount3);   // second arm

  // get the preset angles (in degrees) converted to radians.
  E0_PRESET_RADS = radians(E0_PRESET); // firsr encoder, rotary base
  E1_PRESET_RADS = radians(E1_PRESET); // second encoder, first arm
  E2_PRESET_RADS = radians(E2_PRESET); // third encoder, second arm

  //calculate distance from E0 axis to tip radius 'r' and height above table 'z'
  // here, A = base rotary encoder, B = first arm, C = second arm
  double r = cos(B + E1_PRESET_RADS) * ARM1 + cos(B + E1_PRESET_RADS + C + E2_PRESET_RADS) * ARM2;
  double z = (sin(B + E1_PRESET_RADS) * ARM1) + (sin(B + E1_PRESET_RADS + C + E2_PRESET_RADS) * ARM2) + Z_OFFSET;

  //Calculate tip x,y positions from center 
  double x = r * cos(A + E0_PRESET_RADS)+ X_OFFSET;
  double y = r * sin(A + E0_PRESET_RADS)+ Y_OFFSET;

  //THIS WAS FOR TESTING. BECAUSE THAT FUCKING getRadians FUNCTION SUCKS!
  // Seriously, why is that thing so broke?
  /*
  SerialUSB.print(E0_PRESET);   // encoder 1 (base)
  SerialUSB.print(", ");
  SerialUSB.print(E0_PRESET_RADS);   // encoder 1 (base)
  SerialUSB.print(", ");
  SerialUSB.print(E1_PRESET);   // encoder 1 (base)
  SerialUSB.print(", ");
  SerialUSB.print(E1_PRESET_RADS);  // encoder 2 (first arm)
  SerialUSB.print(", ");
  SerialUSB.print(E2_PRESET);   // encoder 1 (base)
  SerialUSB.print(", ");
  SerialUSB.print(E2_PRESET_RADS);  // encoder 3 (second arm)
  SerialUSB.print(", ");
  SerialUSB.print(r);                // radius of base encoder
  SerialUSB.print(", ");
  SerialUSB.print(x);                // x position of tip
  SerialUSB.print(", ");
  SerialUSB.print(y);                // y position of tip
  SerialUSB.print(", ");
  SerialUSB.println(z);   
  */     


  // This only sends data if something changes (if arms are moved or button press)
  //send data over USB serial
  if(protectedCount != previousCount || protectedCount2 != previousCount2 || protectedCount3 != previousCount3 || buttonState == 0) {
    // data sent is:    
    SerialUSB.print(protectedCount);   // encoder 1 (base)
    SerialUSB.print(",");
    SerialUSB.print(protectedCount2);  // encoder 2 (first arm)
    SerialUSB.print(",");
    SerialUSB.print(protectedCount3);  // encoder 3 (second arm)
    SerialUSB.print(",");
    SerialUSB.print(buttonState);      // button state
    SerialUSB.print(",");
    SerialUSB.print(r);                // radius of base encoder
    SerialUSB.print(",");
    SerialUSB.print(x);                // x position of tip
    SerialUSB.print(",");
    SerialUSB.print(y);                // y position of tip
    SerialUSB.print(",");
    SerialUSB.println(z);              // z position of tip
    digitalWrite(LED_PIN, HIGH);       //turn on LED
  }

  // This keeps track of the encoders positions
  previousCount = protectedCount;
  previousCount2 = protectedCount2;
  previousCount3 = protectedCount3;

  delay(30);  //so Processing can keep up.

  digitalWrite(LED_PIN, LOW);  // turn off LED
    
  /*
  // More Testing
  SerialUSB.print(A);
  SerialUSB.print(", ");
  SerialUSB.print(B); 
  SerialUSB.print(", ");
  SerialUSB.print(C); 
  SerialUSB.print(", ");
  SerialUSB.print(r); 
  SerialUSB.print(", ");
  SerialUSB.print(x); 
  SerialUSB.print(", ");
  SerialUSB.print(y); 
  SerialUSB.print(", ");
  SerialUSB.println(z);
  */
}

//THESE TWO FUNCTIONS DON"T WORK FOR SOME REASON
//do trig for positions of arms
float getRadians(float degs) {
      return (double)degs * M_PI * 2 / (double)STATES_PER_REV;
    }
    
float getDegrees(float rads)
    {
      return (double)rads * 360.0 / (double)STATES_PER_REV;
    }


// The below are each Interrupt Handler
// Each encoder has 2 pins, and as such, this has 6 handlers for 3 encoders

void isrA() {
  if(readB != readA) {
    count ++;
  } else {
    count --;
  }
}
void isrB() {
  if (readA == readB) {

 count ++;
  } else {
    count --;
  }
}

void isrC() {
  if(readD != readC) {
    count2 ++;
  } else {
    count2 --;
  }
}
void isrD() {
  if (readC == readD) {

 count2 ++;
  } else {
    count2 --;
  }
}

void isrE() {
  if(readF != readE) {
    count3 ++;
  } else {
    count3 --;
  }
}

void isrF() {
  if (readE == readF) {

 count3 ++;
  } else {
    count3 --;
  }
}