#include <Wire.h>
#define PWM13       OCR4A
#define DRV         0x5A
#define ModeReg     0x01
#define MUX_ADDR    0x70

// Sensors
const int trigPins[2] = {12, 10};
const int echoPins[2] = {11, 9};

// Potentiometer
int analogPin = A1; //Potentiometer pin
int potVal = 0; 
float minVal = 0.25; //Minimum scale value from potentiometer
float maxVal = 1.75; //Maximum scale value from potentiometer

//leds
const int ledPins[4] = {5, 6, 7, 8}; 


void setup() {
  Wire.begin();
  Serial.begin(9600);

  for (int i = 0; i < 1; i++) {
    pinMode(trigPins[i], OUTPUT);
    pinMode(echoPins[i], INPUT);
  }

  // Init all DRV2605L's on each MUX channel (channels 0 and 1)
  for (int i = 0; i < 1; i++) {
    selectMuxPort(i);
    initializeDRV2605();
  }

  for (int i = 0; i < 4; i++) {
  pinMode(ledPins[i], OUTPUT);
}

}

void loop() {
  potVal = analogRead(analogPin);           // Read potentiometer value
  float mappedVal = mapPotentiometer();     // Apply custom mapping
  updateLEDs(potVal);                       // Update LEDs based on potentiometer

  long dist[2];
  double intensity[2] = {0.0, 0.0};         // Initialize intensities to 0
  double duration, pauseTime;

  for (int i = 0; i < 1; i++) {
    dist[i] = measureDistance(trigPins[i], echoPins[i]);

    // Only trigger vibration if:
    // 1. Distance is greater than 0 (no false readings)
    // 2. Distance is less than 60 cm
    // 3. Potentiometer is turned on (value > 5)
    if (dist[i] > 0 && dist[i] < 90 && potVal > 5) {
      mapPulse(dist[i], mappedVal, &intensity[i], &duration, &pauseTime);
      selectMuxPort(i);
      pulse(intensity[i], duration);
      pause(pauseTime);
    }
    // else intensity remains 0 (no vibration)
  }

  // Print output with Left/Right labels
  Serial.print("Left | Distance: ");
  Serial.print(dist[0]);
  Serial.print(" cm | Scaled intensity: ");
  Serial.print(mappedVal, 2);
  Serial.print(" * ");
  Serial.print(intensity[0], 2);

  Serial.print(";    ");

  Serial.print("Right | Distance: ");
  Serial.print(dist[1]);
  Serial.print(" cm | Scaled intensity: ");
  Serial.print(mappedVal, 2);
  Serial.print(" * ");
  Serial.println(intensity[1], 2);

  delay(100);
}


void selectMuxPort(uint8_t port) {
  Wire.beginTransmission(MUX_ADDR);
  Wire.write(1 << port);
  Wire.endTransmission();
}

long measureDistance(int trig, int echo) {
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH, 20000); // max 20ms
  long distance = duration * 0.034 / 2;
  return distance;
}
void mapPulse(double dist, float mappedVal, double* intensity, double* duration, double* pauseTime) {
  //In this function the pulse parameters are determined
  if (dist < 30) {
    *intensity = 1.0 * mappedVal; //mappedVal comes from potentiometer to manipulate the intensity and speed of the pulse
    *duration = 10.0;
    *pauseTime = 20.0 * mappedVal;
  } else if (dist < 60) {
    *intensity = 0.5 * mappedVal;
    *duration = 20.0;
    *pauseTime = 70.0 * mappedVal;
  } else if (dist < 90) {
    *intensity = 0.1 * mappedVal;
    *duration = 20.0;
    *pauseTime = 200.0 * mappedVal;
  } 
}


void pulse(double intensity, double milliseconds) {
  int minimumint = 140;
  int maximumint = 255;
  int pwmintensity = (int)(intensity * (maximumint - minimumint)) + minimumint;

  standbyOffB();
  PWM13 = pwmintensity;
  usdelay(milliseconds);
  standbyOnB();
}

void standbyOnB() {
  Wire.beginTransmission(DRV);
  Wire.write(ModeReg);
  Wire.write(0x43); // standby aan
  Wire.endTransmission();
}

void standbyOffB() {
  Wire.beginTransmission(DRV);
  Wire.write(ModeReg);
  Wire.write(0x03); // standby uit
  Wire.endTransmission();
}

void usdelay(double time) {
  double us = time - ((int)time);
  for (int i = 0; i <= (int)time; i++) {
    delay(1);
  }
  delayMicroseconds(us * 1000);
}

void pause(double milliseconds) {
  double us = milliseconds - ((int)milliseconds);
  standbyOnB();
  for (int i = 0; i <= (int)milliseconds; i++) {
    delay(1);
  }
  delayMicroseconds(us * 1000);
}

void initializeDRV2605() {
  Wire.beginTransmission(DRV);
  Wire.write(ModeReg);
  Wire.write(0x00); // uit standby
  Wire.endTransmission();

  Wire.beginTransmission(DRV);
  Wire.write(0x1D); // Library Selection
  Wire.write(0xA8); // RTP unsigned
  Wire.endTransmission();

  Wire.beginTransmission(DRV);
  Wire.write(0x03); // Library B
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(DRV);
  Wire.write(0x17); // Full scale ref
  Wire.write(0xFF);
  Wire.endTransmission();

  Wire.beginTransmission(DRV);
  Wire.write(ModeReg);
  Wire.write(0x03); // PWM Mode
  Wire.endTransmission();

  delay(100);
}

float mapPotentiometer() {
  int potVal = analogRead(analogPin);
  
  if (potVal < 5) {
    return 0.0f;
  }
  else if (potVal >= 5 && potVal < 344) {
    return minVal;
  }
  else if (potVal >= 344 && potVal < 684) {
    return 1.0f;
  }
  else {
    return maxVal;
  }
}

void updateLEDs(int potValue) {
  // Zet alle LED's uit
  for (int i = 0; i < 4; i++) {
    digitalWrite(ledPins[i], LOW);
  }

  // Zet de juiste LED aan op basis van potentiometer waarde
  if (potValue < 5) {
    digitalWrite(ledPins[0], HIGH); // pin 5
  } else if (potValue >= 5 && potValue < 344) {
    digitalWrite(ledPins[1], HIGH); // pin 6
  } else if (potValue >= 344 && potValue < 684) {
    digitalWrite(ledPins[2], HIGH); // pin 7
  } else {
    digitalWrite(ledPins[3], HIGH); // pin 8
  }
}