#include <LiquidCrystal.h>
#include <Servo.h>
#include <U8g2lib.h>
#include <Wire.h>

int rs=8,e=2,d4=4,d5=5,d6=6,d7=7;
int tr=10,ec=9,sv=3,ra=A0;
float p=0.0;
int d=0,a=0,f=1;
float pt=0;
float ax=0.0,ay=0.0,v=0.0;
byte obj[180][2];

LiquidCrystal lcd(rs,e,d4,d5,d6,d7);
Servo servo;
U8G2_SH1107_SEEED_128X128_1_HW_I2C OLED(U8G2_R0);

void setup() {
  Wire.begin();
  lcd.begin(16,2);
  servo.attach(sv);
  OLED.begin();
  Serial.begin(9600);
  pinMode(tr,OUTPUT);
  pinMode(ec,INPUT);
  pinMode(sv,OUTPUT);
  pinMode(ra,INPUT);
  delayMicroseconds(1000);
  clean(0);
  lcd.setCursor(0,0);
  lcd.print("Scan Range: ");
}

void scan(int a)
{

p=analogRead(ra)*0.390625;
servo.write(a);
digitalWrite(tr,LOW);
delayMicroseconds(5);
digitalWrite(tr,HIGH);
delayMicroseconds(10);
digitalWrite(tr,LOW);
d=pulseIn(ec,HIGH,((p*58.31)+50));
double dist=(d*0.01715);
lcd.setCursor(0,1);
lcd.print("Deg-");
lcd.setCursor(4,1);
lcd.print(String(a));
lcd.setCursor(8,1);
lcd.print("Sp:");
if(dist<=p)
{
int x=((int)floor(dist*cos(a*0.017453)*0.08)+32);
int y=(int)floor(dist*sin(a*0.017453)*0.08);
obj[a-1][0]=(byte)x;
obj[a-1][1]=(byte)y;
if(a%25==0)
draw();

}
lcd.setCursor(11,1);
lcd.print("     ");
lcd.setCursor(11,1);
lcd.print(v);

}


void clean(int i){
  lcd.setCursor(0,i);
  lcd.print("                ");
}

void draw()
{

 OLED.firstPage();
  do{

for(int i=0;i<180;i++)
  OLED.drawPixel(obj[i][0],obj[i][1]);

  }while(OLED.nextPage());
  

}
void vel()
{

float pax=ax;
float pay=ay;
ax=0;
ay=0;

for(int i=0;i<180;i++)
{
ax=ax+(obj[i][0]*12.5);
ay=ay+(obj[i][1]*12.5);
}
ax=ax*0.0056;
ay=ay*0.0056;
float t=abs((millis()-pt))*0.001;
v=(float)((float)sqrt(sq(ax-pax)+sq(ay-pay))/t);
pt=millis();
}

void loop() {
  p=analogRead(ra)*0.390625;
  lcd.setCursor(12,0);
  lcd.print(String(p));
  a=a+f;
  if(a==0){
    f=1;
    a=1;
    clean(1);
  }

  if(a==99)
    clean(1);

  if(a==181){
    f=-1;
    a=179;
    clean(1);
  }
if((a%50)==0)
  vel();
  scan(a);
}