/*
 GerberConversion
 Copyright (C) 2010 Martin Štrympl
 
 This program is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public
 License as published by the Free Software Foundation; either
 version 3.0 of the License, or (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 General Public License for more details.
 
 You should have received a copy of the GNU General Public
 License along with this program.
 
 Martin Štrympl
 xstrym00@stud.feec.vutbr.cz
 */

#include "Gcodes.h"

#include "Gerber.h"

#define MY_PI (3.141592653589793L)

Mac_Fun_Gcode_Gcodes_Gcode(Gcode_01)
{
  struct sGerber_Point s_A, s_B;
  
  s_A=ps_actual->s_xy.s_old;
  s_B=ps_actual->s_xy.s_new;
  
  if (ps_actual->s_sets.i4_debug>2) printf("Draw transform line from [%6.6f,%6.6f] to [%6.6f,%6.6f].\n",
                                           (double)s_A.ld_x, (double)s_A.ld_y, (double)s_B.ld_x, (double)s_B.ld_y);

  // use offset
  s_B.ld_x+=ps_actual->s_xy.s_offset.ld_x;
  s_B.ld_y+=ps_actual->s_xy.s_offset.ld_y;
  
  // transform
  if (!RealPoint(ps_actual, &s_A, &s_A))
  {
    printf("Transform old point from axis X and Y to axis A and B failed.\n");
    return -1;
  }
  if (!RealPoint(ps_actual, &s_B, &s_B))
  {
    printf("Transform new point from axis X and Y to axis A and B failed.\n");
    return -1;
  }
  
  Gerber_Gerber_LineSegment(ps_gerber, &s_A, &s_B, false);
  
  return 0;
}
Mac_Fun_Gcode_Gcodes_Gcode(Gcode_02)
{
  // clockwise
  struct sGerber_Point s_A, s_B, s_center;
  double do_aglefrom, do_agleto;
  double do_radius;
  
  s_A=ps_actual->s_xy.s_old;
  s_B=ps_actual->s_xy.s_new;
  
  if (ps_actual->s_sets.i4_debug>2) printf("Draw with clockwise circle interpolation from [%6.6f,%6.6f] to [%6.6f,%6.6f].\n",
                                           (double)s_A.ld_x, (double)s_A.ld_y, (double)s_B.ld_x, (double)s_B.ld_y);
  
  s_center=s_A;
  s_center.ld_x+=ps_actual->s_xy.s_offset.ld_x;
  s_center.ld_y+=ps_actual->s_xy.s_offset.ld_y;
  
  // check distance
  do_aglefrom=(s_center.ld_x-s_A.ld_x)*(s_center.ld_x-s_A.ld_x)+(s_center.ld_y-s_A.ld_y)*(s_center.ld_y-s_A.ld_y);
  do_agleto=(s_center.ld_x-s_B.ld_x)*(s_center.ld_x-s_B.ld_x)+(s_center.ld_y-s_B.ld_y)*(s_center.ld_y-s_B.ld_y);
  
  if (abs(do_aglefrom-do_agleto)>0.0000001l)
  {
    printf("Bad set center point. Distance between center point and start point is distinct from ditance between center point and end point.\n");
    return -1;
  }
  
  //agles
  do_radius=do_aglefrom;
  do_aglefrom=atan2((s_A.ld_y-s_center.ld_y)/do_radius, (s_A.ld_x-s_center.ld_x)/do_radius);
  do_agleto=atan2((s_A.ld_y-s_center.ld_y)/do_radius, (s_A.ld_x-s_center.ld_x)/do_radius);
  
  // transform
  if (!RealPoint(ps_actual, &s_A, &s_A))
  {
    printf("Transform old point from axis X and Y to axis A and B failed.\n");
    return -1;
  }
  if (!RealPoint(ps_actual, &s_B, &s_B))
  {
    printf("Transform new point from axis X and Y to axis A and B failed.\n");
    return -1;
  }
  if (!RealPoint(ps_actual, &s_center, &s_center))
  {
    printf("Transform center point from axis X and Y to axis A and B failed.\n");
    return -1;
  }
  
  // drive segment
  return Gerber_Gerber_OvalSegment(ps_gerber, &s_center, do_radius, do_radius, (do_aglefrom/MY_PI)*180.0L, (do_agleto/MY_PI)*180.0L, false);

  return 0;
}
Mac_Fun_Gcode_Gcodes_Gcode(Gcode_03)
{
  // counter clockwise
  struct sGerber_Point s_A, s_B, s_center;
  double do_aglefrom, do_agleto;
  double do_radius;
  
  s_A=ps_actual->s_xy.s_old;
  s_B=ps_actual->s_xy.s_new;
  
  if (ps_actual->s_sets.i4_debug>2) printf("Draw transform counter clockwise circle interpolation from [%6.6f,%6.6f] to [%6.6f,%6.6f].\n",
                                           (double)s_A.ld_x, (double)s_A.ld_y, (double)s_B.ld_x, (double)s_B.ld_y);
  
  s_center=s_A;
  s_center.ld_x+=ps_actual->s_xy.s_offset.ld_x;
  s_center.ld_y+=ps_actual->s_xy.s_offset.ld_y;
  
  // check distance
  do_aglefrom=(s_center.ld_x-s_A.ld_x)*(s_center.ld_x-s_A.ld_x)+(s_center.ld_y-s_A.ld_y)*(s_center.ld_y-s_A.ld_y);
  do_agleto=(s_center.ld_x-s_B.ld_x)*(s_center.ld_x-s_B.ld_x)+(s_center.ld_y-s_B.ld_y)*(s_center.ld_y-s_B.ld_y);
  
  if (abs(do_aglefrom-do_agleto)>0.0000001l)
  {
    printf("Bad set center point. Distance between center point and start point is distinct from ditance between center point and end point.\n");
    return -1;
  }
  
  // agles
  do_radius=do_aglefrom;
  do_aglefrom=atan2((s_A.ld_y-s_center.ld_y)/do_radius, (s_A.ld_x-s_center.ld_x)/do_radius);
  do_agleto=atan2((s_B.ld_y-s_center.ld_y)/do_radius, (s_B.ld_x-s_center.ld_x)/do_radius);
  
  // transform
  if (!RealPoint(ps_actual, &s_A, &s_A))
  {
    printf("Transform old point from axis X and Y to axis A and B failed.\n");
    return -1;
  }
  if (!RealPoint(ps_actual, &s_B, &s_B))
  {
    printf("Transform new point from axis X and Y to axis A and B failed.\n");
    return -1;
  }
  if (!RealPoint(ps_actual, &s_center, &s_center))
  {
    printf("Transform new point from axis X and Y to axis A and B failed.\n");
    return -1;
  }
  do_radius=sqrt((s_center.ld_x-s_A.ld_x)*(s_center.ld_x-s_A.ld_x)+(s_center.ld_y-s_A.ld_y)*(s_center.ld_y-s_A.ld_y));
  
  // count segment
  return Gerber_Gerber_OvalSegment(ps_gerber, &s_center, do_radius, do_radius, (do_aglefrom/MY_PI)*180.0L, (do_agleto/MY_PI)*180.0L, false);
  
  return 0;
}
