1 files changed, 129 insertions, 28 deletions

diff --git a/src/outputwindow.cc b/src/outputwindow.cc
index faa4432..43e691f 100644
--- a/src/outputwindow.cc
+++ b/src/outputwindow.cc
@@ -32,15 +32,10 @@
 
 #include <math.h>
 
-#define SCALE 2
-
 OutputWindow::OutputWindow()
 {
   resize(1000, 1000);
 
-  QPixmap img("gfx/kaiman.png");
-  //QPixmap img("gfx/arrow.png");
-  kaiman = img.toImage();
   /*
   QTransform t;
   t.scale(0.2, 0.2);
@@ -58,6 +53,33 @@ void OutputWindow::stopScript()
   stop = true;
 }
 
+void OutputWindow::loadPen(QString file)
+{
+  sem.acquire();
+  penfile = file;
+  loadpen = true;
+}
+
+void OutputWindow::setScale(double s)
+{
+  scale = s;
+  sem.acquire();
+}
+
+void OutputWindow::setSpeed(int s)
+{
+  if(s < 1) speed = 100;
+  else if(s > 100) speed = 1;
+  else speed = 101 - s;
+  sem.acquire();
+}
+
+void OutputWindow::setColour(int r, int g, int b, int a)
+{
+  colour = QColor(r, g, b, a);
+  sem.acquire();
+}
+
 void OutputWindow::timeout()
 {
   /*
@@ -75,28 +97,82 @@ void OutputWindow::paintEvent(QPaintEvent *)
   QPainter p(this);
   //  QTransform tp; tp.scale(3, 3); p.setTransform(tp);
 
+  if(loadpen) {
+    QPixmap img(penfile);
+    kaiman = img.toImage();
+    loadpen = false;
+  }
+  
   QTransform t;
   t.rotate(-r + 90);
-  t.scale((100.0 / kaiman.width()) * SCALE,
-          (100.0 / kaiman.width()) * SCALE);
+  t.scale((100.0 / kaiman.width() * scale),
+          (100.0 / kaiman.width() * scale) );
   QImage img = kaiman.transformed(t, Qt::SmoothTransformation);
-  p.drawImage((x * SCALE) - img.width()/2 ,
-              (y * SCALE)- img.height()/2, img);
+  p.drawImage((x * scale) - img.width()/2 + width() / 2,
+              (y * scale) - img.height()/2 + height() / 2, img);
+
+  QTransform gt;
+  gt.translate(width() / 2, height() / 2);
+  gt.scale(scale, scale);
+  p.setTransform(gt);
 
-  QPen pen(QColor(255,0,0,100));
-  pen.setWidth(4);
+  QPen pen;
+  pen.setWidth(6);
+  /*
+    p.setPen(pen);
+    p.drawLines(points);
+  */
+  for(int i = 0; i < lines.size(); i++) {
+    ColLine l = lines[i];
+    pen.setColor(l.colour);
+    p.setPen(pen);
+    QLineF line = l.line;
+    line.setP1(line.p1());
+    line.setP2(line.p2());
+    p.drawLine(line);
+  }
+
+  pen.setWidth(2);
+  pen.setStyle(Qt::DotLine);
+  //  pen.setCapStyle(Qt::RoundCap);
+  pen.setColor(Qt::black);
+
+  //  pen.setColor(colour);
   p.setPen(pen);
-  p.drawLines(points);
+  for(int i = 0; i < current_points.size(); i+=2) {
+    QPointF p1 = current_points[i];
+    QPointF p2 = current_points[i + 1];
+    p.drawLine(QLineF(p1, p2));
+  }
+
+  QColor c = colour;
+  c.setAlpha(c.alpha() / 4);
+  pen.setStyle(Qt::SolidLine);
+  pen.setWidth(6);
+  pen.setColor(c);
+  p.setPen(pen);
+  for(int i = 0; i < current_points.size(); i+=2) {
+    QPointF p1 = current_points[i];
+    QPointF p2 = current_points[i + 1];
+    p.drawLine(QLineF(p1, p2));
+  }
 
   sem.release();
 }
 
 void OutputWindow::reset()
 {
-  points.clear();
-  x = (width() / 2) / SCALE;
-  y = (height() / 2) / SCALE;
+  scale = 2.0;
+  speed = 50;
+  penfile = "gfx/kaiman.png";
+  loadpen = true;
+
+  lines.clear();
+  current_points.clear();
+  x = 0;//(width() / 2) / scale;
+  y = 0;//(height() / 2) / scale;
   r = 0;
+  colour = QColor(150, 0, 0, 150);
   stop = false;
   while(sem.tryAcquire()) {}
 }
@@ -107,31 +183,56 @@ static inline int sign(int x)
   return -1;
 }
 
-void OutputWindow::forward(int x)
+void OutputWindow::forward(int dist)
 {
   sem.acquire();
-  float target_x = sin(r * (M_PI / 180.0)) * x;
-  float target_y = cos(r * (M_PI / 180.0)) * x;
+  float target_x = sin(r * (M_PI / 180.0)) * dist;
+  float target_y = cos(r * (M_PI / 180.0)) * dist;
+
+  float source_x = x;
+  float source_y = y;
+  
+  int spd = abs(dist * speed) / 60;//28;
+  for(int i = 0; i < spd; i++) {
+    if(stop) return;
+    float d = (float)i / (float)spd;
 
-  float source_x = this->x;
-  float source_y = this->y;
+    current_points.clear();
+    current_points.append(QPointF(source_x, source_y));
+
+    x = source_x * (1 - d) + (source_x + target_x) * d;
+    y = source_y * (1 - d) + (source_y + target_y) * d;
+
+    current_points.append(QPointF(x, y));
 
-  for(int i = 0; i < abs(x); i++) {
-    if(stop) return;
-    float d = (float)i / (float)x * sign(x);
-    this->x = source_x + target_x * d;
-    this->y = source_y + target_y * d;
-    points.append(QPointF(this->x * SCALE, this->y * SCALE));
     sem.acquire();
   }
+  x = source_x + target_x;
+  y = source_y + target_y;
+
+  ColLine l;
+  l.colour = colour;
+  l.line = QLine(QPoint(source_x, source_y),
+                 QPoint(x, y));
+  lines.append(l);
+
+
+  current_points.clear();
+  sem.acquire();
 }
 
+// Turn x degrees
 void OutputWindow::turn(int x)
 {
+  int spd = (speed * abs(x) / 80);
+  float offset = this->r;
   sem.acquire();
-  for(int i = 0; i < abs(x)/2; i++) {
+  for(int i = 0; i < spd; i++) {
     if(stop) return;
-    this->r += sign(x)*2;
+    float p = (float)i / (float)spd;
+    this->r = offset + (float)x * p;
     sem.acquire();
   }
+  this->r = offset + x;
+  sem.acquire();
 }