/*******************************************************************************
 * Copyright (C) 2018 MINRES Technologies GmbH
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *******************************************************************************/

#include "sysc/top/BLDC.h"

// implementation according to Modeling of BLDC Motor with Ideal Back-EMF for Automotive Applications
// Proceedings of the World Congress on Engineering 2011 Vol II WCE 2011, July 6 - 8, 2011, London, U.K.
BLDC::BLDC(const Config config)
: config(config)
, stateVector({{0.0, 0.0, 0.0, 0.0, 0.0}})
, state(stateVector)
, vin({{0.0, 0.0, 0.0}})
, voltages({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}) {
    state.init();
}

BLDC::~BLDC() = default;

double BLDC::calc_bemf_factor(const State &x, double theta) {
    if (theta >= 0 && theta < 2. / 3. * M_PI) {
        return 1;
    } else if (theta >= 2. / 3. * M_PI && theta < M_PI) {
        return 1 - 6 / M_PI * (theta - 2. / 3. * M_PI);
    } else if (theta >= M_PI && theta < 5. / 3. * M_PI) {
        return -1;
    } else if (theta >= 5. / 3. * M_PI && theta <= 2. * M_PI) {
        return -1 + 6 / M_PI * (theta - 5. / 3. * M_PI);
    } else {
        fprintf(stderr, "ERROR: angle out of bounds can not calculate bemf %f\n", theta);
        throw std::runtime_error("angle out of bounds can not calculate bemf");
    }
}

void BLDC::calc_back_emf(const State &state, double theta_e) {
    double max_bemf = config.Ke * state.omega;
    theta_e-=M_PI * (1. / 3.);
    voltages[EA] = max_bemf * calc_bemf_factor(state, norm_angle(theta_e));
    voltages[EB] = max_bemf * calc_bemf_factor(state, norm_angle(theta_e + M_PI * (2. / 3.)));
    voltages[EC] = max_bemf * calc_bemf_factor(state, norm_angle(theta_e + M_PI * (4. / 3.)));
}

void BLDC::calc_voltages() {
    const double NaN = nan("");
    /* Check which phases are excited. */
    bool pa = isnan(vin[0]) ? false : true;
    bool pb = isnan(vin[1]) ? false : true;
    bool pc = isnan(vin[2]) ? false : true;

    if (pa && pb && pc) {
        voltages[VA] = vin[0];
        voltages[VB] = vin[1];
        voltages[VC] = vin[2];
        voltages[VCENTER] =
            (voltages[VA] + voltages[VB] + voltages[VC] - voltages[EA] - voltages[EB] - voltages[EC]) / 3.;
    } else if (pa && pb) {
        voltages[VA] = vin[0];
        voltages[VB] = vin[1];
        voltages[VCENTER] = (voltages[VA] + voltages[VB] - voltages[EA] - voltages[EB]) / 2.;
        voltages[VC] = voltages[EC] + voltages[VCENTER];
    } else if (pa && pc) {
        voltages[VA] = vin[0];
        voltages[VC] = vin[2];
        voltages[VCENTER] = (voltages[VA] + voltages[VC] - voltages[EA] - voltages[EC]) / 2.;
        voltages[VB] = voltages[EB] + voltages[VCENTER];
    } else if (pb && pc) {
        voltages[VB] = vin[1];
        voltages[VC] = vin[2];
        voltages[VCENTER] = (voltages[VB] + voltages[VC] - voltages[EB] - voltages[EC]) / 2.;
        voltages[VA] = voltages[EA] + voltages[VCENTER];
    } else if (pa) {
        voltages[VA] = vin[0];
        voltages[VCENTER] = (voltages[VA] - voltages[EA]);
        voltages[VB] = voltages[EB] + voltages[VCENTER];
        voltages[VC] = voltages[EC] + voltages[VCENTER];
    } else if (pb) {
        voltages[VB] = vin[1];
        voltages[VCENTER] = (voltages[VB] - voltages[EB]);
        voltages[VA] = voltages[EA] + voltages[VCENTER];
        voltages[VC] = voltages[EC] + voltages[VCENTER];
    } else if (pc) {
        voltages[VC] = vin[0];
        voltages[VCENTER] = (voltages[VC] - voltages[EC]);
        voltages[VA] = voltages[EA] + voltages[VCENTER];
        voltages[VB] = voltages[EB] + voltages[VCENTER];
    } else {
        voltages[VA] = voltages[EA];
        voltages[VB] = voltages[EB];
        voltages[VC] = voltages[EC];
        voltages[VCENTER] = 0;
        // return;
    }
    auto vmax = std::max({pa ? vin[0] : 0, pb ? vin[1] : 0, pc ? vin[2] : 0});
    voltages[VCENTER] = vmax / 2;
}

void BLDC::printToStream(std::ostream &os) const {
    os << state.omega << ";" << state.theta << ";" << state.ia << ";" << state.ib << ";" << state.ic << ";"
       << voltages[VA] << ";" << voltages[VB] << ";" << voltages[VC] << ";" << voltages[EA] << ";" << voltages[EB]
       << ";" << voltages[EC] << ";" << voltages[VCENTER] << ";" << vin[0] << ";" << vin[1] << ";" << vin[2] << ";"
       << etorque;
}

void BLDC::rotor_dyn(const StateVector &x_, StateVector &dxdt_, const double t) {
    const State x(const_cast<StateVector &>(x_));
    State dxdt(dxdt_);
    double theta_e = state.theta * (config.NbPoles / 2.);
    /* Calculate backemf voltages. */
    calc_back_emf(x, theta_e);
    /* Calculate voltages. */
    calc_voltages();
    /* Electromagnetic torque. */
    //    if (x.omega == 0) {
    //        printf("ERROR: input state vector omega equals 0!!!\n");
    //        throw std::runtime_error("input state vector omega equals 0");
    //    }
    /* electrical torque */
    // etorque = ((voltages[EA] * x.ia) + (voltages[EB] * x.ib) + (voltages[EC] * x.ic)) / x.omega;
    // which is equivalent to:
    etorque = config.Ke * (x.ia * (calc_bemf_factor(state, norm_angle(theta_e))) +
                           x.ib * (calc_bemf_factor(state, norm_angle(theta_e + M_PI * (2. / 3.)))) +
                           x.ic * (calc_bemf_factor(state, norm_angle(theta_e + M_PI * (4. / 3.)))));
    /* Mechanical torque. */
    mtorque = ((etorque * (config.NbPoles / 2)) - (config.damping * x.omega) - torque_load);

    if ((mtorque > 0) && (mtorque <= config.static_friction)) {
        mtorque = 0;
    } else if (mtorque > config.static_friction) {
        mtorque -= config.static_friction;
    } else if ((mtorque < 0) && (mtorque >= -(config.static_friction))) {
        mtorque = 0;
    } else if (mtorque < -(config.static_friction)) {
        mtorque += config.static_friction;
    }
    /* Position of the rotor */
    dxdt.theta = x.omega;
    /* Acceleration of the rotor. (omega_dot) */
    // a=M/J with M->torque, J->Inertia, a->angular acceleration
    dxdt.omega = mtorque / config.inertia;
    /* Calculate dot currents. */
    dxdt.ia = (voltages[VA] - (config.R * x.ia) - voltages[EA] - voltages[VCENTER]) / (config.L - config.M);
    dxdt.ib = (voltages[VB] - (config.R * x.ib) - voltages[EB] - voltages[VCENTER]) / (config.L - config.M);
    dxdt.ic = (voltages[VC] - (config.R * x.ic) - voltages[EC] - voltages[VCENTER]) / (config.L - config.M);
}

void BLDC::run(double incr) {
    if (dt > incr) throw std::runtime_error("incr needs to be larger than dt");
    double next_time = current_time + incr;
    odeint::integrate_adaptive(
        make_controlled(1.0e-10, 1.0e-6, stepper_type()),
        [this](const StateVector &x, StateVector &dxdt, double t) { this->rotor_dyn(x, dxdt, t); }, stateVector,
        current_time, next_time, dt);
    current_time = next_time;
    state.theta = norm_angle(state.theta);
}

std::ostream &operator<<(std::ostream &os, const BLDC &bldc) {
    bldc.printToStream(os);
    return os;
}
