// Custom chips playground
// See https://link.wokwi.com/custom-chips-alpha for more info


#include <Adafruit_GFX.h>        // OLED Library
#include <Adafruit_SSD1306.h>    // OLED Library
#include <Wire.h>                // I2C  Library

/*
#include "MAX30105.h"            // MAX3010x library
#include "heartRate.h"           // Heart rate calculating algorithm

MAX30105 particleSensor;

const byte RATE_SIZE = 4;        // Increase this for more averaging. 4 is good.
byte rates[RATE_SIZE];           // Array of heart rates
byte rateSpot = 0;
long lastBeat = 0;               // Time at which the last beat occurred
float beatsPerMinute;
int beatAvg;
*/

#define SCREEN_WIDTH 128         // OLED display width, in pixels
#define SCREEN_HEIGHT 64         // OLED display height, in pixels
#define OLED_RESET    -1         // Reset pin # (or -1 if sharing Arduino reset pin)

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); //Declaring the display name (display)

byte data_buf[16];
int i, s,
    Ix[4], 
    Iy[4],
    positionX[4];
    positionY[4]

void Write_2bytes();
void calculate_pos();

void setup() {
  Serial.begin(9600);
  Wire.setClock(400000);                         // Set I2C speed to 400kHz
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);     // Start the OLED display
  display.setTextColor(WHITE);
  Wire.begin(); 

/*
  // Initialize sensor
  particleSensor.begin(Wire, I2C_SPEED_FAST);    // Use default I2C port, 400kHz speed
  particleSensor.setup();                        // Configure sensor with default settings
  particleSensor.setPulseAmplitudeRed(0x0A);     // Turn Red LED to low to indicate sensor is running
*/
  display.clearDisplay();
  display.setTextSize(3);
  display.setCursor(0, 20);


  display.drawPixel(32,35,1);
  display.drawPixel(64,15,1);
  display.display();

  // IR sensor initialize
  Write_2bytes(0x30,0x01), delay(10);
  Write_2bytes(0x30,0x08), delay(10);
  Write_2bytes(0x06,0x90), delay(10);
  Write_2bytes(0x08,0xc0), delay(10);
  Write_2bytes(0x1A,0x40), delay(10);
  Write_2bytes(0x33,0x33), delay(10);
  
  delay(100);

  // halt script
  //for (;;);
}

void Write_2bytes(byte d1, byte d2) {
  Wire.beginTransmission(0x22);
  Wire.write(d1); Wire.write(d2);
  Wire.endTransmission();
}

void readI2C() {
  i = 0;

  Wire.requestFrom(0x22, 16);

  //Serial.print("data_buf[16] = {\n  ");
  while (Wire.available() && i < 16) {
    data_buf[i] = Wire.read();
    /*
    Serial.print("0x");
    Serial.print(data_buf[i],HEX);
    */
    i++;
    /*
    if (i != 16) Serial.print(", ");
    else Serial.print("\n}");
    if ((i % 4 == 0) && i != 16) Serial.print("\n  ");
    */
  }
  //Serial.println();
}

void loop() {
  // IR sensor read
  Wire.beginTransmission(0x22);
  Wire.write(0x36);
  Wire.endTransmission();
    
  for (i=0;i<16;i++) data_buf[i] = 0;

  readI2C();

  Ix[0]  =  data_buf[1];
  Iy[0]  =  data_buf[2];
  s      =  data_buf[3];
  Ix[0] += (s & 0x30) <<4;
  Iy[0] += (s & 0xC0) <<2;

  Ix[1]  =  data_buf[4];
  Iy[1]  =  data_buf[5];
  s      =  data_buf[6];
  Ix[1] += (s & 0x30) <<4;
  Iy[1] += (s & 0xC0) <<2;

  Ix[2]  =  data_buf[7];
  Iy[2]  =  data_buf[8];
  s      =  data_buf[9];
  Ix[2] += (s & 0x30) <<4;
  Iy[2] += (s & 0xC0) <<2;

  Ix[3]  =  data_buf[10];
  Iy[3]  =  data_buf[11];
  s      =  data_buf[12];
  Ix[3] += (s & 0x30) <<4;
  Iy[3] += (s & 0xC0) <<2;

  Serial.println("pos={");
  for (i=0; i<4; i++) {
    Serial.print("  ");Serial.print((i+1));Serial.print(" = ( ");
    Serial.print( int(Ix[i]) );Serial.print(",");
    Serial.print( int(Iy[i]) );Serial.print(" )");
    if (i<3) Serial.print(")");
  }
  //if (Ix[i] < 1000 || Ix[i] < 100 || Ix[i] < 10) Serial1.print("");
  Serial.print("\n}");
  Serial.println();
  delay(1000);

  delay(3000);
}

//////////////////////////////////////////////////////////////////////////////

// DFRobot IR positioning camera resolution
constexpr int CamResX = 1024;
constexpr int CamResY = 768;

// DFRobot IR positioning camera maximum X and Y
constexpr int CamMaxX = CamResX - 1;
constexpr int CamMaxY = CamResY - 1;

// shift amount for extra precision for the maths
// since the median is an average of 4 values, use 2 more bits
constexpr int CamToMouseShift = 2;

// multiplier to convert IR camera position to mouse position
constexpr int CamToMouseMult = 1 << CamToMouseShift;

// mouse resolution
constexpr int MouseResX = CamResX * CamToMouseMult;
constexpr int MouseResY = CamResY * CamToMouseMult;

// Mouse position maximum X and Y
constexpr int MouseMaxX = MouseResX - 1;
constexpr int MouseMaxY = MouseResY - 1;

void calculate_pos() {

positionX[0] = 

}