/*
 * Project OUIFI_DOWSING01
 * PHOEBE DEGROOT
 */
#include <neopixel.h>

#define NUMREADINGS 5

// PIN DEF -----------------------
uint8_t probepin1 = A0;
uint8_t probepin2 = A1; 
uint8_t motor1 = D2;
uint8_t motor2 = D3; 
uint8_t switch1 = D4; 
uint8_t switch2 = D5;

uint8_t POTthresh = A2;
uint8_t POTband = A3;


// EMF Detection Vars -----------------------
uint8_t  LOWthresh = 5; 
int HIGHthresh = 2000;

uint8_t  ct=0;
int total = 0;
int avg;
int readings[NUMREADINGS];

int wifi_reading;
int probe_reading; 
bool useprobe1 = true;
uint8_t signal_mode = 1; 
String modes[]={"DISABLED","EMF","WIFI LOCAL","WIFI GLOBAL"}; //different modes for singal data collection


typedef struct{
  unsigned long t=0;
  uint32_t delay; 
  bool active = true; 
}timer;

timer pubtimer = {0,1500,true}; //publishing timer 

int probe_read(){
    int val; //
    uint8_t probepin;
    if (useprobe1){ 
        probepin = probepin1; //select antenna
    }
    else {probepin = probepin2;}
    
    val = analogRead(probepin);  // take a reading from the probe
    
    //revisit this code - average reading calculation
    if(val >= 1){              
        val = constrain(val, 1, HIGHthresh); 
        val = map(val, 1, HIGHthresh, 1, 255);  
        total -= readings[ct];            
        readings[ct] = val; //add value to reading
        total += readings[ct];      
        ct++;          //adv index

    if (ct >= NUMREADINGS)  
      ct = 0;         //

    avg = total / NUMREADINGS; 
    return avg;
    }
    
    else { return 1; }
}



void publish_data(){
    if (signal_mode == 1){
        Particle.publish("probe-reading", String(probe_reading));
    }
    else if(signal_mode == 2){
        Particle.publish("wifi-reading", String(wifi_reading));
    }
}

void write_motor(){
    if (probe_reading>LOWthresh){
        int val = map(probe_reading,LOWthresh,255,50,255);
        analogWrite(motor1,val);
        analogWrite(motor2,val);
    }
    else{
        analogWrite(motor1,0);
        analogWrite(motor2,0);
    }
}

int wifi_read(){
    int strength;
    if (signal_mode != 3){
    WiFiSignal sig = WiFi.RSSI();
    float sigstr = sig.getStrength();
    // use the current connected wifi to read strength
    strength = int(sigstr);
    strength = map(strength, 0,100, 1, 255); 
    }
    else{
        //search for strongest wifi signal from all available
    }
    return strength; 
}

void check_switch(){
    if (digitalRead(switch1)==HIGH){
        useprobe1 = false;
    }
    else {useprobe1=true;}
    
    if (digitalRead(switch2)==HIGH){
        signal_mode = 2; 
    }
    else {signal_mode = 1;}
}

void setup() {
    pinMode(switch1, INPUT_PULLUP);
    pinMode(switch2, INPUT_PULLUP);
    pinMode(motor1,OUTPUT);
    pinMode(motor2,OUTPUT);
    Particle.variable("probe",probe_reading);
    Particle.variable("wifi strength",wifi_reading);
    Particle.variable("low thresh",LOWthresh);
    Particle.variable("high thresh",HIGHthresh);

}

void loop() {
    
    LOWthresh = map(analogRead(POTthresh),4070,0,0,255);
    HIGHthresh = map(analogRead(POTband),4070,0,10,4000);
    
    probe_reading = probe_read();
    wifi_reading = wifi_read();
    
    check_switch();
    write_motor();

    if ((pubtimer.active) && (millis()-pubtimer.t>= pubtimer.delay)){
        publish_data();
        pubtimer.t = millis();
    }
    
    delay(5);

}