#include <math.h>
#define LIGHTSENSOR_PIN       A1
#define CAPSENSE_PIN          A2 // works for both A2 and D2 due to Arduino libraries
#define DRIVE_PIN             D10
#define GND_PIN               A0
#define VDD_PIN               A3 // will add a jumper for this

float getDCVD(uint16_t pow2samps) {
  uint16_t samps = 0x01 << pow2samps;
  int32_t temp;
  int32_t capsenseVal = 0;
  
  for(uint16_t n = 0; n<samps; n++) {

    // do positive CVD (capacitor voltage divider) measurement
    pinMode(CAPSENSE_PIN, OUTPUT); 
    digitalWrite(CAPSENSE_PIN, HIGH); // precharge CAPSENSE_PIN to HIGH
    analogRead(GND_PIN); // connect ADC to GND
    pinMode(CAPSENSE_PIN, INPUT); // change CAPSENSE_PIN to be an input
    temp = analogRead(CAPSENSE_PIN); //connect ADC to CAPSENSE_PIN for CVD

    // do negative CVD measurement
    pinMode(CAPSENSE_PIN, OUTPUT);
    digitalWrite(CAPSENSE_PIN, LOW); //precharge CAPSENSE_PIN to LOW
    analogRead(VDD_PIN); //connect ADC to VDD to precharge it
    pinMode(CAPSENSE_PIN, INPUT); // change CAPSENSE_PIN to be an input
    temp -= analogRead(CAPSENSE_PIN); //connect ADC to capsense pin for CVD
	
    capsenseVal += temp;
  }
    return capsenseVal / pow(2.0, pow2samps); // float division
}

void setup() {
  Serial.begin(115200);
  // Set pins to desired type
  int value = analogRead(LIGHTSENSOR_PIN);

  // Set PWM to zero
  analogWrite(DRIVE_PIN,0); // between 0 and 255
  }

void loop() {
    Serial.println(getDCVD(12)); // sends the average of 2^12 capacitance values to Serial Monitor
    delay(10);
    }

















  // Serial.println(readCapsense());



  //uint16_t lightSensorReading = analogRead(LIGHTSENSOR_PIN);
  //analogWrite(DRIVE_PIN, (lightSensorReading/8));
  //Serial.println(lightSensorReading);
  //delay(100);





 
  // If voltage exceeds value for >10ms, set PWM to small number

  // If again, set PWM to medium number

  // If again, set PWM to high number

  // If again, set PWM to zero.
  ////

  // If phototransistor reads current >= value for >2s, set PWM to small number (dim)


  // If phototransistor reads current < value for >2s, set PWM to high number (bright)



// if （analogRead(LIGHTSENSOR_PIN < 3500）) // （3500/4095)*3.3 = 2.8V; max value will be 4095 since RP2040 ADC is 12-bit (2^12 - 1)
//    {analogWrite(DRIVE_PIN, pow(number, 0.33))} // taking the 0.33 root of measured brightness is an approximation for perceived brightness