/* p. 165, 25.6 */
double SunRightAscensionRad( double centuryTime)
{
    double omegaRad = OmegaRad(centuryTime);
    double oc = ObliquityCorrectionEx( centuryTime, omegaRad);
    double al = ApparentLongitudeSunEx( centuryTime, omegaRad);
    return atan2(cosd(oc) * sind(al), cosd(al));
}

static void astro_sunpos(double d, double *lon, double *r)
{
	double M,         /* Mean anomaly of the Sun */
	       w,         /* Mean longitude of perihelion */
	                  /* Note: Sun's mean longitude = M + w */
	       e,         /* Eccentricity of Earth's orbit */
	       E,         /* Eccentric anomaly */
	       x, y,      /* x, y coordinates in orbit */
	       v;         /* True anomaly */

	/* Compute mean elements */
	M = astro_revolution(356.0470 + 0.9856002585 * d);
	w = 282.9404 + 4.70935E-5 * d;
	e = 0.016709 - 1.151E-9 * d;

	/* Compute true longitude and radius vector */
	E = M + e * RADEG * sind(M) * (1.0 + e * cosd(M));
	x = cosd(E) - e;
	y = sqrt(1.0 - e*e) * sind(E);
	*r = sqrt(x*x + y*y);              /* Solar distance */
	v = atan2d(y, x);                  /* True anomaly */
	*lon = v + w;                        /* True solar longitude */
	if (*lon >= 360.0) {
		*lon -= 360.0;                   /* Make it 0..360 degrees */
	}
}

void sunpos( double d, double *lon, double *r )
/******************************************************/
/* Computes the Sun's ecliptic longitude and distance */
/* at an instant given in d, number of days since     */
/* 2000 Jan 0.0.  The Sun's ecliptic latitude is not  */
/* computed, since it's always very near 0.           */
/******************************************************/
{
    double M,         /* Mean anomaly of the Sun */
    w,         /* Mean longitude of perihelion */
    /* Note: Sun's mean longitude = M + w */
    e,         /* Eccentricity of Earth's orbit */
    E,         /* Eccentric anomaly */
    x, y,      /* x, y coordinates in orbit */
    v;         /* True anomaly */
    
    /* Compute mean elements */
    M = revolution( 356.0470 + 0.9856002585 * d );
    w = 282.9404 + 4.70935E-5 * d;
    e = 0.016709 - 1.151E-9 * d;
    
    /* Compute true longitude and radius vector */
    E = M + e * RADEG * sind(M) * ( 1.0 + e * cosd(M) );
    x = cosd(E) - e;
    y = sqrt( 1.0 - e*e ) * sind(E);
    *r = sqrt( x*x + y*y );              /* Solar distance */
    v = atan2d( y, x );                  /* True anomaly */
    *lon = v + w;                        /* True solar longitude */
    if ( *lon >= 360.0 )
        *lon -= 360.0;                   /* Make it 0..360 degrees */
}

void rotate_z(vector *v,double theta)
{
  double xNew,yNew;

  xNew = v->x*cosd(theta)+v->y*sind(theta);
  yNew = -v->x*sind(theta)+v->y*cosd(theta);
  v->x = xNew; v->y = yNew;
}

void rotate_y(vector *v,double theta)
{
  double xNew,zNew;

  zNew = v->z*cosd(theta)+v->x*sind(theta);
  xNew = -v->z*sind(theta)+v->x*cosd(theta);
  v->x = xNew; v->z = zNew;
}

/* -------------------------------------------------------------------------- */
void set_proj()
{
  double psx;
  double cell;
  double cell2;
  double r;
  double phictr;

  dddd = cntrj0 * pj.cds;
  sign = (pj.cenlat >= 0.0) ? 1.0 : -1.0;
  xn = 0.0;
  psi1 = 0.0;
  pole = sign*90.0;
  psi0 = (pole - pj.cenlat) * DEG_TO_RAD;
  if (pj.code == LAMBERT)
  {
    xn = log10(cosd(pj.stdlat1)) - log10(cosd(pj.stdlat2));
    xn = xn/(log10(tand(45.0-sign*pj.stdlat1*0.50)) -
             log10(tand(45.0-sign*pj.stdlat2*0.50)));
    psi1 = (90.0-sign*pj.stdlat1) * DEG_TO_RAD;
    psi1 = sign*psi1;
  }
  else if (pj.code == POLAR)
  {
    xn = 1.0;
    psi1 = (90.0-sign*pj.stdlat1) * DEG_TO_RAD;
    psi1 = sign*psi1;
  }
  if (pj.code != MERCATOR)
  {
    psx = (pole-pj.cenlat) * DEG_TO_RAD;
    if (pj.code == LAMBERT)
    {
      cell = RADIUS_EARTH*sin(psi1)/xn;
      cell2 = tan(psx*0.50) / tan(psi1*0.50);
    }
    else
    {
      cell = RADIUS_EARTH*sin(psx)/xn;
      cell2 = (1.0 + cos(psi1))/(1.0 + cos(psx));
    }
    r = cell*pow(cell2,xn);
    xcntr = 0.0;
    ycntr = -r;
    xc = 0.0;
    yc = -RADIUS_EARTH/xn*sin(psi1)*pow(tan(psi0*0.5)/tan(psi1*0.5),xn);
  }
  else {
    c2 = RADIUS_EARTH*cos(psi1);
    xcntr = 0.0;
    phictr = pj.cenlat * DEG_TO_RAD;
    cell = cos(phictr)/(1.0+sin(phictr));
    ycntr = -c2*log(cell);
    xc = xcntr;
    yc = ycntr;
  }
  return;
}

void fldpnt(double rrho,double rlat,double rlon,double ral,
			double rel,double r,double *frho,double *flat,
                        double *flon) {

   double rx,ry,rz,sx,sy,sz,tx,ty,tz;
   double sinteta;
  
   /* convert from global spherical to global cartesian*/

   sinteta=sind(90.0-rlat);
   rx=rrho*sinteta*cosd(rlon);
   ry=rrho*sinteta*sind(rlon);
   rz=rrho*cosd(90.0-rlat);

   sx=-r*cosd(rel)*cosd(ral);
   sy=r*cosd(rel)*sind(ral);
   sz=r*sind(rel);

   tx  =  cosd(90.0-rlat)*sx + sind(90.0-rlat)*sz;
   ty  =  sy;
   tz  = -sind(90.0-rlat)*sx + cosd(90.0-rlat)*sz;
   sx  =  cosd(rlon)*tx - sind(rlon)*ty;
   sy  =  sind(rlon)*tx + cosd(rlon)*ty;
   sz  =  tz;

   tx=rx+sx;
   ty=ry+sy;
   tz=rz+sz;

   /* convert from cartesian back to global spherical*/
   *frho=sqrt((tx*tx)+(ty*ty)+(tz*tz));
   *flat=90.0-acosd(tz/(*frho));
   if ((tx==0) && (ty==0)) *flon=0;
   else *flon=atan2d(ty,tx);
}

// Input x, y ,z and the angle vector
bool ikine(vector<double> *coords, vector<double> *angles, int grip) {
    //cout << endl << "Andy Ikine says, hello world" << endl << endl;

    double x = coords->at(0);
    double y = coords->at(1);
    double z = coords->at(2) - L1;

    // calculate the closes reach
    double reach_limit = L1*cosd(90) + L2*cosd(90 + (asin((-L1*sind(90))/L2) * 180 / M_PI) );
    //cout << "Min reach: " << reach_limit << endl;

    // Note that the dynamixel and rotate 150(degree) from origin
    double magnitude = sqrt( pow(x,2) + pow(y,2) + pow(z,2) );
    
    #if DEBUG
    cerr << "Resultant Vector: " << magnitude << endl;
    #endif

    if ( magnitude  > (L2 + L3) ) {
	cerr << "Out of reach" << endl;
		//return false;
    }

    // (horizontal, vertical) theta A
    angles->at(0) = atan2(x,y); 

    // converted hypotenuse as the new y value 
    y = y / cos(angles->at(0)); 

    // equation from the online source
    double temp1 = (pow(y,2) + pow(z,2) - pow(L2,2) - pow(L3,2)) / (2 * L2 * L3);
    double temp2 = -sqrt( 1 - pow(temp1, 2) );
	
    // theta C
    angles->at(2) = atan2( temp2, temp1);

    double k1 = L2 + L3 * cos(angles->at(2));
    double k2 = L3 * sin(angles->at(2));
	
    // theta B
    angles->at(1) = atan2( z, y ) - atan2( k2, k1 );

    angles->at(2) += M_PI / 2 - M_PI / 6;
    angles->at(1) -= M_PI / 2;
    
    // open : close
    angles->at(3) = grip ? 1.3 : -1.4;//(-GRIP_ANGLE / 180.0 * M_PI) ;
    //print_values(angles);

    return check_angle_range(angles);

}

void iterate(int value)
{
	int j, k;
	// ITERATIONS_PER_SEC is used to divide
	const double ITERATIONS_PER_SEC = 1000.0 / value;
	
	// call this function again in value milliseconds
	glutTimerFunc(value, iterate, value);
	
	// detect and resolve collisions
	// note: for simplicity, we assume that there are only 2 robots in the ring
	for(j = 1; j < robots_size; j++)
	{
		for(k = 0; k < j; k++)
		{
			double avg_width = (robots[j].width + robots[k].width) / 2.0;
			// if there's a collision between robots[j] and robots[k]
			if(magnitude(robots[j].pos.x - robots[k].pos.x, robots[j].pos.y - robots[k].pos.y) < avg_width)
			{
				point_t robot_j_corner;
				double half_j_width = robots[j].width / 2.0;
				
				robot_j_corner = offset_from_robot(&robots[j], half_j_width, half_j_width);
				if(point_in_robot(&robots[k], &robot_j_corner))
				{
					// place code to figure out where the robots hit, now that we know there's a collision
				}
			}
		}
	}
	
	// update robots positions and angle (with the velocity and angular velocity)
	for(j = 0; j < robots_size; j++)
	{
		// If the robot's angular velocity is great enough, use a more accurate model
		if(fabs(robots[j].rotv) < 10.0)
		{
			robots[j].pos.x += robots[j].v * cosd(robots[j].rot) / ITERATIONS_PER_SEC;
			robots[j].pos.y += robots[j].v * sind(robots[j].rot) / ITERATIONS_PER_SEC;
		}
		else
		{
			robots[j].pos.x += robots[j].v * 180.0 / M_PI 
				* (sind(robots[j].rotv / ITERATIONS_PER_SEC + robots[j].rot) 
				- sind(robots[j].rot)) / robots[j].rotv;
			robots[j].pos.y += robots[j].v * 180.0 / M_PI 
				* (-cosd(robots[j].rotv / ITERATIONS_PER_SEC 
				+ robots[j].rot) + cosd(robots[j].rot)) / robots[j].rotv;
		}
		robots[j].rot += robots[j].rotv / ITERATIONS_PER_SEC;
	}
}

void ObservationPointSet(long double NS,long double EW,long double ro)
{
			if (NS <  -90) NS =  -90;
			if (NS >   90) NS =  -90;
			if (EW < -180) EW = -180;
			if (EW >  180) EW =  180;

			longitude = EW;
			LAT = NS - 0.19241666667 * sind(NS * 2);	/* 天文緯度 */
			RLT = ((0.998327112 + 0.001676399 * cosd(NS * 2) - 0.000003519 * cosd(NS * 4)) * 6378140 + ro) / 6371012;
			if (RLT < 0) RLT = 0;
			return;
}

QVector3D rotateXYZ(QVector3D vector, QVector3D rotation) {
    float x, y, z, x_ = vector.x(), y_ = vector.y(), z_ = vector.z();

    //Rotation autour de X
    x = x_, y = y_ * cosd(rotation.x()) - z_ * sind(rotation.x()), z = y_ * sind(rotation.x()) + z_ * cosd(rotation.x());

    //Rotation autour de Y
    x_ = x * cosd(rotation.y()) + z * sind(rotation.y()), y_ = y, z_ = z * cosd(rotation.y()) - x * sind(rotation.y());

    //Rotation autour de Z
    x = x_ * cosd(rotation.z()) - y_ * sind(rotation.z()), y = x_ * sind(rotation.z()) + y_ * cosd(rotation.z()), z = z_;

    return QVector3D(x, y, -z);
}

int sphpad(
  int nfield,
  double lng0,
  double lat0,
  const double dist[],
  const double pa[],
  double lng[],
  double lat[])

{
  int i;
  double eul[5];

  /* Set the Euler angles for the coordinate transformation. */
  eul[0] = lng0;
  eul[1] = 90.0 - lat0;
  eul[2] = 0.0;
  eul[3] = cosd(eul[1]);
  eul[4] = sind(eul[1]);

  for (i = 0; i < nfield; i++) {
    /* Latitude in the new frame is obtained from angular distance. */
    lat[i] = 90.0 - dist[i];

    /* Longitude in the new frame is obtained from position angle. */
    lng[i] = -pa[i];
  }

  /* Transform field points to the old system. */
  sphx2s(eul, nfield, 0, 1, 1, lng, lat, lng, lat);

  return 0;
}

int sphdpa(
  int nfield,
  double lng0,
  double lat0,
  const double lng[],
  const double lat[],
  double dist[],
  double pa[])

{
  int i;
  double eul[5];

  /* Set the Euler angles for the coordinate transformation. */
  eul[0] = lng0;
  eul[1] = 90.0 - lat0;
  eul[2] = 0.0;
  eul[3] = cosd(eul[1]);
  eul[4] = sind(eul[1]);

  /* Transform field points to the new system. */
  sphs2x(eul, nfield, 0, 1, 1, lng, lat, pa, dist);

  for (i = 0; i < nfield; i++) {
    /* Angular distance is obtained from latitude in the new frame. */
    dist[i] = 90.0 - dist[i];

    /* Position angle is obtained from longitude in the new frame. */
    pa[i] = -pa[i];
    if (pa[i] < -180.0) pa[i] += 360.0;
  }

  return 0;
}

/*
** Hapgood defines a transformation between GSE and HEE in his 1992
** paper (section 6), but this part isn't online.  
**
** The gist of it is, we rotate 180 degrees about Z, and then translate
** along X.
**
** But we also need to add "R", a constant vector defined by
**
**      R = [ Rsun, 0, 0 ]
**
** where
**
**             r0 (1 - e^2)
**    Rsun =   ------------
**             1 + e cos(v)
**
**   r0 = 1.495985E8 km        	mean distance of the Sun from Earth.
**
**    e = 0.016709 - 0.0000418T0	eccentricity of the Sun's apparent
**					orbit around the Earth.
**
**    w = 282.94 + 1.72 T0		longitude of perigee of that orbit
**
**    v = lambda0 - w			(see lambda0 above)
**
**
** Implemented by Ed Santiago, Updated by Kristi Keller
*/
int
gse_twixt_hee(const double et, Vec v_in, Vec v_out, Direction direction)
{
  Mat mat;
  double r0,e, w,v, Rsun;
  hapgood_matrix(180, Z, mat);

  /*
  ** Note that there's no transposition here if the direction is "back";
  ** the operation works identically in both directions.
  */
  mat_times_vec(mat, v_in, v_out);

  /* Translate the X axis about the earth-sun distance */
  r0 = (double)1.495985e8;  
  e = 0.016709 - 0.0000418*T0(et);
  w = 282.94 + 1.72*T0(et);
  v = lambda0(et) - w; 
  Rsun = r0*(1-e*e)/(1.+e*cosd(v)); 
  /*  v_out[0] += (double)1.5e8;  */

  v_out[0] += Rsun;

  return 0;
}

/* -------------------------------------------------------------------------- */
void latlon_to_ij(float latitude, float longitude, float *ri, float *rj)
{
  double cell;
  double ylon;
  double flp;
  double psx;
  double xx = 0;
  double yy = 0;
  double r;

  if (! is_init)
  {
    fprintf(stderr, "Using latlon_to_ij without projection init !!!\n");
    *ri = -999;
    *rj = -999;
    return;
  }

  if (pj.code == MERCATOR)
  {
    if (latitude != -90.0)
    {
      cell = cosd(latitude)/(1.0+sind(latitude));
      yy = -c2*log(cell);
      xx =  c2*((longitude-pj.cenlon)*DEG_TO_RAD);
      if (pj.cenlon > 0.0 && xx < -dddd)
      {
        xx = xx + 2.0*c2*((180.0+pj.cenlon)*DEG_TO_RAD);
      }
      else if (pj.cenlon < 0.0 && xx > dddd)
      {
        xx = xx - c2*(360.0*DEG_TO_RAD);
      }
    }
  }
  else
  {
    ylon = longitude - pj.cenlon;
    if (ylon > 180.0) ylon -= 360.0;
    if (ylon < -180.0) ylon += 360.0;
    flp = xn*(ylon*DEG_TO_RAD);
    psx = (pole - latitude) * DEG_TO_RAD;
    r = -RADIUS_EARTH/xn*sin(psi1)*pow(tan(psx*0.50)/tan(psi1*0.50),xn);
    if (pj.cenlat < 0)
    {
      xx = r*sin(flp);
      yy = r*cos(flp);
    }
    else
    {
      xx = -r*sin(flp);
      yy = r*cos(flp);
    }
  }

  *ri = (xx - xc) / pj.ds + cntrj;
  *rj = (yy - yc) / pj.ds + cntri;

  return;
}

/******************************************************
 * get_tv:
 * Returns a "target state vector", containing the
 * position and velocity of the given point on the
 * earth's surface.*/
stateVector get_tv(double RE,double RP,double lat,double lon)
{
  double Rn;
  stateVector v;
  double ecc2=1-RP*RP/(RE*RE);

  Rn = RE/sqrt(1-ecc2*SQR(sind(lat)));
  v.pos.x = (Rn)*cosd(lon)*cosd(lat);
  v.pos.y = (Rn)*sind(lon)*cosd(lat);
  v.pos.z = (Rn*(1-ecc2))*sind(lat);
  /*fixed2gei with gha 0.0 simply sets the velocity
  of the target point to the rotation rate of the earth.*/
  v.vel.x=v.vel.y=v.vel.z=0.0;
  fixed2gei(&v,0.0);
  return v;
}

/*
** The HAE to HEEQ transformation is given by the matrix
**
** 	S2 = <theta0,Z>*<i,X>*<Omega,Z>
**
** where the rotation angle theta0 is the the longitude of the Sun's
** central meridian, i is the the inclination of the Sun's equator and
** Omega is the the ecliptic longitude of the ascending node of the Sun's
** equator. This transformation comprises a rotation in the plane of the
** ecliptic from the First Point of Aries to the ascending node of the
** solar equator, then a rotation from the plane of the ecliptic to the 
** plane of the equator and finally a rotation in the plane of the solar
** equator from the ascending node to the central meridian. 
**
** Implemented by Kristi Keller on 2/2/2004
*/
void
mat_S2(const double et, Mat mat)
{
  Mat mat_tmp;
  double Omega = 73.6667+0.013958*(MJD(et)+3242)/365.25;
  double theta0 = atand(cosd(7.25) * tand(lambda0(et)-Omega));
  double angle_1 = lambda0(et)-Omega;
  angle_1=fmod(angle_1,360.0);
  if (angle_1 < 0.0) angle_1+=360.0;

  theta0=fmod(theta0,360.0);
  if (theta0 < 0.0) theta0+=360.0;
  if (angle_1 < 180.0) {
      if (theta0 < 180.0) theta0+=180.0;
       }
  if (angle_1 > 180.0) {
      if (theta0 > 180.0) theta0-=180.0;
       }
  hapgood_matrix(theta0, Z, mat);

  hapgood_matrix(7.25, X, mat_tmp);
  mat_times_mat(mat, mat_tmp, mat);

  hapgood_matrix(Omega, Z, mat_tmp);
  mat_times_mat(mat, mat_tmp, mat);
}

void geocnvrt(double gdlat,double gdlon,
			  double xal,double xel,double *ral,double *rel) {

  double kxg,kyg,kzg,kxr,kyr,kzr;
  double rrad,rlat,rlon,del;

  kxg=cosd(xel)*sind(xal);
  kyg=cosd(xel)*cosd(xal);
  kzg=sind(xel);
  geodtgc(1,&gdlat,&gdlon,&rrad,&rlat,&rlon,&del);
  kxr=kxg;
  kyr=kyg*cosd(del)+kzg*sind(del);
  kzr=-kyg*sind(del)+kzg*cosd(del);

  *ral=atan2d(kxr,kyr);
  *rel=atand(kzr/sqrt((kxr*kxr)+(kyr*kyr)));
}

/*	FUNCTION:		Quaternion :: GenerateFromEuler
	ARGUMENTS:		roll (degrees)
					pitch (degrees)
					yaw (degrees)
	RETURN:			n/a
	DESCRIPTION:	Creates quaternion from euler angles.  Borowed from Bullet
*/
void Quaternion :: GenerateFromEuler(float roll, float pitch, float yaw)
{
	float halfYaw = 0.5f * yaw;
	float halfPitch = 0.5f * pitch;
	float halfRoll = 0.5f * roll;
	float cosYaw = cosd(halfYaw);
	float sinYaw = sind(halfYaw);
	float cosPitch = cosd(halfPitch);
	float sinPitch = sind(halfPitch);
	float cosRoll = cosd(halfRoll);
	float sinRoll = sind(halfRoll);

	x = sinRoll*cosPitch*cosYaw - cosRoll*sinPitch*sinYaw;
	y = cosRoll*sinPitch*cosYaw + sinRoll*cosPitch*sinYaw;
	z = cosRoll*cosPitch*sinYaw - sinRoll*sinPitch*cosYaw;
	w = cosRoll*cosPitch*cosYaw + sinRoll*sinPitch*sinYaw;
}

Geometry Geometry::fromDisc(float w, float h, int e) //static
{
    if (e<3) throw GeometryError("fromDisc: Must have at least 3 edges!");

    Geometry geo;
    Geometry::Triangle tri;
    Geometry::Vertex vert;

    const float dDeg = 360.f/float(e);

    w *= 0.5;
    h *= 0.5;

    vert.pos  = Vec3{0.f, 0.f, 0.f};
    vert.norm = Vec3{0.f, 0.f, 1.f};
    vert.tex  = Vec2{0.5f, 0.5f};

    geo.vertices.push_back(vert);
    tri[0] = 0;
    tri[1] = 0;
    tri[2] = 0;

    float deg = -dDeg;
    vert.pos = Vec3{cosd(deg)*w, sind(deg)*h, 0.f};
    vert.tex = Vec2{cosd(deg),   sind(deg)};

    tri[1] = 1;
    geo.vertices.push_back(vert);

    for (int i=0; i<e-1; ++i)
    {
        deg = dDeg*i;
        vert.pos = Vec3{cosd(deg)*w, sind(deg)*h, 0.f};
        vert.tex = Vec2{cosd(deg),   sind(deg)};

        tri[2] = geo.vertices.size();
        geo.vertices.push_back(vert);
        geo.triangles.push_back(tri);
        tri[1] = tri[2];
    }

    tri[2] = 1;
    geo.triangles.push_back(tri);

    return geo;
}