//DoublePendulum.h

#ifndef DoublePendulum_H
#define DoublePendulum_H
#endif

#include "Pendulum.h"

class DoublePendulum
{
public:
    DoublePendulum (Pendulum &, Pendulum &, const double &);
    DoublePendulum (DoublePendulum &);
    Pendulum getUp ();
    Pendulum getDown ();
    double getG ();
    DoublePendulum operator+ (DoublePendulum &);
    DoublePendulum operator= (DoublePendulum &);
    DoublePendulum operator* (const double &);
    DoublePendulum Derivate();

private:
    Pendulum Up;
    Pendulum Down;
    const double g;
};

//Implementation

#include "DoublePendulum.h"
#include <cmath>

DoublePendulum::DoublePendulum(Pendulum &Up, Pendulum &Down, const double &g)
    :Up(Up),Down(Down),g(g)
{}

DoublePendulum::DoublePendulum(DoublePendulum &DoublePendulumA)
    :Up(DoublePendulumA.getUp()),Down(DoublePendulumA.getDown()),g(DoublePendulumA.getG())
{}

Pendulum DoublePendulum::getUp ()
{
    return Up;
}

Pendulum DoublePendulum::getDown ()
{
    return Down;
}

double DoublePendulum::getG()
{
    return g;
}

DoublePendulum DoublePendulum::operator+ (DoublePendulum &DoublePendulumA)
{
    return DoublePendulum(Up+DoublePendulumA.getUp(),Down+DoublePendulumA.getDown(),DoublePendulumA.getG());
}

DoublePendulum DoublePendulum::operator= (DoublePendulum &DoublePendulumA)
{
    return DoublePendulum (DoublePendulumA);
}

DoublePendulum DoublePendulum::operator* (const double &a)
{
    return DoublePendulum(Up*a,Down*a,g);
}
DoublePendulum DoublePendulum::Derivate()
{
    Pendulum dUp(Up.getM(),Up.getL(),Up.getOmega(),0);
    Pendulum dDown(Down.getM(),Down.getL(),Down.getOmega(),0);

    double m1=Up.getM(),m2=Down.getM(),M=m1+m2,l1=Up.getL(),l2=Down.getL(),theta1=Up.getTheta(),theta2=Down.getTheta(),dTheta=theta2-theta1,omega1=Up.getOmega(),omega2=Down.getOmega();

    dUp.setOmega((m2*l1*omega1*omega1*sin(dTheta)*cos(dTheta)+m2*g*sin(theta2)*cos(dTheta)+m2*l2*omega2*omega2*sin(dTheta)-M*g*sin(theta1))/(M*l1-m2*l2*cos(dTheta)*cos(dTheta)));
    dDown.setOmega((-m2*l2*omega2*omega2*sin(dTheta)*cos(dTheta)+M*(g*sin(theta1)*cos(dTheta)-l1*omega1*omega1*sin(dTheta)-g*sin(theta2)))/(M*l2-m2*l2*cos(dTheta)*cos(dTheta)));

    return DoublePendulum(dUp,dDown,g);
}

//Pendulum.h

#ifndef Pendulum_H
#define Pendulum_H
#endif

#include <string>
#include <iostream>
using namespace std;

class Pendulum
{
    public:
        Pendulum (const double &, const double &, double, double);
        Pendulum (Pendulum &);
        double getL () const;
        double getM () const;
        double getTheta () const;
        double getOmega () const;
        void setTheta (const double &);
        void setOmega (const double &);
        Pendulum operator+ (Pendulum &) const;
        Pendulum operator= (Pendulum &) const;
        Pendulum operator* (const double) const;
    private:
        const double l,m;
        double theta;
        double omega;
        double check (const double,const string);
};

//Implementation:

#include "Pendulum.h"
using namespace std;

Pendulum::Pendulum (const double &L, const double &M, double Theta, double Omega)
    :l(check(L,"L")), m(check(M,"M")),theta(Theta),omega(Omega)
{}

Pendulum::Pendulum(Pendulum &PendulumA)
    :m(check(PendulumA.getM(),"M")), l(check(PendulumA.getL(),"L")),theta(PendulumA.getTheta()),omega(PendulumA.getOmega())
{}

double Pendulum::check(const double x,const string str)
{
    if (x < 0)
    {
        cout << "Error: " << str << " must be positive. Exiting program." << endl;
        exit;
    }
    return x;
}

double Pendulum::getL() const
{
    return l;
}

double Pendulum::getM() const
{
    return m;
}

double Pendulum::getTheta() const
{
    return theta;
}

double Pendulum::getOmega() const
{
    return omega;
}

void Pendulum::setTheta (const double &newTheta)
{
    theta=newTheta;
}

void Pendulum::setOmega(const double &newOmega)
{
    omega=newOmega;
}

Pendulum Pendulum::operator+ (Pendulum &PendulumA) const
{
    return Pendulum (m,l,theta+PendulumA.getTheta(),omega+PendulumA.getOmega());
}

Pendulum Pendulum::operator= (Pendulum &PendulumA) const
{
    return Pendulum (PendulumA);
}

Pendulum Pendulum::operator* (const double a) const
{
    return Pendulum (m,l,theta*a,omega*a);
}