/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#include <gtest/gtest.h>

#include "gazebo/math/Vector4.hh"

using namespace gazebo;

class Vector4Test : public ::testing::Test { };

TEST_F(Vector4Test, Vector4)
{
  {
    math::Vector4 v;
    EXPECT_TRUE(math::equal(v.x, 0.0));
    EXPECT_TRUE(math::equal(v.y, 0.0));
    EXPECT_TRUE(math::equal(v.z, 0.0));
    EXPECT_TRUE(math::equal(v.w, 0.0));
  }

  math::Vector4 v1(1, 2, 3, 4);
  EXPECT_TRUE(math::equal(v1.x, 1.0));
  EXPECT_TRUE(math::equal(v1.y, 2.0));
  EXPECT_TRUE(math::equal(v1.z, 3.0));
  EXPECT_TRUE(math::equal(v1.w, 4.0));

  math::Vector4 v(v1);
  EXPECT_TRUE(v == v1);

  EXPECT_TRUE(math::equal(v.Distance(math::Vector4(0, 0, 0, 0)), 5.4772, 1e-3));

  // ::GetLength()
  v.Set(1, 2, 3, 4);
  EXPECT_TRUE(math::equal(v.GetLength(), 5.4772, 1e-3));

  // ::GetSquaredLength()
  EXPECT_TRUE(math::equal(v.GetSquaredLength(), 30.0));

  // ::Normalize
  v.Normalize();
  EXPECT_TRUE(v == math::Vector4(0.182574, 0.365148, 0.547723, 0.730297));

  // ::Set
  v.Set(2, 4, 6, 8);
  EXPECT_TRUE(v == math::Vector4(2, 4, 6, 8));

  // ::operator= vector4
  v = v1;
  EXPECT_TRUE(v == v1);

  // ::operator= double
  v = 1.2;
  EXPECT_TRUE(v == math::Vector4(1.2, 1.2, 1.2, 1.2));

  // ::operator+ vector4
  v = v + math::Vector4(1, 2, 3, 4);
  EXPECT_TRUE(v == math::Vector4(2.2, 3.2, 4.2, 5.2));

  // ::operator+=
  v += v;
  EXPECT_TRUE(v == math::Vector4(4.4, 6.4, 8.4, 10.4));

  // ::operator- vector4
  v = v - math::Vector4(1, 1, 1, 1);
  EXPECT_TRUE(v == math::Vector4(3.4, 5.4, 7.4, 9.4));

  // ::operator-= vector4
  v -= math::Vector4(1, 1, 1, 1);
  EXPECT_TRUE(v == math::Vector4(2.4, 4.4, 6.4, 8.4));


  // ::operator/ vector4
  v = v / math::Vector4(.1, .1, .1, .1);
  EXPECT_TRUE(v == math::Vector4(24, 44, 64, 84));

  // ::operator/ double
  v = v / 2.0;
  EXPECT_TRUE(v == math::Vector4(12, 22, 32, 42));

  // ::operator/= vector4
  v /= math::Vector4(4, 4, 4, 4);
  EXPECT_TRUE(v == math::Vector4(3, 5.5, 8, 10.5));

  // ::operator/= double
  v /= .1;
  EXPECT_TRUE(v == math::Vector4(30, 55, 80, 105));

  // ::operator * matrix4
  v = v * math::Matrix4(1, 2, 3, 4,
                        5, 6, 7, 8,
                        9, 10, 11, 12,
                        13, 14, 15, 16);
  EXPECT_TRUE(v == math::Vector4(2390, 2660, 2930, 3200));


  // ::operator * vector4
  v = v * math::Vector4(.2, .3, .4, .5);
  EXPECT_TRUE(v == math::Vector4(478, 798, 1172, 1600));

  // ::operator *= vector4
  v *= math::Vector4(2, 4, 6, 8);
  EXPECT_TRUE(v == math::Vector4(956, 3192, 7032, 12800));

  // ::operator * double
  v = v * 5.2;
  EXPECT_TRUE(v == math::Vector4(4971.2, 16598.4, 36566.4, 66560));

  // ::operator *= double
  v *= 10.0;
  EXPECT_TRUE(v == math::Vector4(49712, 1.65984e+05, 3.65664e+05, 6.656e+05));

  // ::operator != vector4
  EXPECT_TRUE(v != math::Vector4());

  // ::IsFinite
  EXPECT_TRUE(v.IsFinite());

  // ::operator[]
  v.Set(1, 2, 3, 4);
  EXPECT_DOUBLE_EQ(v[0], 1);
  EXPECT_DOUBLE_EQ(v[1], 2);
  EXPECT_DOUBLE_EQ(v[2], 3);
  EXPECT_DOUBLE_EQ(v[3], 4);
}
