#include <PID_v1.h> 
#include <Wire.h>
#include <SoftwareSerial.h>
#define motorInterfaceType 1
#include <Thread.h>
#include <ThreadController.h>
#include <StaticThreadController.h>

#define THERMISTOR_PINA A11
#define TEMPCONTROLA 10

// StaticThreadController that will controll all threads
// All non-pointers go with '&', but pointers go without '&', 
//StaticThreadController(myThread, &hisThread, &theThread);
//StaticThreadController<3> controll (myThread, &hisThread, theThread);

//Define the aggressive and conservative Tuning Parameters
double aggKpA=150, aggKiA=0.5, aggKdA=0;
int thermoelectricval = 255;
int celsius;
int levelstreamon;
double settemp = 0;
double Setpoint, Input, Output, OutputA, OutputB, OutputC;
int tme = 250;

//Specify the links and initial tuning parameters
PID myPIDA(&Input, &OutputA, &Setpoint, aggKpA, aggKiA, aggKdA, DIRECT);


const int stepsPinA = 11;
const int directionPinA = 12;
const int enablePinA = 2;

const int stepsPinB = 3;
const int directionPinB = 4;
const int enablePinB = 5;

const int stepsPinC = 7;
const int directionPinC = 8;
const int enablePinC = 9;

const int gobuttonA = A0;
const int stopbuttonA = A1;
const int speedpotA = A2;

const int gobuttonB = A5;
const int stopbuttonB = A6;
const int speedpotB = A3;

const int gobuttonC = A7;
const int stopbuttonC = A10;
const int speedpotC = A4;

int directionA = 0;
int directionB = 0;
int directionC = 0;

int ton = 0;

int buttonastate = 0;
int buttonbstate = 0;
int buttoncstate = 0;
int afl = 0;
int bfl = 0;

String command;
long int currpos;
long int pos;

//My Thread (as a pointer)
Thread* myThread = new Thread();
//His Thread (not pointer)
//Thread hisThread = Thread();
//Thread allThread = Thread();
Thread runnerAThread = Thread();
Thread runnerBThread = Thread();
Thread runnerCThread = Thread();


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(stepsPinA, OUTPUT);
  pinMode(directionPinA, OUTPUT);
  pinMode(enablePinA, OUTPUT);
  pinMode(stepsPinB, OUTPUT);
  pinMode(directionPinB, OUTPUT);
  pinMode(enablePinB, OUTPUT);
  pinMode(speedpotA, INPUT);
  pinMode(gobuttonA, INPUT);
  pinMode(stopbuttonA, INPUT);
  pinMode(speedpotB, INPUT);
  pinMode(gobuttonB, INPUT);
  pinMode(stopbuttonB, INPUT);
  pinMode(speedpotC, INPUT);
  pinMode(gobuttonC, INPUT);
  pinMode(stopbuttonC, INPUT);

  /*
  allThread.onRun(runnerA);
  allThread.onRun(runnerB);
  allThread.onRun(runnerC);
  */
  runnerAThread.onRun(runnerA);
  runnerBThread.onRun(runnerB);
  runnerCThread.onRun(runnerC);

  //hisThread.onRun(boringCallback); 

  digitalWrite(enablePinA,LOW);
  digitalWrite(enablePinB,LOW);
  digitalWrite(directionPinA, LOW);
  digitalWrite(directionPinB, LOW);
  digitalWrite(directionPinC, LOW);
  currpos = 0;
}

void loop() {

  Input = read_temp("A");
  double gap = abs(Setpoint-Input); //distance away from setpoint
  myPIDA.SetTunings(aggKpA, aggKiA, aggKdA);
  myPIDA.Compute();
  analogWrite(TEMPCONTROLA,OutputA);

  int rawvalue = analogRead(THERMISTOR_PINA);
  int rawvalueA = rawvalue;
  float celsiusA = read_temp("A");

  Serial.print("");

/*  
  digitalWrite(stepsPinA, HIGH);
  digitalWrite(directionPinA, LOW);
  digitalWrite(enablePinA, HIGH);  

  digitalWrite(stepsPinB, LOW);
  digitalWrite(directionPinB, LOW);
  digitalWrite(enablePinB, LOW); 

  digitalWrite(stepsPinC, LOW);
  digitalWrite(directionPinC, HIGH);
  digitalWrite(enablePinC, LOW); 
*/

  if (levelstreamon == 1) {
   Serial.print("A: ");
   Serial.print(rawvalueA);
   Serial.print(" ");
   Serial.println(celsiusA);
   Serial.print("Settemp: ");
   Serial.println(settemp);   
   delay(500);
  }

 if (digitalRead(stopbuttonA) ==HIGH){
  if (bfl == 0){
   if (directionA==0){directionA = 1; digitalWrite(directionPinA, HIGH); }else{directionA=0; digitalWrite(directionPinA, LOW);}
    Serial.print(directionA);
    Serial.println(" direction changed");
    bfl = 1;
   }
  } else { bfl = 0; }

 if (digitalRead(gobuttonA) == 1){
   Serial.println("moving A");
   //runnerA();
   runnerAThread.run();
  }

 if (digitalRead(stopbuttonB) ==HIGH){
  if (bfl == 0){
   if (directionB==0){directionB = 1; digitalWrite(directionPinB, HIGH); }else{directionB=0; digitalWrite(directionPinB, LOW);}
    Serial.print(directionB);
    Serial.println(" direction changed");
    bfl = 1;
   }
  } else { bfl = 0; }

 if (digitalRead(gobuttonB) == 1){
  Serial.println("moving B");
   //runnerB();
   runnerBThread.run();
  }

 if (digitalRead(stopbuttonC) ==HIGH){
  if (bfl == 0){
   if (directionC==0){directionC = 1; digitalWrite(directionPinC, HIGH); }else{directionC=0; digitalWrite(directionPinC, LOW);}
    Serial.print(directionC);
    Serial.println(" direction changed");
    bfl = 1;
   }
  } else { bfl = 0; }

 if (digitalRead(gobuttonC) == 1){
  Serial.println("moving C");
   //runnerC();
   runnerCThread.run();
  }

  
 if(Serial.available())
 {
    char c = Serial.read();
    if (c== '\n')
    {
      currpos = parseCommand(command, currpos);
      command = "";
    }
    
    else
    {
      command +=c;
    }
 }
 delay(30);
}

long int parseCommand(String com, long int currpos)
{
  //Serial.print("Your command: ");
  //Serial.println(com);

  if(com.equalsIgnoreCase("readpot")){
   readpotentionmetersanity();
  }
  else if(com.equalsIgnoreCase("info")){
    Serial.println("Extruder spool speed controller");
  }
  else if (com.substring(0,7) == "settemp") {
    settemp = com.substring(8).toDouble();
    Setpoint = settemp;
    Serial.print("settemp: ");
    Serial.println(settemp);
  }
  else if(com.equalsIgnoreCase("levelstreamoff")){
   levelstreamon = 0;
  } 
  else if(com.equalsIgnoreCase("levelstreamon")){
   levelstreamon = 1;
  } 
  else if(com.equalsIgnoreCase("readlevel")){
     int rawvalue = analogRead(THERMISTOR_PINA);
     float celsius = read_temp("A");
     Serial.print("A: ");
     Serial.println(celsius);
  }
  else {
   Serial.print("Did not recognize ");
   Serial.println(com);
  }
   return currpos;
}

int readpotentionmetersanity(){
 int val = analogRead(speedpotA);
 Serial.print("speed pinA: ");
 Serial.println(val);
 val = analogRead(speedpotB);
 Serial.print("speed pinB: ");
 Serial.println(val); 
 val = analogRead(speedpotC);
 Serial.print("speed pinC: ");
 Serial.println(val); 
}


int runnerA(){
   int stpper = 1;
   int speed = analogRead(speedpotA);
   int stepdelay = map(speed, 0, 1023, 1, 2000);
   while(stpper > 0){
   speed = analogRead(speedpotA);
   stepdelay = map(speed, 0, 1023, 1, 2000);
   digitalWrite(stepsPinA,HIGH); // Output high
   delayMicroseconds(stepdelay); // Wait 1/2 a ms
   digitalWrite(stepsPinA,LOW); // Output high
   delayMicroseconds(stepdelay); // Wait 1/2 a ms
   if (digitalRead(stopbuttonA) ==HIGH){ break; }
 }
return 0;
}

int runnerB(){
   int stpper = 1;
   int speed = analogRead(speedpotB);
   int stepdelay = map(speed, 0, 1023, 1, 2000);
   while(stpper > 0){
   speed = analogRead(speedpotB);
   stepdelay = map(speed, 0, 1023, 1, 2000);
   digitalWrite(stepsPinB,HIGH); // Output high
   delayMicroseconds(stepdelay); // Wait 1/2 a ms
   digitalWrite(stepsPinB,LOW); // Output high
   delayMicroseconds(stepdelay); // Wait 1/2 a ms
   if (digitalRead(stopbuttonB) ==HIGH){ break; }
 }
return 0;
}

int runnerC(){
   int stpper = 1;
   int speed = analogRead(speedpotB);
   int stepdelay = map(speed, 0, 1023, 1, 2000);
   while(stpper > 0){
   speed = analogRead(speedpotC);
   stepdelay = map(speed, 0, 1023, 1, 2000);
   digitalWrite(stepsPinC,HIGH); // Output high
   delayMicroseconds(stepdelay); // Wait 1/2 a ms
   digitalWrite(stepsPinC,LOW); // Output high
   delayMicroseconds(stepdelay); // Wait 1/2 a ms
   if (digitalRead(stopbuttonC) ==HIGH){ break; }
 }
return 0;
}

#define NUMTEMPS 20
short temptable[NUMTEMPS][2] = {
   {1, 841}, {54, 255},
   {107, 209},
   {160, 184},
   {213, 166},
   {266, 153},
   {319, 142},
   {372, 132},
   {425, 124},
   {478, 116},
   {531, 108},
   {584, 101},
   {637, 93},
   {690, 86},
   {743, 78},
   {796, 70},
   {849, 61},
   {902, 50},
   {955, 34},
   {1008, 3}
};

float read_temp(String sensor_type)
{
   int THERMISTOR_PIN = 0;
   if (sensor_type == "A"){
    THERMISTOR_PIN = THERMISTOR_PINA;
   }
   int rawtemp = analogRead(THERMISTOR_PIN);
   float current_celsius = 0;

   byte i;
   for (i=1; i<NUMTEMPS; i++)
   {
      if (temptable[i][0] > rawtemp)
      {
         float realtemp  = temptable[i-1][1] + (rawtemp - temptable[i-1][0]) * (temptable[i][1] - temptable[i-1][1]) / (temptable[i][0] - temptable[i-1][0]);

         if (realtemp > 255)
           realtemp = 255; 
           current_celsius = realtemp;
           break;
      }
   }

   // Overflow: We just clamp to 0 degrees celsius
   if (i == NUMTEMPS)
   current_celsius = 0;

   return current_celsius;
}