//#include <Adafruit_MCP4725.h>
//Adafruit_MCP4725 myDAC;
byte mcp4725Add = 0x62;
int serialWriteT = 0;
int serialWriteDelay = 300;
int voltEngage = 3900;
int maxOutputVolt = 4800;
int minOutputVolt = 500;
int maxTempOutVolt = 600;
int linearModeVoltAdd = 0;
int maxLinearModeVoltAdd = 700;
bool dataLog = true;

#define inptAFM A0
#define inptTemp A1
#define inptTune A3
#define voltToOutputPot 13
#define voltToTempSensor 12
#define customInpt6 6
#define customInpt10 10
#define customInpt11 11

void printl(){
  printl("");
}
void printl(String msg){
  printT(msg, 0);  
}
void printl(float msg){
  printT(msg, 0);  
}
void printnl(){
  printnl("");
}
void printnl(String msg){
  printT(msg, 1);  
}
void printnl(float msg){
  printT(msg, 1);  
}
void printT(float msg, bool newLine){
  printT(String(msg,0), newLine);
}
void printT(String msg, bool newLine){
  if(serialWriteT > serialWriteDelay && dataLog){
    if(newLine){
      Serial.println(msg);
    }else{
      Serial.print(msg);
    }
  }
}

void setup() {
  myDAC.begin(mcp4725Add);
  myDAC.setVoltage(0, false);
  Serial.begin(9600);

  pinMode(inptAFM, INPUT);
  pinMode(inptTemp, INPUT);
  pinMode(voltToOutputPot, OUTPUT);
  pinMode(voltToTempSensor, OUTPUT);
  pinMode(customInpt6, INPUT_PULLUP);
  pinMode(customInpt10, INPUT_PULLUP);
  pinMode(customInpt11, INPUT_PULLUP);
}

void loop() {
  //TCCR0B = TCCR0B & B11111000 | B00000001;    // set timer 0 divisor to     1 for PWM frequency of 62500.00 Hz
  digitalWrite(voltToOutputPot, HIGH);
  digitalWrite(voltToTempSensor, HIGH);

  int voltAFM = map(analogRead(inptAFM), 0, 1023, 0, 5000);
  //limit inptTemp range between 0.6v to 1.5 volts
  // 122=0.6v=10c | 200=1v=50c | 306=1.5v=105c  | 347=1.8v | 410=2v
  int voltTemp = 
    map(analogRead(inptTemp), 130, 200, maxTempOutVolt, 0);
     
  if(voltTemp > maxTempOutVolt){
    voltTemp = maxTempOutVolt;
  } else if(voltTemp < 0){
    voltTemp = 0;
  }
  maxOutputVolt = 4480;
  linearModeVoltAdd = map(analogRead(inptTune), 0, 1023, 0, maxLinearModeVoltAdd);

  printnl("#### INPUTS #### ");
  printl("  Volt   AFM: ");
  printnl(voltAFM);
  printl("  Volt  Temp: ");
  printnl(voltTemp);
  printl("VoltTempVolt: ");
  int miliVoltTemp = map(analogRead(inptTemp), 0, 1023, 0, 5000); 
  printnl(miliVoltTemp);
  printl("VoltInptTune: ");
  printnl(linearModeVoltAdd); 

  if(digitalRead(customInpt6) == LOW){
    printnl("------PORT 6-------"); //PORT 6
    voltEngage = 0;
    minOutputVolt = 400;
    maxOutputVolt = 4400;

    if(voltAFM < 1600){
      linearModeVoltAdd = -map(analogRead(inptTune), 0, 986, maxLinearModeVoltAdd, 0)/2;
    }else{
     linearModeVoltAdd = 300;
    }    
  }else if(digitalRead(customInpt10) == LOW){
    printnl("------PORT 10-------"); //PORT 10
    voltEngage = 0;
    minOutputVolt = 500+(voltTemp*0.35);
    maxOutputVolt = 4500;
    int variableMinVoltIdle = 1300+(voltTemp/1.5);
    
    if(voltAFM < variableMinVoltIdle){
      linearModeVoltAdd = 0;//-(voltTemp/6);
    /*}else if(voltAFM >= variableMinVoltIdle && voltAFM < 2000){
      linearModeVoltAdd = (linearModeVoltAdd/1.5)-(voltTemp/2);
    }else if(voltAFM >= 2000){*/
    }else if(voltAFM >= variableMinVoltIdle){
     linearModeVoltAdd = linearModeVoltAdd-(voltTemp/1.8);
    }

  }else if(digitalRead(customInpt11) == LOW){
    
    printnl("------PORT 11-------"); //PORT 6
    voltEngage = 0;
    minOutputVolt = 300;
    maxOutputVolt = 4400;
    int variableMinVoltIdle = 1300+(voltTemp/1.5);
    if(voltAFM < variableMinVoltIdle){
      voltAFM = map(analogRead(inptTune), 0, 986, 300, 1000);
      linearModeVoltAdd = 0;
    }else if(voltAFM >= variableMinVoltIdle && voltAFM < 3500){
      linearModeVoltAdd = 300;
    }else{
      linearModeVoltAdd = 500;
    }    

  }else{
    voltEngage = 0;
    minOutputVolt = 600;
    maxOutputVolt = 4400;
    linearModeVoltAdd = 0;
  }
    
  printl("#### Volt Engage: ");
  printnl(voltEngage);
  
  printl("Input AFM Voltage: ");
  printnl(voltAFM);

  if(voltAFM > voltEngage){
    voltAFM += linearModeVoltAdd;
    printl("#### Volt Add/Subtract: ");
    printnl(linearModeVoltAdd);
    printnl("-----------------------");
  }
  //Set max voltage output
  if(voltAFM > maxOutputVolt){
    voltAFM = maxOutputVolt;
  }else if (voltAFM < minOutputVolt){
    voltAFM = minOutputVolt;
  }
  printl("Output AFM Voltage: ");
  printnl(voltAFM);
  printnl("############################");
  if(dataLog==false){
    Serial.println(voltAFM);
  }
  int outptVoltDac = map(voltAFM, 0, 5000, 0, 4095);
  //myDAC.setVoltage(outptVoltDac, false);
   
  if(serialWriteT > serialWriteDelay){
    serialWriteT = 0;
  }
  serialWriteT++;
  printnl();
  delay(1);  
}