#include "ktms1201.h"
#include "common.h"
#include <stdio.h>
#include <string.h>
#include <Servo.h>

/*
 Demonstration of HC-SR04 distance sensor. (<$4)
 Displays distance to:
 (1) the serial port
 (2) KTM-S1201 LCD display (<$3)
 (3) Rotates a RC Servo (<$7)
 
 HC-SR04 portions based on a sketch by terry@yourduino.com
 KTM-S1201 LCD portions based on AVR code by Jack Botner
 Remainder of code by Edward Comer

 This code serves no useful purpose other then to demo the
 HC-SR04, KTM-S1201 and servo.
 */

// HR-SR04 definitions
#define TRIG_PIN  12
#define ECHO_PIN  13
#define CM 1
#define INC 0

// KTM-S1201 LCD pin defines are in common.h

// Global variables
long  duration, distance_cm, distance_inc;
char buffer [50];
Servo myservo;  // create servo object to control a servo
int pos = 0;    // variable to store the servo position 

void setup()
{
  // Establish pin directions for LCD connection
  pinMode(LCD_SCK_PIN,OUTPUT);
  pinMode(LCD_SI_PIN,OUTPUT);
  pinMode(LCD_CD_PIN,OUTPUT);
  pinMode(LCD_RST_PIN,OUTPUT);
  pinMode(LCD_BUSY_PIN,INPUT);
  pinMode(LCD_CS_PIN,OUTPUT);
  pinMode(13,OUTPUT);
  //Do one time to Initalize KTM-S1201
  //digitalWrite(LCD_VCC, HIGH); // sets the Power On
  //digitalWrite(LCD_VSS, LOW);
  
  digitalWrite(LCD_CD_PIN, HIGH); //Put in copmmand mode
  digitalWrite(LCD_CS_PIN, HIGH); //deselect KTM-S1201
  digitalWrite(LCD_SCK_PIN, HIGH);
  digitalWrite(LCD_SI_PIN,  HIGH);
  digitalWrite(LCD_RST_PIN, LOW); // reset lcd

  delay(10);
  digitalWrite(LCD_RST_PIN, HIGH);
  delay(10);
  LCD_init();

  // Initialize HC-SR04 LCD display
  pinMode(TRIG_PIN,OUTPUT);
  pinMode(ECHO_PIN,INPUT);

  //Initialize serial port
  Serial.begin(9600);
  Serial.println("HC-SR04 DEMO");

  // Initialize servo
  myservo.attach(9);   // attaches the servo on pin 9 to the servo object 
}


void loop()
{
  long range;
  range = Ranging(INC);
  ltoa(range,buffer,10);
  strcat(buffer," IN");
  LCD_puts2(buffer, 0 ); 
  Serial.print(range);
  Serial.println(" inches");
  pos = range;
  pos *= 2;               // Double servo travel for more drama 
  pos &= 0x7f;          // but limit rotation to 127 degrees	
  myservo.write(pos);     // tell servo to go to position in variable 'pos' 
    delay(15);            // waits 15ms for the servo to reach the position 
  delay(1000);            // slow to human speeds
}

long Timing()
{
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  duration = pulseIn(ECHO_PIN,HIGH);
  return duration;
}

long Ranging(int sys)
{
  Timing();
  distance_cm = duration /29 / 2 ;
  distance_inc = duration / 74 / 2;
  if (sys)
    return distance_cm;
  else
    return distance_inc;
}