/************************************************************************
  			microadam.cpp - Copyright Michael Bunk 2004
*************************************************************************/

#include "microadam.h"

#include "opencvext.h"
#include <iomanip>

using namespace std;

namespace seemicro
{

extern int DEBUGLEVEL;

std::ostream& operator<<(std::ostream& s, const MicroAdam& m)
{
  return s << Micro(m) << " beta: " << m.beta;
}

float MicroAdam::armlaenge(float radius)
{
  return radius*1.65;
}

float MicroAdam::armbreite(float radius)
{
  return armlaenge(radius)*0.2;
}

void MicroAdam::draw(IplImage* where,
  bool filled, double useColor, bool setZero, bool armeAuch)
{
#ifdef DEBUG
  if(DEBUGLEVEL>=3) cerr << "->MicroAdam::draw()\n";
#endif
  Micro::draw(where, filled, useColor, setZero);

  if(!armeAuch) return;

  double color;
  if(useColor==0) color = CV_RGB(30,30,30);
  else color = useColor;

  // Arm einzeichnen
  double breite = armbreite(radius);
  float alpha1 = -alpha;
  mypoint tmp;

  tmp.fromPolar(radius, alpha1);
  mypoint armansatzpunkt = mitte + tmp;

  tmp.fromPolar(armlaenge(radius), alpha1+beta);
  mypoint armendpunkt = armansatzpunkt - tmp;

  tmp.fromPolar(-breite/2,alpha1+beta-M_PI/2);
  mypoint e1 = armansatzpunkt + tmp;

  tmp.fromPolar( breite/2,alpha1+beta-M_PI/2);
  mypoint e2 = armansatzpunkt + tmp;

  tmp.fromPolar( breite/2,alpha1+beta-M_PI/2);
  mypoint e3 = armendpunkt    + tmp;

  tmp.fromPolar(-breite/2,alpha1+beta-M_PI/2);
  mypoint e4 = armendpunkt    + tmp;

  CvPoint pts[4];
  pts[0] = e1;
  pts[1] = e2;
  pts[2] = e3;
  pts[3] = e4;
  if(filled)
  {
    cvFillConvexPoly(where, pts, 4, color);
  }
  else
  {
    int npts = 4;
    CvPoint *ptsarr = &pts[0];
    cvPolyLine(where, &ptsarr, &npts, 1, 1, color);
  }

  // LED einzeichnen
  double ledcolor;
  if(useColor==0) ledcolor = CV_RGB(200,255,200);
  else ledcolor = useColor;

  tmp.fromPolar(armlaenge(radius)*0.92, alpha1+beta);
  mypoint ledposition = armansatzpunkt - tmp;
  if(filled)
  {
    cvCircle(where, ledposition, int(radius*0.05), ledcolor, CV_FILLED);
  }
  else
  {
    cvCircle(where, ledposition, int(radius*0.05), ledcolor);
  }
}

} // namespace seemicro

