//===========================================================================
bool cEffectStickSlip::computeForce(const cVector3d& a_toolPos,
                                  const cVector3d& a_toolVel,
                                  const unsigned int& a_toolID,
                                  cVector3d& a_reactionForce)
{
    // check if history for this IDN exists
    if (a_toolID < CHAI_EFFECT_MAX_IDN)
    {
        if (m_parent->m_interactionInside)
        {
            // check if a recent valid point has been stored previously
            if (!m_history[a_toolID].m_valid)
            {
                m_history[a_toolID].m_currentStickPosition = a_toolPos;
                m_history[a_toolID].m_valid = true;
            }

            // read parameters for stick and slip model
            double stiffness = m_parent->m_material.getStickSlipStiffness();
            double forceMax = m_parent->m_material.getStickSlipForceMax();

            // compute current force between last stick position and current tool position
            double distance = cDistance(a_toolPos, m_history[a_toolID].m_currentStickPosition);
            double forceMag = distance * stiffness;

            if (forceMag > 0)
            {
                // if force above threshold, slip...
                if (forceMag > forceMax)
                {
                    m_history[a_toolID].m_currentStickPosition = a_toolPos;
                    a_reactionForce.zero();
                }
                // ...otherwise stick
                else
                {
                    a_reactionForce = (forceMag / distance) * cSub(m_history[a_toolID].m_currentStickPosition, a_toolPos);
                }
            }
            else
            {
                a_reactionForce.zero();
            }

            return (true);
        }
        else
        {
            // the tool is located outside the object, so zero reaction force
            m_history[a_toolID].m_valid = false;
            a_reactionForce.zero();
            return (false);
        }
    }
    else
    {
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
bool cProxyPointForceAlgo::goalAchieved(const cVector3d& a_proxy, const cVector3d& a_goal) const
{
    if (m_useDynamicProxy)
    {
        return (!(a_proxy.distance(a_goal) > 0.0));
    }
    else
    {
        return (a_proxy.distance(a_goal) < (m_epsilonBaseValue));
    }
}

//===========================================================================
void cLight::setDir(const cVector3d& a_direction)
{

    // We arbitrarily point lights along the x axis of the stored
    // rotation matrix... this allows matrix transformations
    // to apply to lights.
    // m_localRot.setCol0(a_direction);

    cVector3d c0, c1, c2, t;
    a_direction.copyto(c0);

    // check vector
    if (c0.lengthsq() < 0.0001) { return; }

    // normalize direction vector
    c0.normalize();

    // compute 2 vector perpendicular to a_direction
    t.set(a_direction.y, a_direction.z, a_direction.x);
    t.crossr(c0, c1);
    c1.crossr(c0, c2);    

    // c0.negate();
    c1.negate();
    c2.negate();

    // update rotation matrix
    m_localRot.setCol(c0,c1,c2);
}

//===========================================================================
bool cEffectVibration::computeForce(const cVector3d& a_toolPos,
                                  const cVector3d& a_toolVel,
                                  const unsigned int& a_toolID,
                                  cVector3d& a_reactionForce)
{
    if (m_parent->m_interactionInside)
    {
        // read vibration parameters
        double vibrationFrequency = m_parent->m_material.getVibrationFrequency();
        double vibrationAmplitude = m_parent->m_material.getVibrationAmplitude();

        // read time
        double time = clock.getCurrentTimeSeconds();

        // compute force magnitude
        double forceMag = vibrationAmplitude * sin(2.0 * CHAI_PI *vibrationFrequency * time);

        a_reactionForce = cMul(forceMag, cVector3d(1, 0, 0));
        return (true);
    }
    else
    {
        // the tool is located outside the object, so zero reaction force
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
void cShapeSphere::computeLocalInteraction(const cVector3d& a_toolPos,
                                          const cVector3d& a_toolVel,
                                          const unsigned int a_IDN)
{
    // compute distance from center of sphere to tool
    double distance = a_toolPos.length();

    // from the position of the tool, search for the nearest point located
    // on the surface of the sphere
    if (distance > 0)
    {
        m_interactionProjectedPoint = cMul( (m_radius/distance), a_toolPos);
    }
    else
    {
        m_interactionProjectedPoint = a_toolPos;
    }

    // check if tool is located inside or outside of the sphere
    if (distance <= m_radius)
    {
        m_interactionInside = true;
    }
    else
    {
        m_interactionInside = false;
    }
}

//===========================================================================
int cVirtualDevice::getPosition(cVector3d& a_position)
{
    if (!m_systemReady)
    {
        a_position.set(0, 0, 0);
        return (-1);
    }

    double x,y,z;
    x = (double)(*m_pDevice).PosX;
    y = (double)(*m_pDevice).PosY;
    z = (double)(*m_pDevice).PosZ;
    a_position.set(x, y, z);

    return (0);
}

//==============================================================================
bool cEffectMagnet::computeForce(const cVector3d& a_toolPos,
                                 const cVector3d& a_toolVel,
                                 const unsigned int& a_toolID,
                                 cVector3d& a_reactionForce)
{
    // compute distance from object to tool
    double distance = cDistance(a_toolPos, m_parent->m_interactionPoint);

    // get parameters of magnet
    double magnetMaxForce = m_parent->m_material->getMagnetMaxForce();
    double magnetMaxDistance = m_parent->m_material->getMagnetMaxDistance();
    double stiffness = m_parent->m_material->getStiffness();
    double forceMagnitude = 0;

    if (m_enabledInside || (!m_parent->m_interactionInside))
    {
        if ((distance < magnetMaxDistance) && (stiffness > 0))
        {
            double d = magnetMaxForce / stiffness;
            if (distance < d)
            {
                forceMagnitude = stiffness * distance;
            }
            else
            {
                double dx = (magnetMaxDistance - d);
                if (dx > 0)
                {
                    double k = magnetMaxForce / dx;
                    forceMagnitude = k * (magnetMaxDistance - distance);
                }
            }

            // compute reaction force
            int sign = -1;
            if (m_parent->m_interactionInside)
            {
                sign = 1;
            }

            a_reactionForce = cMul(sign * forceMagnitude, m_parent->m_interactionNormal);

            return (true);
        }
        else
        {
            return (false);
        }
    }
    else
    {
        // the tool is located outside the magnet zone
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
int cPhantomDevice::getPosition(cVector3d& a_position)
{
    // check if drivers are installed
    if (!m_driverInstalled) return (-1);

    double x,y,z;
    int error = hdPhantomGetPosition(m_deviceID, &x, &y, &z);
    a_position.set(x, y, z);
    estimateLinearVelocity(a_position);
    return (error);
}

void VirtualHapticDevice::GetCursorVelocity(cVector3d& velocity) {
	double currentTime = clock.getCurrentTimeSeconds();
	double ellapsedTime = currentTime - lastClock;
	lastClock = currentTime;

	cVector3d currentPos;
	chaiDevice->getPosition(currentPos);
	velocity = currentPos - lastPosition;
	velocity.mul(1.0 / ellapsedTime );
	lastPosition.copyfrom(currentPos);
}

//==============================================================================
void cShapeTorus::computeLocalInteraction(const cVector3d& a_toolPos,
        const cVector3d& a_toolVel,
        const unsigned int a_IDN)
{
    cVector3d toolProjection = a_toolPos;
    toolProjection.z(0.0);
    m_interactionNormal.set(0,0,1);

    // search for the nearest point on the torus medial axis
    if (a_toolPos.lengthsq() > C_SMALL)
    {
        cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(toolProjection));

        // compute eventual penetration of tool inside the torus
        cVector3d vectTorusTool = cSub(a_toolPos, pointAxisTorus);

        double distance = vectTorusTool.length();

        // normal
        if (distance > 0.0)
        {
            m_interactionNormal = vectTorusTool;
            m_interactionNormal.normalize();
        }

        // tool is located inside the torus
        if ((distance < m_innerRadius) && (distance > 0.001))
        {
            m_interactionInside = true;
        }

        // tool is located outside the torus
        else
        {
            m_interactionInside = false;
        }

        // compute surface point
        double dist = vectTorusTool.length();
        if (dist > 0)
        {
            vectTorusTool.mul(1/dist);
        }
        vectTorusTool.mul(m_innerRadius);
        pointAxisTorus.addr(vectTorusTool, m_interactionPoint);
    }
    else
    {
        m_interactionInside = false;
        m_interactionPoint = a_toolPos;
    }
}

//===========================================================================
int cVirtualDevice::getForce(cVector3d& a_force)
{
    if (!m_systemReady)
    {
        a_force.set(0,0,0);
        return (-1);
    }

    a_force.x = ((*m_pDevice).ForceX);
    a_force.y = ((*m_pDevice).ForceY);
    a_force.z = ((*m_pDevice).ForceZ);

    return (0);
}

//===========================================================================
int cHydraDevice::getPosition(cVector3d& a_position)
{
    //std::cout << "get pos";
    /************************************************************************
        STEP 7:
        Here you may implement code which reads the position (X,Y,Z) from
        your haptic device. Read the values from your device and modify
        the local variable (x,y,z) accordingly.
        If the operation fails return an error code such as -1 for instance.

        Note:
        For consistency, units must be in meters.
        If your device is located in front of you, the x-axis is pointing
        towards you (the operator). The y-axis points towards your right
        hand side and the z-axis points up towards the sky. 

    *************************************************************************/

    int error = 0;
    double x,y,z;

    // *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***

    sixenseAllControllerData acd;
    sixenseGetAllNewestData( &acd );


    y = acd.controllers[a_deviceNumber].pos[0] / 1000.0f;
    z = acd.controllers[a_deviceNumber].pos[1] / 1000.0f - 0.3;
    x = acd.controllers[a_deviceNumber].pos[2] / 1000.0f - 0.3;


    // offset
    cMatrix3d r;
    getRotation(r);
    cVector3d v(-0.08,0,0);
    v = r * v;




    // store new position values
    a_position.set(x+v.x(), y+v.y(), z+v.z());

    // estimate linear velocity
    estimateLinearVelocity(a_position);

    // exit
    return (error);
}

//===========================================================================
bool cCollisionBrute::computeCollision(cVector3d& a_segmentPointA,
                                       cVector3d& a_segmentPointB,
                                       cGenericObject*& a_colObject,
                                       cTriangle*& a_colTriangle,
                                       cVector3d& a_colPoint,
                                       double& a_colSquareDistance,
                                       int a_proxyCall)
{
    // temp variables for storing results
    cGenericObject* colObject;
    cTriangle* colTriangle;
    cVector3d colPoint;
    bool hit = false;

    // convert two point segment into a segment described by a point and
    // a directional vector
    cVector3d dir;
    a_segmentPointB.subr(a_segmentPointA, dir);

    // compute the squared length of the segment
    double colSquareDistance = dir.lengthsq();

    // check all triangles for collision and return the nearest one
    unsigned int ntriangles = m_triangles->size();
    for (unsigned int i=0; i<ntriangles; i++)
    {

        // check for a collision between this triangle and the segment by
        // calling the triangle's collision detection method; it will only
        // return true if the distance between the segment origin and this
        // triangle is less than the current closest intersecting triangle
        // (whose distance squared is kept in colSquareDistance)
        if ((*m_triangles)[i].computeCollision(
            a_segmentPointA, dir, colObject, colTriangle, colPoint, colSquareDistance))
        {
            a_colObject = colObject;
            a_colTriangle = colTriangle;
            a_colPoint = colPoint;
            a_colSquareDistance = colSquareDistance;
            hit = true;
        }
    }

    // return result
    return (hit);
}

//===========================================================================
bool cEffectViscosity::computeForce(const cVector3d& a_toolPos,
                                    const cVector3d& a_toolVel,
                                    const unsigned int& a_toolID,
                                    cVector3d& a_reactionForce)
{
    if (m_parent->m_interactionInside)
    {
        // the tool is located inside the object.
        double viscosity = m_parent->m_material.getViscosity();
        a_reactionForce = cMul(-viscosity, a_toolVel);
        return (true);
    }
    else
    {
        // the tool is located outside the object, so zero reaction force.
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
void cDirectionalLight::setDir(const cVector3d& a_direction)
{

    // We arbitrarily point lights along the x axis of the stored
    // rotation matrix... this allows matrix transformations
    // to apply to lights.
    // m_localRot.setCol0(a_direction);

    cVector3d c0, c1, c2, t0, t1;
    a_direction.copyto(c0);

    // check vector
    if (c0.lengthsq() < 0.0001) {
        return;
    }

    // normalize direction vector
    c0.normalize();

    // compute 2 vector perpendicular to a_direction
    t0.set(0.0, 0.0, 1.0);
    t1.set(0.0, 1.0, 0.0);
    double a0 = cAngle(c0, t0);
    double a1 = cAngle(c0, t1);

    if (sin(a0) > sin(a1))
    {
        c0.crossr(t0, c1);
        c0.crossr(c1, c2);
    }
    else
    {
        c0.crossr(t1, c1);
        c0.crossr(c1, c2);
    }

    c1.normalize();
    c2.normalize();

    // update rotation matrix
    m_localRot.setCol(c0,c1,c2);
}

//===========================================================================
bool cEffectSurface::computeForce(const cVector3d& a_toolPos,
                                  const cVector3d& a_toolVel,
                                  const unsigned int& a_toolID,
                                  cVector3d& a_reactionForce)
{
    if (m_parent->m_interactionInside)
    {
        // the tool is located inside the object,
        // we compute a reaction force using Hooke's law
        double stiffness = m_parent->m_material.getStiffness();
        a_reactionForce = cMul(stiffness, cSub(m_parent->m_interactionProjectedPoint, a_toolPos));
        return (true);
    }
    else
    {
        // the tool is located outside the object, so zero reaction force
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
cCollisionSpheresLine::cCollisionSpheresLine(cVector3d& a_segmentPointA,
                                             cVector3d& a_segmentPointB)
{
    // calculate the center of the line segment
    m_center = cAdd(a_segmentPointA, a_segmentPointB);
    m_center.x *= 0.5;
    m_center.y *= 0.5;
    m_center.z *= 0.5;

    // calculate the radius of the bounding sphere as the distance from the
    // center of the segment (calculated above) to an endpoint
    cVector3d rad = cSub(m_center, a_segmentPointA);
    m_radius = sqrt(rad.x*rad.x + rad.y*rad.y + rad.z*rad.z);

    // set origin and direction of the line segment; i.e., redefine the segment
    // as a ray from the first endpoint (presumably the proxy position when
    // the collision detection is being used with the proxy force algorithm) to
    // the second endpoint (presumably the goal position)
    m_segmentPointA = a_segmentPointA;
    a_segmentPointB.subr(a_segmentPointA, m_dir);
}

//===========================================================================
cVector3d cShapeTorus::computeLocalForce(const cVector3d& a_localPosition)
{
	
    // In the following we compute the reaction forces between the tool and the
    // sphere.
    cVector3d localForce;

    // project pointer on torus plane (z=0)
    cVector3d fingerProjection = a_localPosition;
    fingerProjection.z = 0;
    
    // search for the nearest point on the torus medial axis
    if (a_localPosition.lengthsq() > CHAI_SMALL)
    {
        cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(fingerProjection));

        // compute eventual penetration of finger inside the torus
        cVector3d vectTorusFinger = cSub(a_localPosition, pointAxisTorus);

        double distance = vectTorusFinger.length();

        // finger inside torus, compute forces
        if ((distance < m_innerRadius) && (distance > 0.001))
        {
            localForce = cMul((m_innerRadius - distance) * (m_material.getStiffness()), cNormalize(vectTorusFinger));
        }

        // finger is outside torus
        else
        {
            localForce.zero();
        }
    }
    else
    {
        localForce.zero();
    }

    return (localForce);
}

//==============================================================================
bool cMyCustomDevice::getPosition(cVector3d& a_position)
{
    ////////////////////////////////////////////////////////////////////////////
    /*
        STEP 7:

        Here you shall implement code that reads the position (X,Y,Z) from
        your haptic device. Read the values from your device and modify
        the local variable (x,y,z) accordingly.
        If the operation fails return an C_ERROR, C_SUCCESS otherwise

        Note:
        For consistency, units must be in meters.
        If your device is located in front of you, the x-axis is pointing
        towards you (the operator). The y-axis points towards your right
        hand side and the z-axis points up towards the sky. 
    */
    ////////////////////////////////////////////////////////////////////////////

    bool result = C_SUCCESS;
    double x,y,z;

    // *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***

    x = 0.0;    // x = getMyDevicePositionX()
    y = 0.0;    // y = getMyDevicePositionY()
    z = 0.0;    // z = getMyDevicePositionZ()

    // store new position values
    a_position.set(x, y, z);

    // estimate linear velocity
    estimateLinearVelocity(a_position);

    // exit
    return (result);
}

//===========================================================================
bool cEffectPotentialField::computeForce(const cVector3d& a_toolPos,
                                         const cVector3d& a_toolVel,
                                         const unsigned int& a_toolID,
                                         cVector3d& a_reactionForce)
{
    if (m_parent->m_interactionInside)
    {
        // the tool is located inside the object, so zero reaction force
        a_reactionForce.zero();
        return (false);
    }
    else
    {
        // the tool is located outside the object,
        // we compute a reaction force using Hooke's law
        double stiffness = m_parent->m_material->getStiffness();
        double maxForce = m_parent->m_material->getMagnetMaxForce();
        cVector3d force = cMul(stiffness, cSub(m_parent->m_interactionProjectedPoint, a_toolPos));

        if (maxForce > 0.0)
        {
            double ratio = force.length() / maxForce;
            if (ratio > 1.0)
            {
                force = (1.0 / ratio) * force;
            }
        }
        else
        {
            force.zero();
        }
        
        a_reactionForce = force;
        return (true);
    }
}