#include "mbed.h"
#include "VodafoneUSBModem.h"
#include "Websocket.h"
#include "Terminal.h"

Terminal term(USBTX, USBRX);
  AnalogIn ultrasensor(p15);  
  DigitalIn range (p10);
  Timer t;
  int a,b;
  float buffer1[50]; 
  float answer, sum, result;

void test(void const*) 
{
    
    // Put 50 values from ultrasonic sensor into a buffer*************
    for ( int a=0;a<=49;a++)
    {
    buffer1[a]=ultrasensor.read();
    }
    // Set sum to zero so not using values from previous round **********
   sum=0;
   // Add together 50 values taken from ultrasonic sensor ******************
   for (b=0;b<=49;b++)
   {
   sum=sum+buffer1[b];
   }
   // Devide values by 50 to get average ***********************************
   result=sum/50;      
   answer=result*1024;
   
   // Print values in Tera Term
  // term.printf(" Voltage Value is:  %0.2f \n\r", answer);
   //wait (0.4);
   //term.cls();
 
//  ************************    Pulse Width Part of program ***********************//

//I assume here you got 'range' defined as DigitalIn on the correct pin
//Timer timer;  //Define a timer we can use
 
   while(range == 1);    //This blocks the program as long as your range pin is high.
 
//So here the range pin must be low
 
   while(range == 0);    //This blocks the program as long as your range pin is low.
 
//When we get here it means the range pin just went from low to high
t.start();             //So start measuring the time
 
//Wait as long as the pin is high:
while(range == 1);
 
//Here the output is low again, so we can stop the timer.
t.stop();
 
float time_us = t.read_us();
float cm = time_us/58;
term.printf(" Voltage Value is:  %0.2f  Pulse Width Value is: %0.2f \r", answer, cm);
//term.printf(" Pulse Width Value is: %f\r", cm);
wait(1);
t.reset();
//term.cls();
    
    
    
    
    VodafoneUSBModem modem;
    Websocket ws("ws://sockets.mbed.org:443/ws/generator/rw");
    char recv[128];
    
    int ret = modem.connect("smart");
    if(ret)
    {
      printf("Could not connect\n");
      return;
    }
    
    bool c = ws.connect();
    printf("Connect result: %s\n", c?"OK":"Failed");
    
    
    
    for(int i = 0; i < 10000; i++)
    {
        if(!(i%100))
        {
          int ret = ws.send("Generator values are:   %0.2f",answer);
          if(ret<0)
          {
            printf("Timeout\n");
            ws.close();
            c = ws.connect();
            printf("Connect result: %s\n", c?"OK":"Failed");
          }
        }
        
        if (ws.read(recv)) {
            printf("rcv: %s\r\n", recv);
        }

    }
    
    modem.disconnect();  

    while(1) {
    }
}


int main()
{
  Thread testTask(test, NULL, osPriorityNormal, 1024 * 4);
  DigitalOut led(LED1);
  while(1)
  {
    led=!led;
    Thread::wait(1000);  
  }
  return 0;
}