#include "mbed.h"
#include "Motor.h"
#include "TMP102.h"
#include "rtos.h"
#include "math.h"
#include "string"

PwmOut fan(p23); 
DigitalIn fanTacho(p24);
Motor cooler1(p22, p19, p20); // pwm, fwd, rev
Motor cooler2(p21, p19, p20); // pwm, fwd, rev
TMP102 obTemp(p28, p27, 0x90);
TMP102 heatSinkTemp(p9, p10, 0x90);
Serial pc(USBTX, USBRX); // tx, rx
Mutex stdio_mutex;  // mutex lock for multithread

const double Kp = 0.08;  // proportion gain
const double Ki = 0.0008; // integration gain
const double Kd = 0.0001; //differential gain


double desiredTemp = 21.1 ; // desired temperature
double currentObTemp = 21.1; // cuurent temperature
int fanSpeed = 0;  // heat sink fan speed
char str[5] = "21.1"; // desired temperature sent to PC in string format
double pwmOut;   // pwm output between -1 -> 1

// Thread to get desired temperature from Atom PC user interface
void getDesiredTemp(void const *argument)
{   
    while(1){    
        while(pc.readable ())  
        {    
            stdio_mutex.lock();
            pc.gets(str,4);  
            desiredTemp = atof(str);
            stdio_mutex.unlock();
        }
        Thread::wait(200);
     }
}

// Thread that send current temperature, pwm output and relavant data back to PC UI 
void sendCurrentObTemp(void const *argument)
{
    while(1)
    {   
        stdio_mutex.lock();
        currentObTemp = obTemp.read();
        pc.printf("CurTemp %2.1f\n",currentObTemp);
        pc.printf("PwmOut %03d\n",(int)(100*pwmOut));
        pc.printf("FanS %4d\n", fanSpeed);
        pc.printf("HeatTemp %2.1f\n",heatSinkTemp.read());
        
        stdio_mutex.unlock();  
        Thread::wait(200);           
    }  
}

// Thread that read fan speed
void readTacho(void const *args) {
    static int state = 0, count = 0, oldCount = 0;
    static time_t seconds, oldSeconds;
    while(1) {
        if (fanTacho != state)
        {
            state = fanTacho;
            if (state == 1)
            {
                ++count;
            }
        }
        seconds = time(NULL);
        if (seconds !=oldSeconds)
        {
            stdio_mutex.lock();
            fanSpeed = (count - oldCount) / (seconds - oldSeconds) * 30;
            stdio_mutex.unlock();             
            oldCount = count;
            oldSeconds = seconds;
        }
    }
}

int main() {
    set_time(0);
    fanTacho.mode(PullUp);
    fan.pulsewidth_us(40);
    // PWM of fan speed (at max fan speed 2000Rpm) 
    fan = 1;
    
    // variable for PI controller
    double error = 0;
    double error_last = 0;
    double error_lastlast = 0;
           
    double pwmOut_last = 0;
       
 //   int counter = 0;
    
    Thread threadGet(getDesiredTemp);
    Thread threadSend(sendCurrentObTemp);
    Thread threadFanSpeed(readTacho);
    
    while(1)
    {    
     
      error = - (desiredTemp - currentObTemp);
          
      // pwm Output calculation
      pwmOut = pwmOut_last + Kp*(error - error_last) + Ki*error + Kd*(error -2*error_last + error_lastlast);
      
      // fix into certain range
      if (pwmOut > 1)
       pwmOut = 1;
      if (pwmOut < -1)
       pwmOut = -1; 
      if (pwmOut < 0 && pwmOut > -0.1)
       pwmOut = 0;
  
      if(pwmOut < 0.15 && pwmOut > 0)
        pwmOut = 0.15; 
      
      
      // update variable each loop  
      error_lastlast = error_last;
      error_last = error;
      
      pwmOut_last = pwmOut;
       
       // ouput PWM signal
      cooler1.speed(pwmOut);
      cooler2.speed(pwmOut);
 

       Thread::wait(10);           
 
    }
}