#include <Wire.h>
#include <SoftwareSerial.h>
#include <CapacitiveSensor.h>

// CHANGE ME
#define id 1
#define serial_pin_in 12
#define serial_pin_out 13
#define v_sense_pin A3
#define max_msg_len 25
#define delta 5
#define vibe_sense 853
#define ring_sense 512
#define cap_sense 255
#define motor_pin 11
#define sense_1_pin 13
#define sense_2_pin 11
#define step_pin 12

CapacitiveSensor cap_1 = CapacitiveSensor(step_pin, sense_1_pin);
CapacitiveSensor cap_2 = CapacitiveSensor(step_pin, sense_2_pin);
SoftwareSerial SoftSerial = SoftwareSerial(serial_pin_in,serial_pin_out);

enum masterState {
  WAITING,
  READING,
  ANALYZE
};

enum slaveMode {
  POLL_PERIPH,
  VIBRATE,
  SENSE_TOUCH,
  CHANGE_DISP,
  WRITE,
  READ
};

enum peripheral {
  NO_CONN,
  VIBE_MOTOR,
  CAP_TOUCH,
  RING_DISP
};

byte masterMode = 0;
int vSense;
long capSense1, capSense2;

void setup() {
  // put your setup code here, to run once:
  CLKPR = (1 << CLKPCE);
  CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
  //Serial

  //TWI
  Wire.begin(id);

  // Check if Master
  // On V_sense = 3.3v at the start, this is plugged into FTDI
  vSense = analogRead(v_sense_pin);
  if (vSense > 1020){ // should be 1023, but we'll give some wiggle room
    masterMode = 1;
    pinMode(serial_pin_out,OUTPUT);
    SoftSerial.begin(38400);
  }
  if (!masterMode){ // not a master? Then a slave
    Wire.onReceive(receiveEvent);
    Wire.onRequest(requestEvent);
    cap_1.set_CS_AutocaL_Millis(0xFFFFFFFF);
    cap_2.set_CS_AutocaL_Millis(0xFFFFFFFF);
    pinMode(serial_pin_out,OUTPUT);
    pinMode(serial_pin_in,INPUT);
  }
}

char msgBuffer[max_msg_len] = {0};
int index = 0;
masterState curMasterState = WAITING;
volatile slaveMode curSlaveMode = POLL_PERIPH;
peripheral curPeriph, prevPeriph;
volatile byte inByte;

void loop() {
  if (masterMode) {
    switch (curMasterState){
      case WAITING:
        if (SoftSerial.available()){
          char c = SoftSerial.read();
          if (c == '{') { // initializer for data
            memset(msgBuffer, 0, sizeof(msgBuffer));
            index = 0;
            curMasterState = READING;
          }
        }
        break;
      case READING:
        if (SoftSerial.available()){
          char c = SoftSerial.read();
          if (c == '}' || index == max_msg_len) {
            curMasterState = ANALYZE;
          } else {
            msgBuffer[index] = c;
            index++;
          }
        }
        break;
      case ANALYZE:
        if (strcmp(msgBuffer, "poll") == 0){
          for (int i = 1; i <= 7; i++) {
            SoftSerial.print(i);
            SoftSerial.print(",0,");
            if (i != id) {
              int periph = getSlavePeripheral(i);
              SoftSerial.println(periph);
            }
            else {
              SoftSerial.println(0);
            }
          }
        } else {
          byte result[10] = {0};
          parseData(msgBuffer,",",&result[0]);
          sendDataTWI(result[0],&result[1]);
          if (result[1] == 2) {
            Wire.requestFrom((int)result[0],1);
            byte b = Wire.read();
            SoftSerial.print(result[0]);
            SoftSerial.print(",2,");
            SoftSerial.println(b);
          }
          if (result[1] == 3){
            Wire.requestFrom((int)result[0],1); // yeah this is just legacy tbh
            Wire.read();
          }
        }
        curMasterState = WAITING;
        break;
    }
  } else {
    vSense = analogRead(v_sense_pin);
    if (vSense == 0){
      if (curPeriph != NO_CONN){
        curPeriph = NO_CONN;
      }
    } else if (almostEqual(vSense, vibe_sense, delta)) {
      if (curPeriph != VIBE_MOTOR){
        SoftSerial.end();
        curPeriph = VIBE_MOTOR;
        pinMode(motor_pin, OUTPUT);
      }
    } else if (almostEqual(vSense, cap_sense, delta)) {
      if (curPeriph != CAP_TOUCH){
        SoftSerial.end();
        curPeriph = CAP_TOUCH;
        cap_1 = CapacitiveSensor(step_pin, sense_1_pin);
        cap_2 = CapacitiveSensor(step_pin, sense_2_pin);
      }
      capSense1 = cap_1.capacitiveSensor(30);
      capSense2 = cap_2.capacitiveSensor(30);
    } else if (almostEqual(vSense, ring_sense, delta)) {
      if (curPeriph != RING_DISP){
        curPeriph = RING_DISP;
        SoftSerial = SoftwareSerial(serial_pin_in,serial_pin_out);
        pinMode(serial_pin_out,OUTPUT);
        pinMode(serial_pin_in,INPUT);
        SoftSerial.begin(9600);
      }
    }
    delay(10);
  }
}

peripheral getSlavePeripheral(int slave) {
  sendSingleTWI(slave, 0);
  delayMicroseconds(10);
  Wire.requestFrom(slave, 1);
  return Wire.read();
}

int getSlaveAnalog(int slave) {
  sendSingleTWI(slave, 0);
  delayMicroseconds(10);
  Wire.requestFrom(slave, 2);
  byte topByte = Wire.read();
  byte botByte = Wire.read();
  int analogVal = ((int)topByte << 8) + (int)botByte;
  return analogVal;
}

void sendSingleTWI(byte target, byte data) {
  Wire.beginTransmission(target);
  Wire.write(data);
  Wire.endTransmission();
  delayMicroseconds(10);
}

void sendDataTWI(byte target, byte data[]) {
  Wire.beginTransmission(target);
  for (int i = 0; i < sizeof(data); i++){
    Wire.write(data[i]);
  }
  Wire.endTransmission();
  delayMicroseconds(10);
}

void parseData(char data[], char delims[], byte * result){
  char * token;
  token = strtok(data, delims);
  int i = 0;
  while (token != NULL){
    result[i] = atoi(token) & 0xFF;
    token = strtok(NULL, delims);
    i++;
  }
}

void receiveEvent(int howMany){
  byte mode = Wire.read();
  curSlaveMode = mode;
  if (mode == VIBRATE){
    byte vibe = Wire.read();
    if (vibe){
      digitalWrite(motor_pin,HIGH);
    } else {
      digitalWrite(motor_pin,LOW);
    }
  } else if (mode == CHANGE_DISP){
    inByte = Wire.read();
  }
  while (0 < Wire.available()) {
    Wire.read();
  }
}

void requestEvent(){
  if (curSlaveMode == POLL_PERIPH){
      Wire.write(curPeriph);
  } else if (curSlaveMode == SENSE_TOUCH){
      byte result = 0;
      if (capSense1 > 100){
        result |= 0b01;
      } 
      if (capSense2 > 100){
        result |= 0b10;
      }
      Wire.write(result);    
  } else if (curSlaveMode == CHANGE_DISP){
      SoftSerial.write(inByte);
      Wire.write(inByte);
  } else {
    Wire.write(curSlaveMode);
  }
}

bool almostEqual(int val, int goal, int dif) {
  return abs(goal-val) < dif;
}