//*************************************************
//  Demo code for 3D digitizer
//  See description of project here:
//
//  (F)Dzl 2018
//
//  https://blog.dzl.dk/2018/08/21/3d-digitizer/
//
//*************************************************

//#define OLED
#define serial_print_data 0  //serial print readable data


#ifdef OLED
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH        128 // OLED display width, in pixels
#define SCREEN_HEIGHT        64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET            4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
#endif

#define store_button_pin 5
bool store_button = false;

#include "dzl_encoders.h"

IQencoder E0, E1, E2, E3;

//*************************************************
//  Mechanical set up:
//*************************************************
#define ARM1 176.38       // Arm E1-R2 length[mm.]
#define ARM2 176.38       // Arm E2-tip length[mm.]
//Offsets from mechanical set-up:
#define Z_OFFSET 80.782   // E1 axis height above table [mm.]
#define X_OFFSET -125.0   // Distance from E0 axis to preset position [mm.]
#define Y_OFFSET 125.0    // Distance from E0 axis to preset position [mm.]
//Angles from mechanical set-up:
#define E0_PRESET -45.0
#define E1_PRESET 32.048
#define E2_PRESET -113.174
#define E3_PRESET 0.0

//*************************************************
//  Send coordinates to processing program
//*************************************************
void sendFloat(float f, unsigned t) {
  byte * b = (byte *) &f;
  Serial.write(t);
  Serial.write(b[0]);
  Serial.write(b[1]);
  Serial.write(b[2]);
  Serial.write(b[3]);
}
//=====================================================================================
void setup() {
  Serial.begin(19200);

  pinMode(store_button_pin, INPUT_PULLUP);

#ifdef OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  } else {
    Serial.println(F("SSD1306 allocated"));
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(2, 2);
  display.print(F("XYZ"));
  display.display();
  delay(2000);
#endif
  setEncoderRate(10000);

  //Attach encoders so anti-clockwise rotation is positive:
  E0.attach(9, 8);      //E0 (Q,I)
  E1.attach(11, 10);    //E1 (I,Q)
  E2.attach(13, 12);    //E2 (I,Q)
  E3.attach(7, 6);      //tt (Q,I)

  delay(10);            //-Allow time to settle

  //Preset encoders:
  E0.setDegrees(E0_PRESET);     //Horizontal encoder (corner)
  E1.setDegrees(E1_PRESET);     //First vertical encoder
  E2.setDegrees(E2_PRESET);     //Second vertical encoder
  E3.setDegrees(E3_PRESET);     //Turntable
}
//=====================================================================================
void loop() {

  store_button = !digitalRead(store_button_pin);

  //Read encoders in radians
  double A = E1.getRadians();
  double B = E2.getRadians();
  double C = E0.getRadians();
  double D = E3.getRadians();

  //Calculate distance from E0 axis to tip 'r' and height above table 'z'
  double r = cos(A) * ARM1 + cos(A + B) * ARM2;
  double z = (sin(A) * ARM1) + (sin(A + B) * ARM2) + Z_OFFSET;

  //Calculate tip x,y
  double x = r * cos(C) + X_OFFSET;
  double y = r * sin(C) + Y_OFFSET;

  /*
    //Print encoder angles:
    Serial.print(E0.getDegrees());
    Serial.print(",");
    Serial.print(E1.getDegrees());
    Serial.print(",");
    Serial.print(E2.getDegrees());
    Serial.print(",");
    Serial.println(E3.getDegrees());
  */

#if serial_print_data ==1
  //Print coordinates:
  Serial.print(x);
  Serial.print(",");
  Serial.print(y);
  Serial.print(",");
  Serial.print(z);
  Serial.print(",");
  Serial.println(D);
#else

  /*
    //Send coordinates to Processing
    sendFloat(x, 'x');
    sendFloat(y, 'y');
    sendFloat(z, 'z');
    sendFloat(D, 'a');
  */
#endif
  delay(100);

}
//=====================================================================================