CurveGraphBezier3.h

#pragma once

#include "CurveGraph.h"
#include "Interpolators.h"

template<class T>
class CCurveGraphBezier3 : public CCurveGraph<T> {
public:
        CCurveGraphBezier3()
        {
        }
        virtual T Interpolate(float time)
        {
                
                if(m_nodes.size() < 4 ) return m_nodes.back().value;

                // hledani nod mezi kterymi lezi zadany time
                int curr = -1, last = -1;
                for(size_t i = 0; i < m_nodes.size(); i+=3) {
                        if(m_nodes[i].t >= time) {
                                curr = i;
                                break;
                        }
                        last = i;
                }
                m_currIndex = curr;

                // interpolace
                if(curr != -1 && last != -1) {
                        float local_time = (m_nodes[curr].t - m_nodes[last].t);
                        local_time =  (time -  m_nodes[last].t) / local_time;
                        local_time = Bezier3InterpolationInverse(time, local_time, 0.01f, m_nodes[last].t, m_nodes[last + 1].t, m_nodes[curr - 1].t, m_nodes[curr].t);

                        return Bezier3Interpolation(local_time, m_nodes[last].value, m_nodes[last + 1].value, m_nodes[curr - 1].value, m_nodes[curr].value);
                
                
                } else if(curr == -1) {
                        return  m_nodes.back().value;
                } else if(last == -1) {
                        return m_nodes.front().value;
                }

                
                return T(0.0f);
        }

        virtual size_t AddNode(const float t, const T& value)
        {
                GRAPH_NODE node;
                GRAPH_NODE control_node;
                node.t = t;
                node.value = value;
                control_node.value = value;

                if(m_nodes.empty()) { // prvni
                        control_node.t = 10;    // random cislo, nezalezi na nem
                        m_nodes.push_back(node);
                        m_nodes.push_back(control_node);
                        return 0;
                }
                // je cas mensi nez u prvni node?
                if(m_nodes.front() > t) {
                        node.t = m_nodes.front().t + 0.0001f;
                }
                if(m_nodes.size() == 2) { // posledni
                        
                        // vzdalenost 1. bodu a posledniho
                        float l = (node.t - m_nodes.front().t) / 2;
                        
                        // uprava control pointu prvni node
                        m_nodes[1].t = m_nodes.front().t + l;
                        
                        // control point posledni node
                        control_node.t = node.t - l;
                        m_nodes.push_back(control_node);
                        m_nodes.push_back(node);
                }

                // je cas vets nez u posledni node?
                if(m_nodes.back() < t) {
                        node.t = m_nodes.back().t - 0.0001f;
                }

                // najdi misto kam patri (vzdy se vklada nalevo od nalezene nody)
                for(size_t i = 3; i < m_nodes.size(); i+=3) {
                        if( node < m_nodes[i]) {
                                /*
                                        Zpusob vkladani:

                                        1. vychozi stav
                                        (N0) --- (C0r) --- (C1l) --- (N1)
                                        2. rozhodneme se vlozit nalevo od N1
                                        3. pokud bychom pouzili hodnotu i = 3, vlozili bychom novy node za control point stareho, takze musime vlozit na i - 1
                                           coz je za C1l
                                        4. Korekce C1l tak, aby melo t = t Nnove 

                                */
                                float l = node.t - (node.t - m_nodes[i - 3].t) / 2.0f;
                                float r = node.t + (m_nodes[i].t - node.t) / 2.0f; 

                                // korekce C(i - 1)l
                                if(m_nodes[i - 1].t < node.t) {
                                        m_nodes[i - 1].t = node.t + 0.0001f;
                                }
                                // korekce C(i - 2)r
                                if(m_nodes[i - 2].t > node.t) {
                                        m_nodes[i - 2].t = node.t - 0.0001f;
                                }

                                m_nodes.insert(m_nodes.begin() + (i - 1), node);
                                // L
                                control_node.t = l;
                                m_nodes.insert(m_nodes.begin() + (i - 1), control_node);
                                // R
                                control_node.t = r;
                                m_nodes.insert(m_nodes.begin() + (i + 1), control_node);


                                return i + 1;            
                        }
                }
                

                return 0;
        }
};