#include "stdafx.h"
#include "token.h"
#include <math.h>
#include "moon.h"

Moon::Moon()
{
	_ov = NULL;
	_oa = NULL;
	_head = 0;
	_points = 0;
	_history = 0;
    _fixed = false;
    _m = 0.0;
    _steps = 0;
}


Moon::~Moon()
{
	DeleteArrays();
	_ov = NULL;
	_oa = NULL;
}


void Moon::DeleteArrays()
{
	if (_ov)
	{
		delete[] _ov[0];
		delete[] _oa[0];
		delete[] _ov;
		delete[] _oa;
	}
}


void Moon::InitArrays()
{
	DeleteArrays();

	_history = 2 * _points - 3;
	_head = _history;
	_ov = new Point*[_history * 2];
	_oa = new Point*[_history * 2];
	Point *ov = new Point[_history];
	Point *oa = new Point[_history];
	for (int iHist = 0; iHist < _history; ++iHist)
	{
		_ov[iHist] = _ov[iHist + _history] = &ov[iHist];
		_oa[iHist] = _oa[iHist + _history] = &oa[iHist];
	}
}




void Moon::ReadConfig(const char* spec)
{
    Tokenizer t;
    ASSERT(t.InitFromString(spec));
    if (t.Skip("x"))
    {
        _fixed = true;
    }
    t.Expect("p");
    t.Expect("(");
    if (1 != sscanf_s(t.Text(), "%lg", &_p._x)) t.Throw("p.x");
    ASSERT(t.Next());
    t.Skip(",");
    if (1 != sscanf_s(t.Text(), "%lg", &_p._y)) t.Throw("p.y");
    ASSERT(t.Next());
    t.Skip(",");
    if (1 != sscanf_s(t.Text(), "%lg", &_p._z)) t.Throw("p.z");
    ASSERT(t.Next());
    t.Expect(")");
    t.Expect("v");
    t.Expect("(");
    if (1 != sscanf_s(t.Text(), "%lg", &_v._x)) t.Throw("v.x");
    ASSERT(t.Next());
    t.Skip(",");
    if (1 != sscanf_s(t.Text(), "%lg", &_v._y)) t.Throw("v.y");
    ASSERT(t.Next());
    t.Skip(",");
    if (1 != sscanf_s(t.Text(), "%lg", &_v._z)) t.Throw("v.z");
    ASSERT(t.Next());
    t.Expect(")");
    if (1 != sscanf_s(t.Text(), "%lg", &_m)) t.Throw("mass");
	ASSERT(t.Next());

	_color = t.Color();

    ASSERT(t.Next());
    if (1 != sscanf_s(t.Text(), "%lg", &_size)) t.Throw("size");
    ASSERT(!t.Next());
}


// Add the attraction of this moon to the other, and the other to this
void Moon::Attract(Moon* other)
{
    Point p(_p);
    p -= other->_p;
    double scale = p.Dot();
    double dist = sqrt(scale);
    scale = 1.0/(scale * dist);
    _a.PlusCP(-other->_m * scale, p);
    other->_a.PlusCP((_m * scale), p);
}


void Moon::Step14()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-11];
	v.PlusCP(136462207.0 / 79833600.0, temp);
	temp = *a[-1];
	temp += *a[-10];
	v.PlusCP(-207556851.0 / 79833600.0, temp);
	temp = *a[-2];
	temp += *a[-9];
	v.PlusCP(867125681.0 / 79833600.0, temp);
	temp = *a[-3];
	temp += *a[-8];
	v.PlusCP(-1296919125.0 / 79833600.0, temp);
	temp = *a[-4];
	temp += *a[-7];
	v.PlusCP(1550731494.0 / 79833600.0, temp);
	temp = *a[-5];
	temp += *a[-6];
	v.PlusCP(-570841806.0 / 79833600.0, temp);

	v += *_ov[_head - 11];
	_p += v;
}


void Moon::Step13()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-10];
	v.PlusCP(25671198.0 / 14515200.0, temp);
	temp = *a[-1];
	temp += *a[-9];
	v.PlusCP(-48082866.0 / 14515200.0, temp);
	temp = *a[-2];
	temp += *a[-8];
	v.PlusCP(214734403.0 / 14515200.0, temp);
	temp = *a[-3];
	temp += *a[-7];
	v.PlusCP(-426775928.0 / 14515200.0, temp);
	temp = *a[-4];
	temp += *a[-6];
	v.PlusCP(713681566.0 / 14515200.0, temp);
	temp = *a[-5];
	v.PlusCP(-798789548.0 / 14515200.0, temp);

	v += *_ov[_head - 10];
	_p += v;
}


void Moon::Step12()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-9];
	v.PlusCP(1153247.0 / 725760.0, temp);
	temp = *a[-1];
	temp += *a[-8];
	v.PlusCP(-1055189.0 / 725760.0, temp);
	temp = *a[-2];
	temp += *a[-7];
	v.PlusCP(4412680.0 / 725760.0, temp);
	temp = *a[-3];
	temp += *a[-6];
	v.PlusCP(-3621776.0 / 725760.0, temp);
	temp = *a[-4];
	temp += *a[-5];
	v.PlusCP(2739838.0 / 725760.0, temp);

	v += *_ov[_head - 9];
	_p += v;
}


void Moon::Step11()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-8];
	v.PlusCP(666151.0 / 403200.0, temp);
	temp = *a[-1];
	temp += *a[-7];
	v.PlusCP(-841748.0 / 403200.0, temp);
	temp = *a[-2];
	temp += *a[-6];
	v.PlusCP(3606748.0 / 403200.0, temp);
	temp = *a[-3];
	temp += *a[-5];
	v.PlusCP(-5111276.0 / 403200.0, temp);
	temp = *a[-4];
	v.PlusCP(6989050.0 / 403200.0, temp);

	v += *_ov[_head - 8];
	_p += v;
}


// Use a multistep method to move this moon forward one step.
// This is an explicit time symmetric 9th degree multistep method, see
// http://burtleburtle.net/bob/math/multistep.html .  Specifically
// 0=p8-p9+p1-p0+dt*dt*(22081(a1+a8)-7337(a2+a7)+45765(a3+a6)-29(a4+a5))/15120
// This assumes current accelerations are already known.
// Masses and velocities have been scaled such that dt is 1.0 .
void Moon::Step10()
{
    Point** a = &_oa[_head];
    Point& v = _v;
    Point temp;

    v.Zero();
    temp = *a[0];
    temp += *a[-7];
    v.PlusCP( 22081.0/15120.0, temp);
    temp = *a[-1];
    temp += *a[-6];
    v.PlusCP( -7337.0/15120.0, temp);
    temp = *a[-2];
    temp += *a[-5];
    v.PlusCP( 45765.0/15120.0, temp);
    temp = *a[-3];
    temp += *a[-4];
    v.PlusCP( -29.0/15120.0, temp);
    
    v += *_ov[_head-7];
    _p += v;
}


void Moon::Step9()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-6];
	v.PlusCP(13207.0 / 8640.0, temp);
	temp = *a[-1];
	temp += *a[-5];
	v.PlusCP(-8934.0 / 8640.0, temp);
	temp = *a[-2];
	temp += *a[-4];
	v.PlusCP(42873.0 / 8640.0, temp);
	temp = *a[-3];
	v.PlusCP(-33812.0 / 8640.0, temp);

	v += *_ov[_head - 6];
	_p += v;
}



void Moon::Step8()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-5];
	v.PlusCP(317.0 / 240.0, temp);
	temp = *a[-1];
	temp += *a[-4];
	v.PlusCP(69.0 / 240.0, temp);
	temp = *a[-2];
	temp += *a[-3];
	v.PlusCP(334.0 / 240.0, temp);

	v += *_ov[_head - 5];
	_p += v;
}


void Moon::Step7()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-4];
	v.PlusCP(67.0 / 48.0, temp);
	temp = *a[-1];
	temp += *a[-3];
	v.PlusCP(-8.0 / 48.0, temp);
	temp = *a[-2];
	v.PlusCP(122.0 / 48.0, temp);

	v += *_ov[_head - 4];
	_p += v;
}



void Moon::Step6()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-3];
	v.PlusCP(7.0 / 6.0, temp);
	temp = *a[-1];
	temp += *a[-2];
	v.PlusCP(5.0 / 6.0, temp);

	v += *_ov[_head - 3];
	_p += v;
}


void Moon::Step5()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-2];
	v.PlusCP(5.0 / 4.0, temp);
	temp = *a[-1];
	v.PlusCP(2.0 / 4.0, temp);

	v += *_ov[_head - 2];
	_p += v;
}


void Moon::Step4()
{
	Point** a = &_oa[_head];
	Point& v = _v;
	Point temp;

	v.Zero();
	temp = *a[0];
	temp += *a[-1];
	v += temp;

	v += *_ov[_head - 1];
	_p += v;
}


// Leapfrog is the least accurate method, it's the 3rd degree
// explicit time symmetric multistep method.  But it needs only
// one previous measurement, which makes it useful for bootstrapping.
void Moon::Leapfrog() // step 3
{
    _v = *_ov[_head];
    _v += *_oa[_head];
    _p += _v;
}

// Advance _step and _head and save _a, _v to history.
void Moon::RecordStep()
{
    _steps++;
    _head++;
    if (_head == 2*_history)
        _head -= _history;
    *_ov[_head] = _v;
    *_oa[_head] = _a;
}


// Return approximate actual velocity at time ov[head+4]
// Here are some increasingly accurate velocity estimates:
//  1/2
// -1/12,    2/3
//  1/60,   -3/20,   3/4
// -1/280  , 4/105, -1/5,   4/5
//  1/1260, -5/504,  5/84, -5/21,   5/6
// -1/5544,  1/385, -1/56,  3/38, -15/56, 6/7
// We track old velocities rather than old positions because velocities
// tend to be tiny compared to positions, so velocities have more accuracy.
Point Moon::EstimateVelocity()
{
    Point** ov = &_ov[_head];
    Point v;
    Point t;  // note that t = _op[-4] - _op[-4] is 0, so is irrelevant
    // this assumes stepSize = 1.0, otherwise divide all scales by stepSize
    t = *ov[-3];
    t += *ov[-4];  // t = _op[-3] - _op[-5]
    v.PlusCP( 4.0/5.0, t);
    t += *ov[-2];
    t += *ov[-5];  // t = _op[-2] - _op[-6]
    v.PlusCP( -1.0/5.0, t);
    t += *ov[-1];
    t += *ov[-6];  // t = _op[-1] - _op[-7]
    v.PlusCP( 4.0/105.0, t);
    t += *ov[-0];
    t += *ov[-7];  // t = _op[-0] - _op[-8]
    v.PlusCP( -1.0/280.0, t);
    return v;
}

// given a point that just crossed Z=0 between t=0 and t=-1, estimate exactly when it crossed
double Moon::FindZeroZ()
{
    double p0 = _p._z;
    double p1 = p0 - _ov[_head]->_z;
    double p2 = p1 - _ov[_head-1]->_z;

    ASSERT((p0 > 0.0) != (p1 > 0.0) || p0 == 0.0, "%g %g %g", p0, p1, p2);

    double a = (-2*p1 + p0+p2)/2;
    double b = (p0-p2)/2;
    double c = p1;

    if (a != 0.0)
    {
        double rad = sqrt(b*b-4*a*c);
        double x0 = (-b - rad)/(2*a);
        double x1 = (-b + rad)/(2*a);
        return -1.0 - ((x0 > 0) ? x1 : x0);
    }
    else if (b != 0)
    {
        return -1.0 - c/b;
    }
    else
    {
        return -1.0;
    }
}


void Moon::UnitTest()
{
    // initialization
    Moon m;
	m._points = 10;
	m.InitArrays();
    Point p(1.0, 1.0, 1.0);
    ASSERTFG(m._p.Dot(p), 0.0);
    ASSERTFG(m._v.Dot(p), 0.0);
    ASSERTFG(m._a.Dot(p), 0.0);
    for (int iPos = 0; iPos < m._history; ++iPos)
    {
        ASSERT(m._ov[iPos] == m._ov[iPos+m._history]);
        ASSERTFG(m._ov[iPos]->_x, 0.0);
        ASSERTFG(m._ov[iPos]->_y, 0.0);
        ASSERTFG(m._ov[iPos]->_z, 0.0);
        ASSERT(m._oa[iPos] == m._oa[iPos+m._history]);
        ASSERTFG(m._oa[iPos]->_x, 0.0);
        ASSERTFG(m._oa[iPos]->_y, 0.0);
        ASSERTFG(m._oa[iPos]->_z, 0.0);
    }
    ASSERT(m._head == m._history);
    ASSERT(m._steps == 0);
    ASSERTFG(m._m, 0.0);

    // ZeroAcceleration()
    m.ZeroAcceleration();
    ASSERTFG(m._a.Dot(), 0.0);

    // Step() and EstimateVelocity()
    Point a(1.0, 2.0, 3.0);
    m._a = a;
    for (int i=0; i<m._points; ++i)
    {
        *m._oa[m._head-i] = a;
        m._ov[m._head-i]->PlusCP(-i, a);
    }
    while (m._steps < 4*m._points)
    {
        ASSERTFG(m._a._x, a._x);
        ASSERTFG(m._a._y, a._y);
        ASSERTFG(m._a._z, a._z);
        m.Step10();
		m.RecordStep();
        ASSERT((m._head % m._history) == ((m._history + m._steps) % m._history));
        u8 choose1 = m._steps;
        u8 choose2 = choose1 * (choose1 + 1) / 2;
        ASSERTFG(m._v._x, choose1 * a._x);
        ASSERTFG(m._v._y, choose1 * a._y);
        ASSERTFG(m._v._z, choose1 * a._z);
        ASSERTFG(m._p._x, a._x * choose2);
        ASSERTFG(m._p._y, a._y * choose2);
        ASSERTFG(m._p._z, a._z * choose2);
    }
    Point v = m.EstimateVelocity();
    ASSERTFG(v._x, (m._steps-3.5) * a._x);
    ASSERTFG(v._y, (m._steps-3.5) * a._y);
    ASSERTFG(v._z, (m._steps-3.5) * a._z);

    // FindZeroZ
    double t;

    m._p._z = 0;
    m._ov[m._head]->_z = 0;
    m._ov[m._head-1]->_z = 0;
    t = m.FindZeroZ();
    ASSERTFG(t, -1.0);

    m._p._z = 1;
    m._ov[m._head]->_z = 2;
    m._ov[m._head-1]->_z = 2;
    t = m.FindZeroZ();
    ASSERTFG(t, -0.5);

    m._p._z = 3;
    m._ov[m._head]->_z = 4;
    m._ov[m._head-1]->_z = -4;
    t = m.FindZeroZ();
    ASSERTFG(t, -0.5);
    
    printf("unit tested Moon\n");
}