#include "SPG4Function.h"
#include <Tle.h>
#include <SGP4.h>
#include <Observer.h>
#include <CoordGeodetic.h>
#include <CoordTopocentric.h>

#include <list>
#include <string>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <vector>
#include <cstdlib>



std::vector<SatelliteAntPos> RunTle(libsgp4::Tle tle, double start, double end, double inc, bool printfinfoflag)
{
    std::vector<SatelliteAntPos> resultpos(0);
    double current = start;
    libsgp4::SGP4 model(tle);
    bool running = true;
    bool first_run = true;

    std::cout << std::setprecision(0) << tle.NoradNumber() << " xx"
        << std::endl;

    while (running)
    {
        bool error = false;
        libsgp4::Vector position;
        libsgp4::Vector velocity;
        double tsince;

        try
        {
            if (first_run && current != 0.0)
            {
                /*
                 * make sure first run is always as zero
                 */
                tsince = 0.0;
            }
            else
            {
                /*
                 * otherwise run as normal
                 */
                tsince = current;
            }

            libsgp4::Eci eci = model.FindPosition(tsince);
            position = eci.Position();
            velocity = eci.Velocity();
        }
        catch (libsgp4::SatelliteException& e)
        {
            std::cerr << e.what() << std::endl;
            error = true;
            running = false;
        }
        catch (libsgp4::DecayedException& e)
        {
            std::cerr << e.what() << std::endl;

            position = e.Position();
            velocity = e.Velocity();

            if (!first_run)
            {
                // print out position on first run
                error = true;
            }

            running = false;
        }

        if (!error)
        {
            SatelliteAntPos antpos{};
            antpos.time = tsince;
            antpos.Px = position.x;
            antpos.Py = position.y;
            antpos.Pz = position.z;
            antpos.Vx = velocity.z;
            antpos.Vy = velocity.z;
            antpos.Vz = velocity.z;

            resultpos.push_back(antpos);
            if (printfinfoflag) {
                std::cout << std::setprecision(8) << std::fixed;
                std::cout.width(17);
                std::cout << tsince << " ";
                std::cout.width(16);
                std::cout << position.x << " ";
                std::cout.width(16);
                std::cout << position.y << " ";
                std::cout.width(16);
                std::cout << position.z << " ";
                std::cout << std::setprecision(9) << std::fixed;
                std::cout.width(14);
                std::cout << velocity.x << " ";
                std::cout.width(14);
                std::cout << velocity.y << " ";
                std::cout.width(14);
                std::cout << velocity.z << std::endl;
            }
        }

        if ((first_run && current == 0.0) || !first_run)
        {
            if (current == end)
            {
                running = false;
            }
            else if (current + inc > end)
            {
                current = end;
            }
            else
            {
                current += inc;
            }
        }
        first_run = false;
    }


    return resultpos;
}